[Spatial] Add SE3 method to generate the dual action matrix

bindings/python/spatial/se3.hpp

@@ -69,8 +69,9 @@ namespace se3
                       "The translation part of the transformation."
                       )
         
-        .add_property("homogeneous",&SE3::toHomogeneousMatrix,"Returns the homegeneous matrix of *this.")
-        .add_property("action",&SE3::toActionMatrix,"Returns the action matrix of *this.")
+        .add_property("homogeneous",&SE3::toHomogeneousMatrix,"Returns the homegeneous matrix of *this (acting on SE3).")
+        .add_property("action",&SE3::toActionMatrix,"Returns the action matrix of *this (acting on Motion).")
+        .add_property("dualAction",&SE3::toDualActionMatrix,"Returns the dual action matrix of *this (acting on Force).")
         
         .def("setIdentity",&SE3PythonVisitor::setIdentity,"Set *this to the identity placement.")
         .def("setRandom",&SE3PythonVisitor::setRandom,"Set *this to a random placement.")

src/spatial/se3.hpp

@@ -77,6 +77,8 @@ namespace se3
         return derived().toActionMatrix_impl();
       }
       operator Matrix6() const { return toActionMatrix(); }
+    
+    Matrix6 toDualActionMatrix() const { return derived().toDualActionMatrix_impl(); }
 
 
 
@@ -254,6 +256,21 @@ namespace se3
       B.col(2) = trans.cross(rot.col(2));
       return M;
     }
+    
+    Matrix6 toDualActionMatrix_impl() const
+    {
+      typedef Eigen::Block<Matrix6,3,3> Block3;
+      Matrix6 M;
+      M.template block<3,3>(ANGULAR,ANGULAR)
+      = M.template block<3,3>(LINEAR,LINEAR) = rot;
+      M.template block<3,3>(LINEAR,ANGULAR).setZero();
+      Block3 B = M.template block<3,3>(ANGULAR,LINEAR);
+      
+      B.col(0) = trans.cross(rot.col(0));
+      B.col(1) = trans.cross(rot.col(1));
+      B.col(2) = trans.cross(rot.col(2));
+      return M;
+    }
 
     void disp_impl(std::ostream & os) const
     {

unittest/tspatial.cpp

@@ -65,10 +65,13 @@ BOOST_AUTO_TEST_CASE ( test_SE3 )
   Matrix6 bXc = bmc;
   Matrix6 aXc = amc;
   BOOST_CHECK(aXc.isApprox(aXb*bXc, 1e-12));
-
+  
   Matrix6 bXa = amb.inverse();
   BOOST_CHECK(bXa.isApprox(aXb.inverse(), 1e-12));
   
+  // Test dual action matrix
+  BOOST_CHECK(aXb.inverse().transpose().isApprox(amb.toDualActionMatrix(),1e-12));
+
   // Test isIdentity
   SE3 identity = SE3::Identity();
   BOOST_CHECK(identity.isIdentity());