feat: improve support for transformation matrices (#214)

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

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<const Eigen::Matrix<double, 7, 1>&>(&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<const std::vector<double>&>(&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<const Eigen::Vector3d&>(&CartesianState::set_linear_velocity), "Setter of the linear velocity attribute");
   c.def("set_linear_velocity", py::overload_cast<const std::vector<double>&>(&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<std::string> 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());

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)]:

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)

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<double, 7, 1>& pose) = delete;
   void set_pose(const std::vector<double>& 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<double>& linear_velocity) = delete;
   void set_linear_velocity(const double& x, const double& y, const double& z) = delete;

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

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

@@ -234,6 +234,12 @@ class CartesianState : public SpatialState {
    */
   void set_pose(const std::vector<double>& 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
    */

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<double, 7, 1>& pose) = delete;
   void set_pose(const std::vector<double>& 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<double>& linear_acceleration) = delete;
   void set_linear_acceleration(const double& x, const double& y, const double& z) = delete;

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<double, 7, 1>& pose) = delete;
   void set_pose(const std::vector<double>& 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<double>& linear_velocity) = delete;
   void set_linear_velocity(const double& x, const double& y, const double& z) = delete;

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();
 }

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

@@ -330,6 +330,11 @@ void CartesianState::set_pose(const std::vector<double>& 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);
 }

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) {

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<double>(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));