diff --git a/CHANGELOG.md b/CHANGELOG.md index 0ace72e79..4eeb2ce81 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/python/source/state_representation/bind_joint_space.cpp b/python/source/state_representation/bind_joint_space.cpp index 2f30f4ead..c860f014b 100644 --- a/python/source/state_representation/bind_joint_space.cpp +++ b/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(&JointPositions::set_data), "Set the positions data from a vector", "data"_a); c.def("set_data", py::overload_cast&>(&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(&JointVelocities::set_data), "Set the velocities data from a vector", "data"_a); c.def("set_data", py::overload_cast&>(&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(&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(&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(&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(&JointAccelerations::set_data), "Set the accelerations data from a vector", "data"_a); c.def("set_data", py::overload_cast&>(&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(&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(&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(&JointAccelerations::clamp), "Clamp inplace the magnitude of the accelerations to the values in argument", "max_absolute_value_array"_a, "noise_ratio_array"_a); diff --git a/source/state_representation/include/state_representation/space/joint/JointAccelerations.hpp b/source/state_representation/include/state_representation/space/joint/JointAccelerations.hpp index 95bd4c963..d32bbd3b0 100644 --- a/source/state_representation/include/state_representation/space/joint/JointAccelerations.hpp +++ b/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) diff --git a/source/state_representation/include/state_representation/space/joint/JointPositions.hpp b/source/state_representation/include/state_representation/space/joint/JointPositions.hpp index 649ec8fff..acce0c31f 100644 --- a/source/state_representation/include/state_representation/space/joint/JointPositions.hpp +++ b/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 */ diff --git a/source/state_representation/include/state_representation/space/joint/JointVelocities.hpp b/source/state_representation/include/state_representation/space/joint/JointVelocities.hpp index af3e0b343..1f17c1332 100644 --- a/source/state_representation/include/state_representation/space/joint/JointVelocities.hpp +++ b/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) diff --git a/source/state_representation/src/space/joint/JointAccelerations.cpp b/source/state_representation/src/space/joint/JointAccelerations.cpp index 67960485c..480e9e400 100644 --- a/source/state_representation/src/space/joint/JointAccelerations.cpp +++ b/source/state_representation/src/space/joint/JointAccelerations.cpp @@ -44,7 +44,7 @@ JointAccelerations::JointAccelerations(const JointAccelerations& accelerations) JointAccelerations(static_cast(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) { diff --git a/source/state_representation/src/space/joint/JointPositions.cpp b/source/state_representation/src/space/joint/JointPositions.cpp index cfdf2f1ce..781c53222 100644 --- a/source/state_representation/src/space/joint/JointPositions.cpp +++ b/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(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) { diff --git a/source/state_representation/src/space/joint/JointVelocities.cpp b/source/state_representation/src/space/joint/JointVelocities.cpp index 38a4038ce..1c01ee99f 100644 --- a/source/state_representation/src/space/joint/JointVelocities.cpp +++ b/source/state_representation/src/space/joint/JointVelocities.cpp @@ -44,10 +44,9 @@ JointVelocities::JointVelocities(const JointVelocities& velocities) : JointVelocities(static_cast(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) { diff --git a/source/state_representation/test/tests/space/joint/test_joint_accelerations.cpp b/source/state_representation/test/tests/space/joint/test_joint_accelerations.cpp index a68508a32..e7dc19f91 100644 --- a/source/state_representation/test/tests/space/joint/test_joint_accelerations.cpp +++ b/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())); } diff --git a/source/state_representation/test/tests/space/joint/test_joint_positions.cpp b/source/state_representation/test/tests/space/joint/test_joint_positions.cpp index de1f71e1b..cfc4727d8 100644 --- a/source/state_representation/test/tests/space/joint/test_joint_positions.cpp +++ b/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())); } diff --git a/source/state_representation/test/tests/space/joint/test_joint_velocities.cpp b/source/state_representation/test/tests/space/joint/test_joint_velocities.cpp index 079e56ab4..e57277c95 100644 --- a/source/state_representation/test/tests/space/joint/test_joint_velocities.cpp +++ b/source/state_representation/test/tests/space/joint/test_joint_velocities.cpp @@ -189,29 +189,35 @@ TEST(JointVelocitiesTest, MatrixMultiplication) { EXPECT_THROW(gains * jv, exceptions::IncompatibleSizeException); } -TEST(JointVelocitiesTest, ChronoMultiplication) { - JointVelocities jv = JointVelocities::Random("test", 3); - EXPECT_EQ(jv.get_type(), StateType::JOINT_VELOCITIES); - auto time = std::chrono::seconds(2); - JointPositions jp1 = jv * time; - EXPECT_EQ(jp1.get_type(), StateType::JOINT_POSITIONS); - EXPECT_EQ(jp1.get_positions(), jv.get_velocities() * time.count()); - JointPositions jp2 = time * jv; - EXPECT_EQ(jp1.get_type(), StateType::JOINT_POSITIONS); - EXPECT_EQ(jp2.get_positions(), jv.get_velocities() * time.count()); - - JointVelocities empty; - EXPECT_THROW(empty * time, exceptions::EmptyStateException); +TEST(JointVelocitiesTest, TestIntegrate) { + auto jv = JointVelocities::Random("test", 3); + auto dt1 = 0.1; + std::chrono::milliseconds dt2(100); + auto res1 = jv * dt2; + EXPECT_EQ(res1.get_type(), StateType::JOINT_POSITIONS); + EXPECT_TRUE((dt1 * jv.get_velocities()).isApprox(res1.get_positions())); + auto res2 = dt2 * jv; + EXPECT_EQ(res2.get_type(), StateType::JOINT_POSITIONS); + EXPECT_TRUE((dt1 * jv.get_velocities()).isApprox(res2.get_positions())); + auto res3 = jv.integrate(dt1); + EXPECT_EQ(res3.get_type(), StateType::JOINT_POSITIONS); + EXPECT_TRUE((dt1 * jv.get_velocities()).isApprox(res3.get_positions())); + + JointPositions jp(jv); + EXPECT_TRUE(jv.get_velocities().isApprox(jp.get_positions())); } -TEST(JointVelocitiesTest, ChronoDivision) { - JointVelocities jv = JointVelocities::Random("test", 3); - EXPECT_EQ(jv.get_type(), StateType::JOINT_VELOCITIES); - auto time = std::chrono::seconds(2); - JointAccelerations ja = jv / time; - EXPECT_EQ(ja.get_type(), StateType::JOINT_ACCELERATIONS); - EXPECT_EQ(ja.get_accelerations(), jv.get_velocities() / time.count()); - - JointVelocities empty; - EXPECT_THROW(empty / time, exceptions::EmptyStateException); +TEST(JointVelocitiesTest, TestDifferentiate) { + auto jv = JointVelocities::Random("test", 3); + auto dt1 = 0.1; + std::chrono::milliseconds dt2(100); + auto res1 = jv / dt2; + EXPECT_EQ(res1.get_type(), StateType::JOINT_ACCELERATIONS); + EXPECT_TRUE(jv.get_velocities().isApprox(dt1 * res1.get_accelerations())); + auto res2 = jv.differentiate(dt1); + EXPECT_EQ(res2.get_type(), StateType::JOINT_ACCELERATIONS); + EXPECT_TRUE(jv.get_velocities().isApprox(dt1 * res2.get_accelerations())); + + JointAccelerations ja(jv); + EXPECT_TRUE(ja.get_accelerations().isApprox(jv.get_velocities())); }