[Spatial] Add dualActionMatrix to Motion class

src/spatial/motion.hpp

@@ -55,6 +55,7 @@ namespace se3
     operator Vector6 () const { return toVector(); }
 
     ActionMatrix_t toActionMatrix() const { return derived().toActionMatrix_impl(); }
+    ActionMatrix_t toDualActionMatrix() const { return derived().toDualActionMatrix_impl(); }
     operator Matrix6 () const { return toActionMatrix(); }
 
     bool operator==(const Derived_t & other) const {return derived().isEqual(other);}
@@ -160,6 +161,16 @@ namespace se3
 
       return X;
     }
+    
+    ActionMatrix_t toDualActionMatrix_impl () const
+    {
+      ActionMatrix_t X;
+      X.template block <3,3> (ANGULAR, ANGULAR) = X.template block <3,3> (LINEAR, LINEAR) = skew (angular_impl());
+      X.template block <3,3> (ANGULAR, LINEAR) = skew (linear_impl());
+      X.template block <3,3> (LINEAR, ANGULAR).setZero ();
+      
+      return X;
+    }
 
     // Getters
     ConstAngular_t angular_impl() const { return data.template segment<3> (ANGULAR); }

unittest/tspatial.cpp

@@ -134,8 +134,21 @@ BOOST_AUTO_TEST_CASE ( test_Motion )
   Motion vxv = bv.cross(bv);
   BOOST_CHECK_SMALL(vxv.toVector().tail(3).norm(), 1e-3); //previously ensure that (vxv.toVector().tail(3).isMuchSmallerThan(1e-3));
 
+  // Test Action Matrix
+  Motion v2xv = bv2.cross(bv);
+  Motion::ActionMatrix_t actv2 = bv2.toActionMatrix();
+  
+  BOOST_CHECK(v2xv.toVector().isApprox(actv2*bv.toVector()));
+  
+  // Test Dual Action Matrix
+  Force f(bv.toVector());
+  Force v2xf = bv2.cross(f);
+  Motion::ActionMatrix_t dualactv2 = bv2.toDualActionMatrix();
+  
+  BOOST_CHECK(v2xf.toVector().isApprox(dualactv2*f.toVector()));
+  BOOST_CHECK(dualactv2.isApprox(-actv2.transpose()));
+  
   // Simple test for cross product vxf
-  Force f = Force(bv.toVector());
   Force vxf = bv.cross(f);
   BOOST_CHECK(vxf.linear().isApprox(bv.angular().cross(f.linear()), 1e-12));
   BOOST_CHECK_SMALL(vxf.angular().norm(), 1e-3);//previously ensure that ( vxf.angular().isMuchSmallerThan(1e-3));