[All] Change oYo to oYcrb

src/algorithm/crba.hxx

@@ -292,8 +292,8 @@ namespace se3
 
       const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
       const Model::Index & parent = model.parents[i];
-      const Inertia & Y = data.oYo[i];
-      const Inertia::Matrix6 & doYo = data.doYo[i];
+      const Inertia & Y = data.oYcrb[i];
+      const Inertia::Matrix6 & doYcrb = data.doYcrb[i];
       
       ColsBlock J_cols = jmodel.jointCols(data.J);
       J_cols = data.oMi[i].act(jdata.S());
@@ -301,9 +301,9 @@ namespace se3
       ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
       motionSet::motionAction(data.ov[i],J_cols,dJ_cols);
       
-      data.oYo[parent] += Y;
+      data.oYcrb[parent] += Y;
       if(parent > 0)
-        data.doYo[parent] += doYo;
+        data.doYcrb[parent] += doYcrb;
       
       // Calc Ag
       ColsBlock Ag_cols = jmodel.jointCols(data.Ag);
@@ -312,7 +312,7 @@ namespace se3
       // Calc dAg = Ivx + vxI
       ColsBlock dAg_cols = jmodel.jointCols(data.dAg);
       rhsInertiaMult(Y,dJ_cols,dAg_cols);
-      dAg_cols += doYo * J_cols;
+      dAg_cols += doYcrb * J_cols;
     }
     
     template<typename Min, typename Mout>
@@ -359,12 +359,12 @@ namespace se3
     typedef Eigen::Block <Data::Matrix6x,3,-1> Block3x;
     
     forwardKinematics(model,data,q,v);
-    data.oYo[0].setZero();
+    data.oYcrb[0].setZero();
     for(Model::Index i=1;i<(Model::Index)(model.njoints);++i)
     {
-      data.oYo[i] = data.oMi[i].act(model.inertias[i]);
+      data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
       data.ov[i] = data.oMi[i].act(data.v[i]); // v_i expressed in the world frame
-      data.doYo[i] = data.oYo[i].variation(data.ov[i]);
+      data.doYcrb[i] = data.oYcrb[i].variation(data.ov[i]);
     }
     
     for(Model::Index i=(Model::Index)(model.njoints-1);i>0;--i)
@@ -372,7 +372,7 @@ namespace se3
       DCcrbaBackwardStep::run(model.joints[i],data.joints[i],
                               DCcrbaBackwardStep::ArgsType(model,data));
     }
-    data.com[0] = data.oYo[0].lever();
+    data.com[0] = data.oYcrb[0].lever();
     
     const Block3x Ag_lin = data.Ag.middleRows<3> (Force::LINEAR);
     Block3x Ag_ang = data.Ag.middleRows<3>  (Force::ANGULAR);
@@ -380,16 +380,16 @@ namespace se3
       Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
     
     data.hg = data.Ag*v;
-    data.vcom[0] = data.hg.linear()/data.oYo[0].mass();
+    data.vcom[0] = data.hg.linear()/data.oYcrb[0].mass();
     
     const Block3x dAg_lin = data.dAg.middleRows<3>(Force::LINEAR);
     Block3x dAg_ang = data.dAg.middleRows<3>(Force::ANGULAR);
     for (long i = 0; i<model.nv; ++i)
       dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]);
 
-    data.Ig.mass() = data.oYo[0].mass();
+    data.Ig.mass() = data.oYcrb[0].mass();
     data.Ig.lever().setZero();
-    data.Ig.inertia() = data.oYo[0].inertia();
+    data.Ig.inertia() = data.oYcrb[0].inertia();
     
     return data.dAg;
   }

src/algorithm/rnea-derivatives.hxx

@@ -53,7 +53,7 @@ namespace se3
       else
         data.oMi[i] = data.liMi[i];
       
-      data.oYo[i] = data.oMi[i].act(model.inertias[i]);
+      data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
       data.f[i].setZero();
       
       typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
@@ -92,22 +92,22 @@ namespace se3
       ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
       ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq);
       
-      motionSet::inertiaAction(data.oYo[i],dAdq_cols,dFdq_cols);
+      motionSet::inertiaAction(data.oYcrb[i],dAdq_cols,dFdq_cols);
       
       gravity_partial_dq.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
       = J_cols.transpose()*data.dFdq.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
       
