[C++][Python] Add acceleration related to gravity field

The idea is to separate the real spatial acceleration of a body to the
spatial acceleration containing the gravity field, which is useful in
the context of dynamical equations

src/algorithm/non-linear-effects.hpp

@@ -64,10 +64,10 @@ namespace se3
       data.v[i] = jdata.v();
       if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[(size_t) parent]);
       
-      data.a[i]  = jdata.c() + (data.v[i] ^ jdata.v());
-      data.a[i] += data.liMi[i].actInv(data.a[(size_t) parent]);
+      data.a_gf[i]  = jdata.c() + (data.v[i] ^ jdata.v());
+      data.a_gf[i] += data.liMi[i].actInv(data.a_gf[(size_t) parent]);
       
-      data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
+      data.f[i] = model.inertias[i]*data.a_gf[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
     }
 
   };
@@ -99,7 +99,7 @@ namespace se3
        const Eigen::VectorXd & v)
   {
     data.v[0].setZero ();
-    data.a[0] = -model.gravity;
+    data.a_gf[0] = -model.gravity;
 
     for( size_t i=1;i<(size_t) model.nbody;++i )
       {

src/algorithm/rnea.hpp

@@ -67,10 +67,10 @@ namespace se3
       data.v[(Model::Index)i] = jdata.v();
       if(parent>0) data.v[(Model::Index)i] += data.liMi[(Model::Index)i].actInv(data.v[parent]);
       
-      data.a[(Model::Index)i]  = jdata.S()*jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[(Model::Index)i] ^ jdata.v()) ; 
-      data.a[(Model::Index)i] += data.liMi[(Model::Index)i].actInv(data.a[parent]);
+      data.a_gf[(Model::Index)i] = jdata.S()*jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[(Model::Index)i] ^ jdata.v()) ;
+      data.a_gf[(Model::Index)i] += data.liMi[(Model::Index)i].actInv(data.a_gf[parent]);
       
-      data.f[(Model::Index)i] = model.inertias[(Model::Index)i]*data.a[(Model::Index)i] + model.inertias[(Model::Index)i].vxiv(data.v[(Model::Index)i]); // -f_ext
+      data.f[(Model::Index)i] = model.inertias[(Model::Index)i]*data.a_gf[(Model::Index)i] + model.inertias[(Model::Index)i].vxiv(data.v[(Model::Index)i]); // -f_ext
       return 0;
     }
 
@@ -103,8 +103,8 @@ namespace se3
        const Eigen::VectorXd & v,
        const Eigen::VectorXd & a)
   {
-    data.v[0] = Motion::Zero();
-    data.a[0] = -model.gravity;
+    data.v[0].setZero();
+    data.a_gf[0] = -model.gravity;
 
     for( int i=1;i<model.nbody;++i )
       {

src/multibody/model.hpp

@@ -120,6 +120,7 @@ namespace se3
     const Model& model;
     JointDataVector joints;
     std::vector<Motion> a;                // Body acceleration
+    std::vector<Motion> a_gf;             // Body acceleration with gravity
     std::vector<Motion> v;                // Body velocity
     std::vector<Force> f;                 // Body force
     std::vector<SE3> oMi;                 // Body absolute placement (wrt world)

src/multibody/model.hxx

@@ -177,6 +177,7 @@ namespace se3
     :model(ref)
     ,joints(0)
     ,a((std::size_t)ref.nbody)
+    ,a_gf((std::size_t)ref.nbody)
     ,v((std::size_t)ref.nbody)
     ,f((std::size_t)ref.nbody)
     ,oMi((std::size_t)ref.nbody)
@@ -225,6 +226,7 @@ namespace se3
     
     a[0].setZero();
     v[0].setZero();
+    a_gf[0] = -model.gravity;
     f[0].setZero();
     oMi[0].setIdentity();
     liMi[0].setIdentity();

src/python/data.hpp

@@ -70,6 +70,7 @@ namespace se3
 	cl
 
 	  .ADD_DATA_PROPERTY(std::vector<Motion>,a,"Body acceleration")
+    .ADD_DATA_PROPERTY(std::vector<Motion>,a_gf,"Body acceleration containing also the gravity acceleration")
 	  .ADD_DATA_PROPERTY(std::vector<Motion>,v,"Body velocity")
 	  .ADD_DATA_PROPERTY(std::vector<Force>,f,"Body force")
 	  .ADD_DATA_PROPERTY(std::vector<SE3>,oMi,"Body absolute placement (wrt world)")
@@ -106,6 +107,7 @@ namespace se3
       }
 
       IMPL_DATA_PROPERTY(std::vector<Motion>,a,"Body acceleration")
+      IMPL_DATA_PROPERTY(std::vector<Motion>,a_gf,"Body acceleration containing also the gravity acceleration")
       IMPL_DATA_PROPERTY(std::vector<Motion>,v,"Body velocity")
       IMPL_DATA_PROPERTY(std::vector<Force>,f,"Body force")
       IMPL_DATA_PROPERTY(std::vector<SE3>,oMi,"Body absolute placement (wrt world)")

src/simulation/compute-all-terms.hpp

@@ -81,10 +81,13 @@ namespace se3
 
       jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
 
-      data.a[i]  = jdata.c() + (data.v[i] ^ jdata.v());
-      data.a[i] += data.liMi[i].actInv(data.a[parent]);
+      data.a_gf[i] = data.a[i] = jdata.c() + (data.v[i] ^ jdata.v());
+      if (parent > 0)
+        data.a[i] += data.liMi[i].actInv(data.a[parent]);
+      
+      data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);
 
-      data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
+      data.f[i] = model.inertias[i]*data.a_gf[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
     }
 
   };
@@ -144,8 +147,9 @@ namespace se3
                   const Eigen::VectorXd & q,
                   const Eigen::VectorXd & v)
   {
-    data.v[0].setZero ();
-    data.a[0] = -model.gravity;
+    data.v[0].setZero();
+    data.a[0].setZero();
+    data.a_gf[0] = -model.gravity;
 
     for(Model::Index i=1;i<(Model::Index) model.nbody;++i)
     {