feat: add integrate and differentiate methods to Cartesian types

CHANGELOG.md

@@ -20,6 +20,7 @@ Release Versions
 
 - fix(robot-model): improve ik performance (#205)
 - fix(robot-model): kinematics tests (#207)
+- feat: add integrate and differentiate methods to Cartesian types (#209)
 
 ## 9.0.1
 

python/source/state_representation/bind_cartesian_space.cpp

@@ -268,6 +268,12 @@ void cartesian_pose(py::module_& m) {
 
   c.def("copy", &CartesianPose::copy, "Return a copy of the CartesianPose");
   c.def("data", &CartesianPose::data, "Returns the pose data as a vector");
+  c.def("differentiate", [](const CartesianPose &pose, double dt) -> CartesianTwist {
+    return pose.differentiate(dt);
+  }, "Differentiate a Cartesian pose over a time period in seconds", "dt"_a);
+  c.def("differentiate", [](const CartesianPose &pose, const std::chrono::nanoseconds& dt) -> CartesianTwist {
+    return pose.differentiate(dt);
+  }, "Differentiate a Cartesian pose over a time period", "dt"_a);
   c.def("inverse", &CartesianPose::inverse, "Compute the inverse of the current CartesianPose");
   c.def("set_data", py::overload_cast<const Eigen::VectorXd&>(&CartesianPose::set_data), "Set the pose data from a vector", "data"_a);
   c.def("set_data", py::overload_cast<const std::vector<double>&>(&CartesianPose::set_data), "Set the pose data from a list", "data"_a);
@@ -360,6 +366,18 @@ void cartesian_twist(py::module_& m) {
 
   c.def("copy", &CartesianTwist::copy, "Return a copy of the CartesianTwist");
   c.def("data", &CartesianTwist::data, "Returns the twist data as a vector");
+  c.def("differentiate", [](const CartesianTwist &twist, double dt) -> CartesianAcceleration {
+    return twist.differentiate(dt);
+  }, "Differentiate a Cartesian twist over a time period in seconds", "dt"_a);
+  c.def("differentiate", [](const CartesianTwist &twist, const std::chrono::nanoseconds& dt) -> CartesianAcceleration {
+    return twist.differentiate(dt);
+  }, "Differentiate a Cartesian twist over a time period", "dt"_a);
+  c.def("integrate", [](const CartesianTwist &twist, double dt) -> CartesianPose {
+    return twist.integrate(dt);
+  }, "Integrate a Cartesian twist over a time period in seconds", "dt"_a);
+  c.def("integrate", [](const CartesianTwist &twist, const std::chrono::nanoseconds& dt) -> CartesianPose {
+    return twist.integrate(dt);
+  }, "Integrate a Cartesian twist over a time period", "dt"_a);
   c.def("inverse", &CartesianTwist::inverse, "Compute the inverse of the current CartesianTwist");
   c.def("set_data", py::overload_cast<const Eigen::VectorXd&>(&CartesianTwist::set_data), "Set the twist data from a vector", "data"_a);
   c.def("set_data", py::overload_cast<const std::vector<double>&>(&CartesianTwist::set_data), "Set the twist data from a list", "data"_a);
@@ -451,6 +469,12 @@ void cartesian_acceleration(py::module_& m) {
 
   c.def("copy", &CartesianAcceleration::copy, "Return a copy of the CartesianAcceleration");
   c.def("data", &CartesianAcceleration::data, "Returns the acceleration data as a vector");
+  c.def("integrate", [](const CartesianAcceleration &acceleration, double dt) -> CartesianTwist {
+    return acceleration.integrate(dt);
+  }, "Integrate a Cartesian acceleration over a time period in seconds", "dt"_a);
+  c.def("integrate", [](const CartesianAcceleration &acceleration, const std::chrono::nanoseconds& dt) -> CartesianTwist {
+    return acceleration.integrate(dt);
+  }, "Integrate a Cartesian acceleration over a time period", "dt"_a);
   c.def("inverse", &CartesianAcceleration::inverse, "Compute the inverse of the current CartesianPose");
   c.def("set_data", py::overload_cast<const Eigen::VectorXd&>(&CartesianAcceleration::set_data), "Set the acceleration data from a vector", "data"_a);
   c.def("set_data", py::overload_cast<const std::vector<double>&>(&CartesianAcceleration::set_data), "Set the acceleration data from a list", "data"_a);

python/test/state_representation/space/cartesian/test_cartesian_pose.py

@@ -1,7 +1,8 @@
 import unittest
 import copy
+from datetime import timedelta
 
-from state_representation import CartesianPose
+from state_representation import CartesianPose, CartesianTwist
 import numpy as np
 
 class TestCartesianPose(unittest.TestCase):
@@ -38,5 +39,21 @@ def test_inverse(self):
         inv_pose = pose.inverse()
         self.assertIsInstance(inv_pose, CartesianPose)
 
+    def test_differentiation(self):
+        pose = CartesianPose.Random("test")
+        pose.set_orientation([0, 1, 0, 0])
+        dt1 = 0.1
+        dt2 = timedelta(milliseconds=100)
+
+        res1 = pose / dt2
+        self.assertIsInstance(res1, CartesianTwist)
+        self.assert_np_array_equal(pose.get_position(), dt1 * res1.get_linear_velocity())
+        self.assert_np_array_equal(np.array([np.pi, 0, 0]), dt1 * res1.get_angular_velocity())
+
+        res2 = pose.differentiate(dt1)
+        self.assertIsInstance(res2, CartesianTwist)
+        self.assert_np_array_equal(pose.get_position(), dt1 * res2.get_linear_velocity())
+        self.assert_np_array_equal(np.array([np.pi, 0, 0]), dt1 * res2.get_angular_velocity())
+
 if __name__ == '__main__':
     unittest.main()

source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp

@@ -189,6 +189,20 @@ class CartesianAcceleration : public CartesianState {
    */
   CartesianAcceleration copy() const;
 
+  /**
+   * @brief Integrate the Cartesian acceleration over a time period
+   * @param dt The time period used for integration in seconds
+   * @return The resulting Cartesian twist after integration
+   */
+  CartesianTwist integrate(double dt) const;
+
+  /**
+   * @brief Integrate the Cartesian acceleration over a time period
+   * @param dt The time period used for integration
+   * @return The resulting Cartesian twist after integration
+   */
+  CartesianTwist integrate(const std::chrono::nanoseconds& dt) const;
+
   /**
    * @brief Compute the inverse of the current Cartesian acceleration
    */

source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp

@@ -180,6 +180,20 @@ class CartesianPose : public CartesianState {
    */
   CartesianPose copy() const;
 
+  /**
+   * @brief Differentiate a Cartesian pose over a time period
+   * @param dt The time period used for derivation in seconds
+   * @return The resulting Cartesian twist after derivation
+   */
+  CartesianTwist differentiate(double dt) const;
+
+  /**
+   * @brief Differentiate a Cartesian pose over a time period
+   * @param dt The time period used for derivation
+   * @return The resulting Cartesian twist after derivation
+   */
+  CartesianTwist differentiate(const std::chrono::nanoseconds& dt) const;
+
   /**
    * @brief Compute the inverse of the current Cartesian pose
    * @return The inverse corresponding to b_S_f (assuming this is f_S_b)

source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp

@@ -195,6 +195,34 @@ class CartesianTwist : public CartesianState {
    */
   CartesianTwist copy() const;
 
+  /**
+   * @brief Differentiate a Cartesian twist over a time period
+   * @param dt The time period used for derivation in seconds
+   * @return The resulting Cartesian acceleration after derivation
+   */
+  CartesianAcceleration differentiate(double dt) const;
+
+  /**
+   * @brief Differentiate a Cartesian twist over a time period
+   * @param dt The time period used for derivation
+   * @return The resulting Cartesian acceleration after derivation
+   */
+  CartesianAcceleration differentiate(const std::chrono::nanoseconds& dt) const;
+
+  /**
+   * @brief Integrate the Cartesian twist over a time period
+   * @param dt The time period used for integration in seconds
+   * @return The resulting Cartesian pose after integration
+   */
+  CartesianPose integrate(double dt) const;
+
+  /**
+   * @brief Integrate the Cartesian twist over a time period
+   * @param dt The time period used for integration
+   * @return The resulting Cartesian pose after integration
+   */
+  CartesianPose integrate(const std::chrono::nanoseconds& dt) const;
+
   /**
    * @brief Compute the inverse of the current Cartesian twist
    */

source/state_representation/src/space/cartesian/CartesianAcceleration.cpp

@@ -49,7 +49,7 @@ CartesianAcceleration::CartesianAcceleration(const CartesianAcceleration& accele
     CartesianAcceleration(static_cast<const CartesianState&>(acceleration)) {}
 
 CartesianAcceleration::CartesianAcceleration(const CartesianTwist& twist) :
-    CartesianAcceleration(twist / std::chrono::seconds(1)) {}
+    CartesianAcceleration(twist.differentiate(1.0)) {}
 
 CartesianAcceleration CartesianAcceleration::Zero(const std::string& name, const std::string& reference) {
   return CartesianState::Identity(name, reference);
@@ -99,6 +99,17 @@ CartesianAcceleration CartesianAcceleration::copy() const {
   return result;
 }
 
+CartesianTwist CartesianAcceleration::integrate(double dt) const {
+  CartesianTwist twist(this->get_name(), this->get_reference_frame());
+  twist.set_twist(dt * this->get_acceleration());
+  return twist;
+}
+
+CartesianTwist CartesianAcceleration::integrate(const std::chrono::nanoseconds& dt) const {
+  // convert the dt to a double with the second as reference
+  return this->integrate(dt.count() / 1e9);
+}
+
 CartesianAcceleration CartesianAcceleration::inverse() const {
   return this->CartesianState::inverse();
 }
@@ -131,19 +142,11 @@ CartesianAcceleration operator*(const Eigen::Matrix<double, 6, 6>& lambda, const
 }
 
 CartesianTwist CartesianAcceleration::operator*(const std::chrono::nanoseconds& dt) const {
-  // operations
-  CartesianTwist twist(this->get_name(), this->get_reference_frame());
-  // convert the period to a double with the second as reference
-  double period = dt.count();
-  period /= 1e9;
-  // convert the acceleration into a twist
-  twist.set_linear_velocity(period * this->get_linear_acceleration());
-  twist.set_angular_velocity(period * this->get_angular_acceleration());
-  return twist;
+  return this->integrate(dt);
 }
 
 CartesianTwist operator*(const std::chrono::nanoseconds& dt, const CartesianAcceleration& acceleration) {
-  return acceleration * dt;
+  return acceleration.integrate(dt);
 }
 
 CartesianAcceleration& CartesianAcceleration::operator/=(double lambda) {

source/state_representation/src/space/cartesian/CartesianPose.cpp

@@ -53,7 +53,7 @@ CartesianPose::CartesianPose(const CartesianState& state) : CartesianState(state
 
 CartesianPose::CartesianPose(const CartesianPose& pose) : CartesianPose(static_cast<const CartesianState&>(pose)) {}
 
-CartesianPose::CartesianPose(const CartesianTwist& twist) : CartesianPose(std::chrono::seconds(1) * twist) {}
+CartesianPose::CartesianPose(const CartesianTwist& twist) : CartesianPose(twist.integrate(1.0)) {}
 
 CartesianPose CartesianPose::Identity(const std::string& name, const std::string& reference) {
   return CartesianState::Identity(name, reference);
@@ -84,6 +84,19 @@ CartesianPose CartesianPose::copy() const {
   return result;
 }
 
+CartesianTwist CartesianPose::differentiate(double dt) const {
+  CartesianTwist twist(this->get_name(), this->get_reference_frame());
+  twist.set_linear_velocity(this->get_position() / dt);
+  // set angular velocity from the log of the quaternion error
+  Eigen::Quaterniond log_q = math_tools::log(this->get_orientation());
+  twist.set_angular_velocity(2 * log_q.vec() / dt);
+  return twist;
+}
+
+CartesianTwist CartesianPose::differentiate(const std::chrono::nanoseconds& dt) const {
+  return this->differentiate(dt.count() / 1e9);
+}
+
 CartesianPose CartesianPose::inverse() const {
   return this->CartesianState::inverse();
 }
@@ -149,17 +162,7 @@ CartesianPose CartesianPose::operator/(double lambda) const {
 }
 
 CartesianTwist CartesianPose::operator/(const std::chrono::nanoseconds& dt) const {
-  // operations
-  CartesianTwist twist(this->get_name(), this->get_reference_frame());
-  // convert the period to a double with the second as reference
-  double period = dt.count();
-  period /= 1e9;
-  // set linear velocity
-  twist.set_linear_velocity(this->get_position() / period);
-  // set angular velocity from the log of the quaternion error
-  Eigen::Quaterniond log_q = math_tools::log(this->get_orientation());
-  twist.set_angular_velocity(2 * log_q.vec() / period);
-  return twist;
+  return this->differentiate(dt);
 }
 
 CartesianPose& CartesianPose::operator+=(const CartesianPose& pose) {

source/state_representation/src/space/cartesian/CartesianTwist.cpp

@@ -48,10 +48,10 @@ CartesianTwist::CartesianTwist(const CartesianState& state) : CartesianState(sta
 CartesianTwist::CartesianTwist(const CartesianTwist& twist) :
     CartesianTwist(static_cast<const CartesianState&>(twist)) {}
 
-CartesianTwist::CartesianTwist(const CartesianPose& pose) : CartesianTwist(pose / std::chrono::seconds(1)) {}
+CartesianTwist::CartesianTwist(const CartesianPose& pose) : CartesianTwist(pose.differentiate(1.0)) {}
 
 CartesianTwist::CartesianTwist(const CartesianAcceleration& acceleration) :
-    CartesianTwist(acceleration * std::chrono::seconds(1)) {}
+    CartesianTwist(acceleration.integrate(1.0)) {}
 
 CartesianTwist CartesianTwist::Zero(const std::string& name, const std::string& reference) {
   return CartesianState::Identity(name, reference);
@@ -101,6 +101,36 @@ CartesianTwist CartesianTwist::copy() const {
   return result;
 }
 
+CartesianAcceleration CartesianTwist::differentiate(double dt) const {
+  CartesianAcceleration acceleration(this->get_name(), this->get_reference_frame());
+  acceleration.set_acceleration(this->get_twist() / dt);
+  return acceleration;
+}
+
+CartesianAcceleration CartesianTwist::differentiate(const std::chrono::nanoseconds& dt) const {
+  // convert the dt to a double with the second as reference
+  return this->differentiate(dt.count() / 1e9);
+}
+
+CartesianPose CartesianTwist::integrate(double dt) const {
+  CartesianPose displacement(this->get_name(), this->get_reference_frame());
+  displacement.set_position(dt * this->get_linear_velocity());
+  Eigen::Quaterniond angular_displacement = Eigen::Quaterniond::Identity();
+  double angular_norm = this->get_angular_velocity().norm();
+  if (angular_norm > 1e-4) {
+    double theta = angular_norm * dt * 0.5;
+    angular_displacement.w() = cos(theta);
+    angular_displacement.vec() = this->get_angular_velocity() / angular_norm * sin(theta);
+  }
+  displacement.set_orientation(angular_displacement);
+  return displacement;
+}
+
+CartesianPose CartesianTwist::integrate(const std::chrono::nanoseconds& dt) const {
+  // convert the dt to a double with the second as reference
+  return this->integrate(dt.count() / 1e9);
+}
+
 CartesianTwist CartesianTwist::inverse() const {
   return this->CartesianState::inverse();
 }
@@ -133,26 +163,11 @@ CartesianTwist operator*(const Eigen::Matrix<double, 6, 6>& lambda, const Cartes
 }
 
 CartesianPose CartesianTwist::operator*(const std::chrono::nanoseconds& dt) const {
-  // operations
-  CartesianPose displacement(this->get_name(), this->get_reference_frame());
-  // convert the period to a double with the second as reference
-  double period = dt.count();
-  period /= 1e9;
-  // convert the velocities into a displacement
-  displacement.set_position(period * this->get_linear_velocity());
-  Eigen::Quaterniond angular_displacement = Eigen::Quaterniond::Identity();
-  double angular_norm = this->get_angular_velocity().norm();
-  if (angular_norm > 1e-4) {
-    double theta = angular_norm * period * 0.5;
-    angular_displacement.w() = cos(theta);
-    angular_displacement.vec() = this->get_angular_velocity() / angular_norm * sin(theta);
-  }
-  displacement.set_orientation(angular_displacement);
-  return displacement;
+  return this->integrate(dt);
 }
 
 CartesianPose operator*(const std::chrono::nanoseconds& dt, const CartesianTwist& twist) {
-  return twist * dt;
+  return twist.integrate(dt);
 }
 
 CartesianTwist& CartesianTwist::operator/=(double lambda) {
@@ -165,13 +180,7 @@ CartesianTwist CartesianTwist::operator/(double lambda) const {
 }
 
 CartesianAcceleration CartesianTwist::operator/(const std::chrono::nanoseconds& dt) const {
-  CartesianAcceleration acceleration(this->get_name(), this->get_reference_frame());
-  // convert the period to a double with the second as reference
-  double period = dt.count();
-  period /= 1e9;
-  acceleration.set_linear_acceleration(this->get_linear_velocity() / period);
-  acceleration.set_angular_acceleration(this->get_angular_velocity() / period);
-  return acceleration;
+  return this->differentiate(dt);
 }
 
 CartesianTwist& CartesianTwist::operator+=(const CartesianTwist& twist) {

source/state_representation/test/tests/space/cartesian/test_cartesian_acceleration.cpp

@@ -117,3 +117,21 @@ TEST(CartesianAccelerationTest, TestAccelerationNorms) {
   EXPECT_NEAR(twist_norms[0], ct.get_linear_acceleration().norm(), tolerance);
   EXPECT_NEAR(twist_norms[1], ct.get_angular_acceleration().norm(), tolerance);
 }
+
+TEST(CartesianAccelerationTest, TestIntegrate) {
+  auto acc = CartesianAcceleration::Random("test");
+  auto dt1 = 0.1;
+  std::chrono::milliseconds dt2(100);
+  auto res1 = acc * dt2;
+  EXPECT_EQ(res1.get_type(), StateType::CARTESIAN_TWIST);
+  EXPECT_TRUE((dt1 * acc.get_acceleration()).isApprox(res1.get_twist()));
+  auto res2 = dt2 * acc;
+  EXPECT_EQ(res2.get_type(), StateType::CARTESIAN_TWIST);
+  EXPECT_TRUE((dt1 * acc.get_acceleration()).isApprox(res2.get_twist()));
+  auto res3 = acc.integrate(dt1);
+  EXPECT_EQ(res3.get_type(), StateType::CARTESIAN_TWIST);
+  EXPECT_TRUE((dt1 * acc.get_acceleration()).isApprox(res3.get_twist()));
+
+  CartesianTwist twist(acc);
+  EXPECT_TRUE(acc.get_acceleration().isApprox(twist.get_twist()));
+}