-      data.f[i] = data.oYo[i] * oa;
+      data.f[i] = data.oYcrb[i] * oa;
       motionSet::act<ADDTO>(J_cols,data.f[i],dFdq_cols);
       
-      lhsInertiaMult(data.oYo[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv()));
+      lhsInertiaMult(data.oYcrb[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv()));
       for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
         gravity_partial_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dAdq.col(j);
       
       jmodel.jointVelocitySelector(data.g).noalias() = J_cols.transpose()*data.f[i].toVector();
       if(parent>0)
       {
-        data.oYo[parent] += data.oYo[i];
+        data.oYcrb[parent] += data.oYcrb[i];
       }
     }
     
@@ -191,12 +191,12 @@ namespace se3
         data.a[i] += data.liMi[i].actInv(data.a[parent]);
       }
       
-      data.oYo[i] = data.oMi[i].act(model.inertias[i]);
+      data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
       ov = data.oMi[i].act(data.v[i]);
       oa = data.oMi[i].act(data.a[i]) + data.oa[0]; // add gravity contribution
       
-      data.h[i] = data.oYo[i] * ov;
-      data.f[i] = data.oYo[i] * oa + ov.cross(data.h[i]);
+      data.h[i] = data.oYcrb[i] * ov;
+      data.f[i] = data.oYcrb[i] * oa + ov.cross(data.h[i]);
       
       typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
       ColsBlock J_cols = jmodel.jointCols(data.J);
@@ -219,9 +219,9 @@ namespace se3
         dVdq_cols.setZero();
 
       // computes variation of inertias
-      data.doYo[i] = data.oYo[i].variation(ov);
+      data.doYcrb[i] = data.oYcrb[i].variation(ov);
       
-      addForceCrossMatrix(data.h[i],data.doYo[i]);
+      addForceCrossMatrix(data.h[i],data.doYcrb[i]);
     }
     
     template<typename ForceDerived, typename M6>
@@ -274,21 +274,21 @@ namespace se3
       jmodel.jointVelocitySelector(data.tau).noalias() = J_cols.transpose()*data.f[i].toVector();
       
       // dtau/da similar to data.M
-      motionSet::inertiaAction(data.oYo[i],J_cols,dFda_cols);
+      motionSet::inertiaAction(data.oYcrb[i],J_cols,dFda_cols);
       rnea_partial_da.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
       = J_cols.transpose()*data.dFda.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
       
       // dtau/dv
-      motionSet::inertiaAction(data.oYo[i],dAdv_cols,dFdv_cols);
-      dFdv_cols += data.doYo[i] * J_cols;
+      motionSet::inertiaAction(data.oYcrb[i],dAdv_cols,dFdv_cols);
+      dFdv_cols += data.doYcrb[i] * J_cols;
       
       rnea_partial_dv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
       = J_cols.transpose()*data.dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
       
       // dtau/dq
-      motionSet::inertiaAction(data.oYo[i],dAdq_cols,dFdq_cols);
+      motionSet::inertiaAction(data.oYcrb[i],dAdq_cols,dFdq_cols);
       if(parent>0)
-        dFdq_cols += data.doYo[i] * dVdq_cols;
+        dFdq_cols += data.doYcrb[i] * dVdq_cols;
 
       rnea_partial_dq.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
       = J_cols.transpose()*data.dFdq.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
@@ -297,13 +297,13 @@ namespace se3
       
       if(parent > 0)
       {
-        lhsInertiaMult(data.oYo[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv()));
+        lhsInertiaMult(data.oYcrb[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv()));
         for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
           rnea_partial_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dAdq.col(j);
         for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
           rnea_partial_dv.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dAdv.col(j);
         
-        M6tmpR.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.doYo[i];
+        M6tmpR.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.doYcrb[i];
         for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
           rnea_partial_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j) += M6tmpR.topRows(jmodel.nv()) * data.dVdq.col(j);
         for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
@@ -312,8 +312,8 @@ namespace se3
       
       if(parent>0)
       {
-        data.oYo[parent] += data.oYo[i];
-        data.doYo[parent] += data.doYo[i];
+        data.oYcrb[parent] += data.oYcrb[i];
+        data.doYcrb[parent] += data.doYcrb[i];
         data.f[parent] += data.f[i];
       }
     }

