From 6fc945c3166ee3ab5a84ab7f10961778177367ac Mon Sep 17 00:00:00 2001 From: Dominic Reber <71256590+domire8@users.noreply.github.com> Date: Fri, 10 Jan 2025 14:40:00 +0100 Subject: [PATCH] feat: improve support for transformation matrices (#214) --- CHANGELOG.md | 4 ++++ .../bind_cartesian_space.cpp | 5 +++++ .../space/cartesian/test_cartesian_pose.py | 3 +++ .../space/cartesian/test_cartesian_state.py | 4 ++++ .../space/cartesian/CartesianAcceleration.hpp | 1 + .../space/cartesian/CartesianPose.hpp | 22 ++++++++++++------- .../space/cartesian/CartesianState.hpp | 6 +++++ .../space/cartesian/CartesianTwist.hpp | 1 + .../space/cartesian/CartesianWrench.hpp | 1 + .../src/space/cartesian/CartesianPose.cpp | 7 ++++++ .../src/space/cartesian/CartesianState.cpp | 5 +++++ .../space/cartesian/test_cartesian_pose.cpp | 3 +++ .../space/cartesian/test_cartesian_state.cpp | 9 ++++++-- 13 files changed, 61 insertions(+), 10 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4b74fecaf..6cfb0830b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,10 @@ Release Versions - [6.3.0](#630) - [6.2.0](#620) +## Upcoming changes + +- feat: improve support for transformation matrices (#146) + ## 9.1.0 Version 9.1.0 adds `integrate` and `differentiate` methods to relevant Cartesian and joint states to allow integration diff --git a/python/source/state_representation/bind_cartesian_space.cpp b/python/source/state_representation/bind_cartesian_space.cpp index 1477760e6..b0ceb9c88 100644 --- a/python/source/state_representation/bind_cartesian_space.cpp +++ b/python/source/state_representation/bind_cartesian_space.cpp @@ -103,6 +103,7 @@ void cartesian_state(py::module_& m) { }, "Setter of the orientation attribute from a pyquaternion.Quaternion, numpy.array(w, x, y, z), or list(w, x, y, z)"); c.def("set_pose", py::overload_cast&>(&CartesianState::set_pose), "Setter of the pose from a 7d vector of position and orientation coefficients (x, y, z, qw, qx, qy, qz)"); c.def("set_pose", py::overload_cast&>(&CartesianState::set_pose), "Setter of the pose from a 7d list of position and orientation coefficients (x, y, z, qw, qx, qy, qz)"); + c.def("set_pose_from_transformation_matrix", &CartesianState::set_pose_from_transformation_matrix, "Setter of the pose from a transformation matrix"); c.def("set_linear_velocity", py::overload_cast(&CartesianState::set_linear_velocity), "Setter of the linear velocity attribute"); c.def("set_linear_velocity", py::overload_cast&>(&CartesianState::set_linear_velocity), "Setter of the linear velocity from a list"); @@ -204,6 +205,7 @@ void cartesian_pose(py::module_& m) { c.def_static("Identity", &CartesianPose::Identity, "Constructor for the identity pose", "name"_a, "reference_frame"_a=std::string("world")); c.def_static("Random", &CartesianPose::Random, "Constructor for a random pose", "name"_a, "reference_frame"_a=std::string("world")); + c.def_static("from_transformation_matrix", &CartesianPose::from_transformation_matrix, "Constructor of a Cartesian pose from a transformation matrix", "name"_a, "transformation_matrix"_a, "reference_frame"_a=std::string("world")); std::vector deleted_attributes = { "linear_velocity", @@ -326,6 +328,7 @@ void cartesian_twist(py::module_& m) { c.def(std::string("set_" + attr).c_str(), [](const CartesianTwist& twist) -> CartesianTwist { return twist; }, "Deleted method from parent class."); } c.def(std::string("get_orientation_coefficients").c_str(), [](const CartesianTwist&) -> void {}, "Deleted method from parent class."); + c.def(std::string("set_pose_from_transformation_matrix").c_str(), [](const CartesianTwist&) -> void {}, "Deleted method from parent class."); c.def(py::self *= double()); c.def(py::self * double()); @@ -436,6 +439,7 @@ void cartesian_acceleration(py::module_& m) { c.def(std::string("set_" + attr).c_str(), [](const CartesianAcceleration& acceleration) -> CartesianAcceleration { return acceleration; }, "Deleted method from parent class."); } c.def(std::string("get_orientation_coefficients").c_str(), [](const CartesianAcceleration&) -> void {}, "Deleted method from parent class."); + c.def(std::string("set_pose_from_transformation_matrix").c_str(), [](const CartesianTwist&) -> void {}, "Deleted method from parent class."); c.def(py::self *= double()); c.def(py::self * double()); @@ -532,6 +536,7 @@ void cartesian_wrench(py::module_& m) { c.def(std::string("set_" + attr).c_str(), [](const CartesianWrench& wrench) -> CartesianWrench { return wrench; }, "Deleted method from parent class."); } c.def(std::string("get_orientation_coefficients").c_str(), [](const CartesianWrench&) -> void {}, "Deleted method from parent class."); + c.def(std::string("set_pose_from_transformation_matrix").c_str(), [](const CartesianTwist&) -> void {}, "Deleted method from parent class."); c.def(py::self *= double()); c.def(py::self * double()); diff --git a/python/test/state_representation/space/cartesian/test_cartesian_pose.py b/python/test/state_representation/space/cartesian/test_cartesian_pose.py index bfe36e6f2..35abfc03d 100755 --- a/python/test/state_representation/space/cartesian/test_cartesian_pose.py +++ b/python/test/state_representation/space/cartesian/test_cartesian_pose.py @@ -27,6 +27,9 @@ def test_constructors(self): self.assert_np_array_equal(A.get_position(), [1, 2, 3]) self.assert_np_array_equal(A.get_orientation().elements, [1, 2, 3, 4] / np.linalg.norm([1, 2, 3, 4])) + B = CartesianPose().from_transformation_matrix(A.get_name(), A.get_transformation_matrix()) + self.assert_np_array_equal(B.get_pose(), A.get_pose()) + def test_copy(self): state = CartesianPose().Random("test") for state_copy in [copy.copy(state), copy.deepcopy(state)]: diff --git a/python/test/state_representation/space/cartesian/test_cartesian_state.py b/python/test/state_representation/space/cartesian/test_cartesian_state.py index ddf6f684c..d67e83166 100755 --- a/python/test/state_representation/space/cartesian/test_cartesian_state.py +++ b/python/test/state_representation/space/cartesian/test_cartesian_state.py @@ -58,6 +58,7 @@ 'set_name', 'set_orientation', 'set_pose', + 'set_pose_from_transformation_matrix', 'set_position', 'set_reference_frame', 'set_torque', @@ -228,6 +229,9 @@ def test_get_set_fields(self): assert_array_almost_equal(np.hstack((position, orientation_vec)), cs.get_pose()) with self.assertRaises(IncompatibleSizeError): cs.set_pose(position) + cs2 = CartesianState().Identity(cs.get_name()) + cs2.set_pose_from_transformation_matrix(cs.get_transformation_matrix()) + assert_array_almost_equal(cs2.get_pose(), cs.get_pose()) # twist linear_velocity = np.random.rand(3) diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp index d3cd2f0c3..82d98e302 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp @@ -39,6 +39,7 @@ class CartesianAcceleration : public CartesianState { void set_pose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation) = delete; void set_pose(const Eigen::Matrix& pose) = delete; void set_pose(const std::vector& pose) = delete; + void set_pose_from_transformation_matrix(const Eigen::Matrix4d& transformation_matrix) = delete; void set_linear_velocity(const Eigen::Vector3d& linear_velocity) = delete; void set_linear_velocity(const std::vector& linear_velocity) = delete; void set_linear_velocity(const double& x, const double& y, const double& z) = delete; diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp index 3be6c1320..cf3909276 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp @@ -1,8 +1,8 @@ #pragma once +#include "state_representation/space/cartesian/CartesianAcceleration.hpp" #include "state_representation/space/cartesian/CartesianState.hpp" #include "state_representation/space/cartesian/CartesianTwist.hpp" -#include "state_representation/space/cartesian/CartesianAcceleration.hpp" #include "state_representation/space/cartesian/CartesianWrench.hpp" namespace state_representation { @@ -67,7 +67,6 @@ class CartesianPose : public CartesianState { CartesianState operator-(const CartesianAcceleration& acceleration) const = delete; CartesianState operator-(const CartesianWrench& wrench) const = delete; - /** * @brief Empty constructor */ @@ -102,8 +101,7 @@ class CartesianPose : public CartesianState { * @param reference The name of the reference frame (default is "world") */ explicit CartesianPose( - const std::string& name, const Eigen::Vector3d& position, const std::string& reference = "world" - ); + const std::string& name, const Eigen::Vector3d& position, const std::string& reference = "world"); /** * @brief Constructor of a Cartesian pose from a position given as three scalar coordinates @@ -122,8 +120,7 @@ class CartesianPose : public CartesianState { * @param reference The name of the reference frame (default is "world") */ explicit CartesianPose( - const std::string& name, const Eigen::Quaterniond& orientation, const std::string& reference = "world" - ); + const std::string& name, const Eigen::Quaterniond& orientation, const std::string& reference = "world"); /** * @brief Constructor of a Cartesian pose from a position given as a vector of coordinates and a quaternion @@ -134,8 +131,7 @@ class CartesianPose : public CartesianState { */ explicit CartesianPose( const std::string& name, const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation, - const std::string& reference = "world" - ); + const std::string& reference = "world"); /** * @brief Constructor for the identity pose @@ -153,6 +149,16 @@ class CartesianPose : public CartesianState { */ static CartesianPose Random(const std::string& name, const std::string& reference = "world"); + /** + * @brief Constructor of a Cartesian pose from a transformation matrix + * @param name The name of the state + * @param transformation_matrix The transformation matrix + * @param reference The name of the reference frame (default is "world") + * @return Cartesian pose + */ + static CartesianPose from_transformation_matrix( + const std::string& name, const Eigen::Matrix4d& transformation_matrix, const std::string& reference = "world"); + /** * @brief Copy assignment operator that has to be defined to the custom assignment operator * @param pose The pose with value to assign diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp index 871ca6131..01dad24a5 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp @@ -234,6 +234,12 @@ class CartesianState : public SpatialState { */ void set_pose(const std::vector& pose); + /** + * @brief Setter of the pose from a transformation matrix + * @param transformation_matrix The pose as a transformation matrix + */ + void set_pose_from_transformation_matrix(const Eigen::Matrix4d& transformation_matrix); + /** * @brief Setter of the linear velocity attribute */ diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp index 0d77404e8..13b957bff 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp @@ -39,6 +39,7 @@ class CartesianTwist : public CartesianState { void set_pose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation) = delete; void set_pose(const Eigen::Matrix& pose) = delete; void set_pose(const std::vector& pose) = delete; + void set_pose_from_transformation_matrix(const Eigen::Matrix4d& transformation_matrix) = delete; void set_linear_acceleration(const Eigen::Vector3d& linear_acceleration) = delete; void set_linear_acceleration(const std::vector& linear_acceleration) = delete; void set_linear_acceleration(const double& x, const double& y, const double& z) = delete; diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp index 9dc82597c..3f5c19de5 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp @@ -39,6 +39,7 @@ class CartesianWrench : public CartesianState { void set_pose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation) = delete; void set_pose(const Eigen::Matrix& pose) = delete; void set_pose(const std::vector& pose) = delete; + void set_pose_from_transformation_matrix(const Eigen::Matrix4d& transformation_matrix) = delete; void set_linear_velocity(const Eigen::Vector3d& linear_velocity) = delete; void set_linear_velocity(const std::vector& linear_velocity) = delete; void set_linear_velocity(const double& x, const double& y, const double& z) = delete; diff --git a/source/state_representation/src/space/cartesian/CartesianPose.cpp b/source/state_representation/src/space/cartesian/CartesianPose.cpp index 1539d79f6..bc14d702d 100644 --- a/source/state_representation/src/space/cartesian/CartesianPose.cpp +++ b/source/state_representation/src/space/cartesian/CartesianPose.cpp @@ -63,6 +63,13 @@ CartesianPose CartesianPose::Random(const std::string& name, const std::string& return CartesianPose(name, Eigen::Vector3d::Random(), Eigen::Quaterniond::UnitRandom(), reference); } +CartesianPose CartesianPose::from_transformation_matrix( + const std::string& name, const Eigen::Matrix4d& transformation_matrix, const std::string& reference) { + auto pose = CartesianPose(name, reference); + pose.set_pose_from_transformation_matrix(transformation_matrix); + return pose; +} + Eigen::VectorXd CartesianPose::data() const { return this->get_pose(); } diff --git a/source/state_representation/src/space/cartesian/CartesianState.cpp b/source/state_representation/src/space/cartesian/CartesianState.cpp index c44a4e5aa..65471ab7a 100644 --- a/source/state_representation/src/space/cartesian/CartesianState.cpp +++ b/source/state_representation/src/space/cartesian/CartesianState.cpp @@ -330,6 +330,11 @@ void CartesianState::set_pose(const std::vector& pose) { this->set_state_variable(pose, CartesianStateVariable::POSE); } +void CartesianState::set_pose_from_transformation_matrix(const Eigen::Matrix4d& transformation_matrix) { + this->set_position(transformation_matrix.topRightCorner(3, 1)); + this->set_orientation(Eigen::Quaterniond(Eigen::Matrix3d(transformation_matrix.topLeftCorner(3, 3)))); +} + void CartesianState::set_linear_velocity(const Eigen::Vector3d& linear_velocity) { this->set_state_variable(linear_velocity, CartesianStateVariable::LINEAR_VELOCITY); } diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_pose.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_pose.cpp index 54452a3f2..af3abcf41 100644 --- a/source/state_representation/test/tests/space/cartesian/test_cartesian_pose.cpp +++ b/source/state_representation/test/tests/space/cartesian/test_cartesian_pose.cpp @@ -49,6 +49,9 @@ TEST(CartesianPoseTest, RandomPoseInitialization) { EXPECT_NE(random.get_orientation().y(), 0); EXPECT_NE(random.get_orientation().z(), 0); expect_only_pose(random); + + auto new_pose = CartesianPose::from_transformation_matrix(random.get_name(), random.get_transformation_matrix()); + EXPECT_TRUE(random.get_pose().isApprox(new_pose.get_pose())); } TEST(CartesianPoseTest, CopyPose) { diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp index a46091684..b95f94bdc 100644 --- a/source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp +++ b/source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp @@ -155,6 +155,11 @@ TEST(CartesianStateTest, GetSetFields) { // pose EXPECT_THROW(cs.set_pose(std::vector(8)), exceptions::IncompatibleSizeException); + auto cs2 = CartesianState::Random(cs.get_name()); + EXPECT_FALSE(cs2.get_pose().isApprox(cs.get_pose())); + cs2.set_pose_from_transformation_matrix(cs.get_transformation_matrix()); + EXPECT_TRUE(cs2.get_position().isApprox(cs.get_position())); + EXPECT_TRUE(cs2.get_orientation().angularDistance(cs.get_orientation()) < 1e-3); // linear velocity data = Eigen::Vector3d::Random(); @@ -733,7 +738,7 @@ TEST(CartesianStateTest, ScalarMultiplication) { CartesianState cscaled = scalar * cs; EXPECT_TRUE(cscaled.get_position().isApprox(scalar * cs.get_position())); auto qscaled = math_tools::exp(math_tools::log(cs.get_orientation()), scalar); - EXPECT_TRUE(cscaled.get_orientation().coeffs().isApprox(qscaled.coeffs())); + EXPECT_TRUE(cscaled.get_orientation().angularDistance(qscaled) < 1e-3); EXPECT_TRUE(cscaled.get_twist().isApprox(scalar * cs.get_twist())); EXPECT_TRUE(cscaled.get_acceleration().isApprox(scalar * cs.get_acceleration())); EXPECT_TRUE(cscaled.get_wrench().isApprox(scalar * cs.get_wrench())); @@ -751,7 +756,7 @@ TEST(CartesianStateTest, ScalarDivision) { CartesianState cscaled = cs / scalar; EXPECT_TRUE(cscaled.get_position().isApprox(cs.get_position() / scalar)); auto qscaled = math_tools::exp(math_tools::log(cs.get_orientation()), 1.0 / scalar); - EXPECT_TRUE(cscaled.get_orientation().coeffs().isApprox(qscaled.coeffs())); + EXPECT_TRUE(cscaled.get_orientation().angularDistance(qscaled) < 1e-3); EXPECT_TRUE(cscaled.get_twist().isApprox(cs.get_twist() / scalar)); EXPECT_TRUE(cscaled.get_acceleration().isApprox(cs.get_acceleration() / scalar)); EXPECT_TRUE(cscaled.get_wrench().isApprox(cs.get_wrench() / scalar));