[Spatial] Add time variation of a Inertia according to the current spatial velocity of a the body

src/spatial/inertia.hpp

 //
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2017 CNRS
 // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
 //
 // This file is part of Pinocchio
@@ -61,6 +61,7 @@ namespace se3
     Force operator*(const Motion & v) const    { return derived().__mult__(v); }
 
     Scalar vtiv(const Motion & v) const { return derived().vtiv_impl(v); }
+    Matrix6 variation(const Motion & v) const { return derived().variation_impl(v); }
 
     void setZero() { derived().setZero(); }
     void setIdentity() { derived().setIdentity(); }
@@ -306,6 +307,27 @@ namespace se3
       
       return res;
     }
+    
+    Matrix6 variation(const Motion & v) const
+    {
+      Matrix6 res;
+      const Motion mv(v*m);
+      
+      res.template block<3,3>(LINEAR,ANGULAR) = -skew(mv.linear()) - skewSquare(mv.angular(),c) + skewSquare(c,mv.angular());
+      res.template block<3,3>(ANGULAR,LINEAR) = res.template block<3,3>(LINEAR,ANGULAR).transpose();
+      
+      res.template block<3,3>(LINEAR,LINEAR) = mv.linear()*c.transpose(); // use as temporary variable
+      res.template block<3,3>(ANGULAR,ANGULAR) = res.template block<3,3>(LINEAR,LINEAR) - res.template block<3,3>(LINEAR,LINEAR).transpose();
+      res.template block<3,3>(ANGULAR,ANGULAR) = -skewSquare(mv.linear(),c) - skewSquare(c,mv.linear());
+      
+      res.template block<3,3>(LINEAR,LINEAR) = (typename Symmetric3::Matrix3)(I - AlphaSkewSquare(m,c));
+      
+      res.template block<3,3>(ANGULAR,ANGULAR) -= res.template block<3,3>(LINEAR,LINEAR) * skew(v.angular());
+      res.template block<3,3>(ANGULAR,ANGULAR) += cross(v.angular(),res.template block<3,3>(LINEAR,LINEAR));
+      
+      res.template block<3,3>(LINEAR,LINEAR).setZero();
+      return res;
+    }
 
     // Getters
     Scalar           mass()    const { return m; }

unittest/tspatial.cpp

@@ -341,6 +341,19 @@ BOOST_AUTO_TEST_CASE ( test_Inertia )
   Inertia aI_approx(aI);
   aI_approx.mass() += eps;
   BOOST_CHECK(aI_approx.isApprox(aI,eps));
+  
+  // Test Variation
+  Inertia::Matrix6 aIvariation = aI.variation(v);
+  
+  Motion::ActionMatrix_t vAction = v.toActionMatrix();
+  Motion::ActionMatrix_t vDualAction = v.toDualActionMatrix();
+  
+  Inertia::Matrix6 aImatrix = aI.matrix();
+  Inertia::Matrix6 aIvariation_ref = vDualAction * aImatrix - aImatrix * vAction;
+  
+  BOOST_CHECK(aIvariation.isApprox(aIvariation_ref));
+  BOOST_CHECK(vxIv.isApprox(Force(aIvariation*v.toVector())));
+  
 }
 
 BOOST_AUTO_TEST_CASE ( test_ActOnSet )