feat: add integrate and differentiate methods to joint types (#210)

CHANGELOG.md

@@ -20,7 +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)
+- feat: add integrate and differentiate methods to Cartesian and joint types (#193)
 - feat(robot-model): improve inverse kinematics (#208)
 
 ## 9.0.1

python/source/state_representation/bind_joint_space.cpp

@@ -185,6 +185,13 @@ void joint_positions(py::module_& m) {
   c.def("set_data", py::overload_cast<const Eigen::VectorXd&>(&JointPositions::set_data), "Set the positions data from a vector", "data"_a);
   c.def("set_data", py::overload_cast<const std::vector<double>&>(&JointPositions::set_data), "Set the positions data from a list", "data"_a);
 
+  c.def("differentiate", [](const JointPositions &positions, double dt) -> JointVelocities {
+    return positions.differentiate(dt);
+  }, "Differentiate joint positions over a time period in seconds", "dt"_a);
+  c.def("differentiate", [](const JointPositions &positions, const std::chrono::nanoseconds& dt) -> JointVelocities {
+    return positions.differentiate(dt);
+  }, "Differentiate joint positions over a time period", "dt"_a);
+
   c.def("__copy__", [](const JointPositions &positions) {
     return JointPositions(positions);
   });
@@ -270,6 +277,19 @@ void joint_velocities(py::module_& m) {
   c.def("set_data", py::overload_cast<const Eigen::VectorXd&>(&JointVelocities::set_data), "Set the velocities data from a vector", "data"_a);
   c.def("set_data", py::overload_cast<const std::vector<double>&>(&JointVelocities::set_data), "Set the velocities data from a list", "data"_a);
 
+  c.def("differentiate", [](const JointVelocities &velocities, double dt) -> JointAccelerations {
+    return velocities.differentiate(dt);
+  }, "Differentiate joint velocities over a time period in seconds", "dt"_a);
+  c.def("differentiate", [](const JointVelocities &velocities, const std::chrono::nanoseconds& dt) -> JointAccelerations {
+    return velocities.differentiate(dt);
+  }, "Differentiate joint velocities over a time period", "dt"_a);
+  c.def("integrate", [](const JointVelocities &velocities, double dt) -> JointPositions {
+    return velocities.integrate(dt);
+  }, "Integrate joint velocities over a time period in seconds", "dt"_a);
+  c.def("integrate", [](const JointVelocities &velocities, const std::chrono::nanoseconds& dt) -> JointPositions {
+    return velocities.integrate(dt);
+  }, "Integrate joint velocities over a time period", "dt"_a);
+
   c.def("clamp", py::overload_cast<double, double>(&JointVelocities::clamp), "Clamp inplace the magnitude of the velocity to the values in argument", "max_absolute_value"_a, "noise_ratio"_a=0.0);
   c.def("clamped", py::overload_cast<double, double>(&JointVelocities::clamp), "Return the velocity clamped to the values in argument", "max_absolute_value"_a, "noise_ratio"_a=0.0);
   c.def("clamp", py::overload_cast<const Eigen::ArrayXd&, const Eigen::ArrayXd&>(&JointVelocities::clamp), "Clamp inplace the magnitude of the velocity to the values in argument", "max_absolute_value_array"_a, "noise_ratio_array"_a);
@@ -358,6 +378,13 @@ void joint_accelerations(py::module_& m) {
   c.def("set_data", py::overload_cast<const Eigen::VectorXd&>(&JointAccelerations::set_data), "Set the accelerations data from a vector", "data"_a);
   c.def("set_data", py::overload_cast<const std::vector<double>&>(&JointAccelerations::set_data), "Set the accelerations data from a list", "data"_a);
 
+  c.def("integrate", [](const JointAccelerations &accelerations, double dt) -> JointVelocities {
+    return accelerations.integrate(dt);
+  }, "Integrate joint accelerations over a time period in seconds", "dt"_a);
+  c.def("integrate", [](const JointAccelerations &accelerations, const std::chrono::nanoseconds& dt) -> JointVelocities {
+    return accelerations.integrate(dt);
+  }, "Integrate joint accelerations over a time period", "dt"_a);
+
   c.def("clamp", py::overload_cast<double, double>(&JointAccelerations::clamp), "Clamp inplace the magnitude of the accelerations to the values in argument", "max_absolute_value"_a, "noise_ratio"_a=0.0);
   c.def("clamped", py::overload_cast<double, double>(&JointAccelerations::clamp), "Return the accelerations clamped to the values in argument", "max_absolute_value"_a, "noise_ratio"_a=0.0);
   c.def("clamp", py::overload_cast<const Eigen::ArrayXd&, const Eigen::ArrayXd&>(&JointAccelerations::clamp), "Clamp inplace the magnitude of the accelerations to the values in argument", "max_absolute_value_array"_a, "noise_ratio_array"_a);

source/state_representation/include/state_representation/space/joint/JointAccelerations.hpp

@@ -201,6 +201,20 @@ class JointAccelerations : public JointState {
    */
   JointAccelerations copy() const;
 
+  /**
+   * @brief Integrate joint accelerations over a time period
+   * @param dt The time period used for integration in seconds
+   * @return The resulting joint velocities after integration
+   */
+  JointVelocities integrate(double dt) const;
+
+  /**
+   * @brief Integrate joint accelerations over a time period
+   * @param dt The time period used for integration
+   * @return The resulting joint velocities after integration
+   */
+  JointVelocities integrate(const std::chrono::nanoseconds& dt) const;
+
   /**
    * @brief Scale inplace by a scalar
    * @copydetails JointState::operator*=(double)

source/state_representation/include/state_representation/space/joint/JointPositions.hpp

@@ -199,6 +199,20 @@ class JointPositions : public JointState {
    */
   JointPositions copy() const;
 
+  /**
+   * @brief Differentiate joint positions over a time period
+   * @param dt The time period used for derivation in seconds
+   * @return The resulting joint velocities after derivation
+   */
+  JointVelocities differentiate(double dt) const;
+
+  /**
+   * @brief Differentiate joint positions over a time period
+   * @param dt The time period used for derivation
+   * @return The resulting joint velocities after derivation
+   */
+  JointVelocities differentiate(const std::chrono::nanoseconds& dt) const;
+
   /**
    * @brief Scale inplace by a scalar
    * @copydetails JointState::operator*=(double)
@@ -249,7 +263,7 @@ class JointPositions : public JointState {
   JointPositions operator/(double lambda) const;
 
   /**
-   * @brief Differentiate joint positions pose over a time period
+   * @brief Differentiate joint positions over a time period
    * @param dt The time period used for derivation
    * @return The resulting joint velocities after derivation
    */

source/state_representation/include/state_representation/space/joint/JointVelocities.hpp

@@ -207,6 +207,34 @@ class JointVelocities : public JointState {
    */
   JointVelocities copy() const;
 
+  /**
+   * @brief Differentiate joint velocities over a time period
+   * @param dt The time period used for derivation in seconds
+   * @return The resulting joint accelerations after derivation
+   */
+  JointAccelerations differentiate(double dt) const;
+
+  /**
+   * @brief Differentiate joint velocities over a time period
+   * @param dt The time period used for derivation
+   * @return The resulting joint accelerations after derivation
+   */
+  JointAccelerations differentiate(const std::chrono::nanoseconds& dt) const;
+
+  /**
+   * @brief Integrate joint velocities over a time period
+   * @param dt The time period used for integration in seconds
+   * @return The resulting joint positions after integration
+   */
+  JointPositions integrate(double dt) const;
+
+  /**
+   * @brief Integrate joint velocities over a time period
+   * @param dt The time period used for integration
+   * @return The resulting joint positions after integration
+   */
+  JointPositions integrate(const std::chrono::nanoseconds& dt) const;
+
   /**
    * @brief Scale inplace by a scalar
    * @copydetails JointState::operator*=(double)

source/state_representation/src/space/joint/JointAccelerations.cpp

@@ -44,7 +44,7 @@ JointAccelerations::JointAccelerations(const JointAccelerations& accelerations)
     JointAccelerations(static_cast<const JointState&>(accelerations)) {}
 
 JointAccelerations::JointAccelerations(const JointVelocities& velocities) :
-    JointAccelerations(velocities / std::chrono::seconds(1)) {}
+    JointAccelerations(velocities.differentiate(1.0)) {}
 
 JointAccelerations JointAccelerations::Zero(const std::string& robot_name, unsigned int nb_joints) {
   return JointState::Zero(robot_name, nb_joints);
@@ -103,6 +103,15 @@ JointAccelerations JointAccelerations::copy() const {
   return result;
 }
 
+JointVelocities JointAccelerations::integrate(double dt) const {
+  return JointVelocities(this->get_name(), this->get_names(), dt * this->get_accelerations());
+}
+
+JointVelocities JointAccelerations::integrate(const std::chrono::nanoseconds& dt) const {
+  // convert the period to a double with the second as reference
+  return this->integrate(dt.count() / 1e9);  
+}
+
 JointAccelerations& JointAccelerations::operator*=(double lambda) {
   this->JointState::operator*=(lambda);
   return (*this);
@@ -125,18 +134,11 @@ JointAccelerations operator*(const Eigen::MatrixXd& lambda, const JointAccelerat
 }
 
 JointVelocities JointAccelerations::operator*(const std::chrono::nanoseconds& dt) const {
-  // operations
-  JointVelocities velocities(this->get_name(), this->get_names());
-  // convert the period to a double with the second as reference
-  double period = dt.count();
-  period /= 1e9;
-  // multiply the velocities by this period value and assign it as position
-  velocities.set_velocities(period * this->get_accelerations());
-  return velocities;
+  return this->integrate(dt);
 }
 
 JointVelocities operator*(const std::chrono::nanoseconds& dt, const JointAccelerations& accelerations) {
-  return accelerations * dt;
+  return accelerations.integrate(dt);
 }
 
 JointAccelerations& JointAccelerations::operator/=(double lambda) {

source/state_representation/src/space/joint/JointPositions.cpp

@@ -44,8 +44,7 @@ JointPositions::JointPositions(const JointState& state) : JointState(state) {
 JointPositions::JointPositions(const JointPositions& positions) :
     JointPositions(static_cast<const JointState&>(positions)) {}
 
-JointPositions::JointPositions(const JointVelocities& velocities) :
-    JointPositions(std::chrono::seconds(1) * velocities) {}
+JointPositions::JointPositions(const JointVelocities& velocities) : JointPositions(velocities.integrate(1.0)) {}
 
 JointPositions JointPositions::Zero(const std::string& robot_name, unsigned int nb_joints) {
   return JointState::Zero(robot_name, nb_joints);
@@ -102,6 +101,15 @@ JointPositions JointPositions::copy() const {
   return result;
 }
 
+JointVelocities JointPositions::differentiate(double dt) const {
+  return JointVelocities(this->get_name(), this->get_names(), this->get_positions() / dt);
+}
+
+JointVelocities JointPositions::differentiate(const std::chrono::nanoseconds& dt) const {
+  // convert the period to a double with the second as reference
+  return this->differentiate(dt.count() / 1e9);
+}
+
 JointPositions& JointPositions::operator*=(double lambda) {
   this->JointState::operator*=(lambda);
   return (*this);
@@ -133,14 +141,7 @@ JointPositions JointPositions::operator/(double lambda) const {
 }
 
 JointVelocities JointPositions::operator/(const std::chrono::nanoseconds& dt) const {
-  // operations
-  JointVelocities velocities(this->get_name(), this->get_names());
-  // convert the period to a double with the second as reference
-  double period = dt.count();
-  period /= 1e9;
-  // divide the positions by this period value and assign it as velocities
-  velocities.set_velocities(this->get_positions() / period);
-  return velocities;
+  return this->differentiate(dt);
 }
 
 JointPositions& JointPositions::operator+=(const JointPositions& positions) {

source/state_representation/src/space/joint/JointVelocities.cpp

@@ -44,10 +44,9 @@ JointVelocities::JointVelocities(const JointVelocities& velocities) :
     JointVelocities(static_cast<const JointState&>(velocities)) {}
 
 JointVelocities::JointVelocities(const JointAccelerations& accelerations) :
-    JointVelocities(std::chrono::seconds(1) * accelerations) {}
+    JointVelocities(accelerations.integrate(1.0)) {}
 
-JointVelocities::JointVelocities(const JointPositions& positions) :
-    JointVelocities(positions / std::chrono::seconds(1)) {}
+JointVelocities::JointVelocities(const JointPositions& positions) : JointVelocities(positions.differentiate(1.0)) {}
 
 JointVelocities JointVelocities::Zero(const std::string& robot_name, unsigned int nb_joints) {
   return JointState::Zero(robot_name, nb_joints);
@@ -104,6 +103,24 @@ JointVelocities JointVelocities::copy() const {
   return result;
 }
 
+JointAccelerations JointVelocities::differentiate(double dt) const {
+  return JointAccelerations(this->get_name(), this->get_names(), this->get_velocities() / dt);
+}
+
+JointAccelerations JointVelocities::differentiate(const std::chrono::nanoseconds& dt) const {
+  // convert the period to a double with the second as reference
+  return this->differentiate(dt.count() / 1e9);
+}
+
+JointPositions JointVelocities::integrate(double dt) const {
+  return JointPositions(this->get_name(), this->get_names(), dt * this->get_velocities());
+}
+
+JointPositions JointVelocities::integrate(const std::chrono::nanoseconds& dt) const {
+  // convert the period to a double with the second as reference
+  return this->integrate(dt.count() / 1e9);  
+}
+
 JointVelocities& JointVelocities::operator*=(double lambda) {
   this->JointState::operator*=(lambda);
   return (*this);
@@ -126,18 +143,11 @@ JointVelocities operator*(const Eigen::MatrixXd& lambda, const JointVelocities&
 }
 
 JointPositions JointVelocities::operator*(const std::chrono::nanoseconds& dt) const {
-  // operations
-  JointPositions displacement(this->get_name(), this->get_names());
-  // convert the period to a double with the second as reference
-  double period = dt.count();
-  period /= 1e9;
-  // multiply the velocities by this period value and assign it as position
-  displacement.set_positions(period * this->get_velocities());
-  return displacement;
+  return this->integrate(dt);
 }
 
 JointPositions operator*(const std::chrono::nanoseconds& dt, const JointVelocities& velocities) {
-  return velocities * dt;
+  return velocities.integrate(dt);
 }
 
 JointVelocities& JointVelocities::operator/=(double lambda) {
@@ -150,14 +160,7 @@ JointVelocities JointVelocities::operator/(double lambda) const {
 }
 
 JointAccelerations JointVelocities::operator/(const std::chrono::nanoseconds& dt) const {
-  // operations
-  JointAccelerations accelerations(this->get_name(), this->get_names());
-  // convert the period to a double with the second as reference
-  double period = dt.count();
-  period /= 1e9;
-  // divide the positions by this period value and assign it as velocities
-  accelerations.set_accelerations(this->get_velocities() / period);
-  return accelerations;
+  return this->differentiate(dt);
 }
 
 JointVelocities& JointVelocities::operator+=(const JointVelocities& velocities) {

source/state_representation/test/tests/space/joint/test_joint_accelerations.cpp

@@ -170,16 +170,20 @@ TEST(JointAccelerationsTest, MatrixMultiplication) {
   EXPECT_THROW(gains * ja, exceptions::IncompatibleSizeException);
 }
 
-TEST(JointAccelerationsTest, ChronoMultiplication) {
-  JointAccelerations ja = JointAccelerations::Random("test", 3);
-  auto time = std::chrono::seconds(2);
-  JointVelocities jv1 = ja * time;
-  EXPECT_EQ(jv1.get_type(), StateType::JOINT_VELOCITIES);
-  EXPECT_EQ(jv1.get_velocities(), ja.get_accelerations() * time.count());
-  JointVelocities jv2 = time * ja;
-  EXPECT_EQ(jv2.get_type(), StateType::JOINT_VELOCITIES);
-  EXPECT_EQ(jv2.get_velocities(), ja.get_accelerations() * time.count());
-
-  JointAccelerations empty;
-  EXPECT_THROW(empty * time, exceptions::EmptyStateException);
+TEST(JointAccelerationTest, TestIntegrate) {
+  auto ja = JointAccelerations::Random("test", 3);
+  auto dt1 = 0.1;
+  std::chrono::milliseconds dt2(100);
+  auto res1 = ja * dt2;
+  EXPECT_EQ(res1.get_type(), StateType::JOINT_VELOCITIES);
+  EXPECT_TRUE((dt1 * ja.get_accelerations()).isApprox(res1.get_velocities()));
+  auto res2 = dt2 * ja;
+  EXPECT_EQ(res2.get_type(), StateType::JOINT_VELOCITIES);
+  EXPECT_TRUE((dt1 * ja.get_accelerations()).isApprox(res2.get_velocities()));
+  auto res3 = ja.integrate(dt1);
+  EXPECT_EQ(res3.get_type(), StateType::JOINT_VELOCITIES);
+  EXPECT_TRUE((dt1 * ja.get_accelerations()).isApprox(res3.get_velocities()));
+
+  JointVelocities jv(ja);
+  EXPECT_TRUE(ja.get_accelerations().isApprox(jv.get_velocities()));
 }

source/state_representation/test/tests/space/joint/test_joint_positions.cpp

@@ -168,14 +168,17 @@ TEST(JointPositionsTest, MatrixMultiplication) {
   EXPECT_THROW(gains * jp, exceptions::IncompatibleSizeException);
 }
 
-TEST(JointPositionsTest, ChronoDivision) {
-  JointPositions jp = JointPositions::Random("test", 3);
-  EXPECT_EQ(jp.get_type(), StateType::JOINT_POSITIONS);
-  auto time = std::chrono::seconds(1);
-  JointVelocities jv = jp / time;
-  EXPECT_EQ(jv.get_type(), StateType::JOINT_VELOCITIES);
-  EXPECT_EQ(jv.get_velocities(), jp.get_positions() / time.count());
-
-  JointPositions empty;
-  EXPECT_THROW(empty / time, exceptions::EmptyStateException);
+TEST(JointPositionsTest, TestDifferentiate) {
+  auto jp = JointPositions::Random("test", 3);
+  auto dt1 = 0.1;
+  std::chrono::milliseconds dt2(100);
+  auto res1 = jp / dt2;
+  EXPECT_EQ(res1.get_type(), StateType::JOINT_VELOCITIES);
+  EXPECT_TRUE(jp.get_positions().isApprox(dt1 * res1.get_velocities()));
+  auto res2 = jp.differentiate(dt1);
+  EXPECT_EQ(res2.get_type(), StateType::JOINT_VELOCITIES);
+  EXPECT_TRUE(jp.get_positions().isApprox(dt1 * res2.get_velocities()));
+
+  JointVelocities jv(jp);
+  EXPECT_TRUE(jp.get_positions().isApprox(jv.get_velocities()));
 }