src/algorithm/rnea.hxx

@@ -325,7 +325,7 @@ namespace se3
       else data.oMi[i] = data.liMi[i];
       
       // express quantities in the world frame
-      data.oYo[i] = data.oMi[i].act(model.inertias[i]);
+      data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
       
       data.v[i] = jdata.v();
       if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[parent]);
@@ -341,7 +341,7 @@ namespace se3
       motionSet::motionAction(data.ov[i],Jcols,dJcols);
 
       // computes vxI
-      Inertia::vxi(data.ov[i],data.oYo[i],data.vxI[i]);
+      Inertia::vxi(data.ov[i],data.oYcrb[i],data.vxI[i]);
     }
 
   };
@@ -367,13 +367,13 @@ namespace se3
       ColsBlock dJcols = jmodel.jointCols(data.dJ);
       ColsBlock Jcols = jmodel.jointCols(data.J);
       
-      motionSet::inertiaAction(data.oYo[i],dJcols,jmodel.jointCols(data.dFdv));
+      motionSet::inertiaAction(data.oYcrb[i],dJcols,jmodel.jointCols(data.dFdv));
       jmodel.jointCols(data.dFdv) += data.vxI[i] * Jcols;
 
       data.C.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
       = Jcols.transpose()*data.dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
       
-      lhsInertiaMult(data.oYo[i],Jcols.transpose(),M6tmpR.topRows(jmodel.nv()));
+      lhsInertiaMult(data.oYcrb[i],Jcols.transpose(),M6tmpR.topRows(jmodel.nv()));
       for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
         data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dJ.col(j);
 
@@ -383,7 +383,7 @@ namespace se3
 
       if(parent>0)
       {
-        data.oYo[parent] += data.oYo[i];
+        data.oYcrb[parent] += data.oYcrb[i];
         data.vxI[parent] += data.vxI[i];
       }
       

src/multibody/model.hpp

@@ -437,10 +437,10 @@ namespace se3
     container::aligned_vector<Inertia::Matrix6> Ivx;
     
     /// \brief Inertia quantities expressed in the world frame
-    container::aligned_vector<Inertia> oYo;
+    container::aligned_vector<Inertia> oYcrb;
     
     /// \brief Time variation of the inertia quantities expressed in the world frame
-    container::aligned_vector<Inertia::Matrix6> doYo;
+    container::aligned_vector<Inertia::Matrix6> doYcrb;
     
     /// \brief Temporary for derivative algorithms
     Inertia::Matrix6 Itmp;

src/multibody/model.hxx

@@ -256,8 +256,8 @@ namespace se3
     ,dFda(6,model.nv)
     ,vxI((std::size_t)model.njoints)
     ,Ivx((std::size_t)model.njoints)
-    ,oYo((std::size_t)model.njoints)
-    ,doYo((std::size_t)model.njoints)
+    ,oYcrb((std::size_t)model.njoints)
+    ,doYcrb((std::size_t)model.njoints)
     ,ddq(model.nv)
     ,Yaba((std::size_t)model.njoints)
     ,u(model.nv)

unittest/crba.cpp

@@ -210,7 +210,7 @@ BOOST_AUTO_TEST_CASE (test_ccrb)
     dccrba(model,data,q,0*v);
     BOOST_CHECK(data.dAg.isZero());
     
-    // Check that dYcrb is equal to doYo
+    // Check that dYcrb is equal to doYcrb
     {
       // Compute dYcrb
       Data data_ref(model), data_ref_plus(model), data(model);
@@ -229,7 +229,7 @@ BOOST_AUTO_TEST_CASE (test_ccrb)
       {
         Inertia::Matrix6 dYcrb = (data_ref_plus.oMi[i].act(data_ref_plus.Ycrb[i]).matrix() -
                     data_ref.oMi[i].act(data_ref.Ycrb[i]).matrix())/alpha;
-        BOOST_CHECK(data.doYo[i].isApprox(dYcrb,sqrt(alpha)));
+        BOOST_CHECK(data.doYcrb[i].isApprox(dYcrb,sqrt(alpha)));
       }
     }