[Minor] Update name in Data of oMof to oMf + in frames algo get directly the...

[Minor] Update name in Data of oMof to oMf + in frames algo get directly the value of the parent joint when the relative placement of the frame wrt parent joint is Identity

src/algorithm/frames.hpp

@@ -82,8 +82,16 @@ namespace se3
   {
     for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nFrames; ++i)
     {
-      const Model::JointIndex & parent = model.frames[i].parent;
-      data.oMof[i] = (data.oMi[parent] * model.frames[i].placement);
+      const Frame & frame = model.frames[i];
+      const Model::JointIndex & parent = frame.parent;
+      if (frame.placement == SE3::Identity())
+      {
+        data.oMf[i] = data.oMi[parent];
+      }
+      else
+      {
+        data.oMf[i] = (data.oMi[parent] * frame.placement);
+      }
     }
   }
   
@@ -108,7 +116,7 @@ namespace se3
     assert( J.cols() == data.J.cols() );
     
     const Model::JointIndex & parent = model.frames[frame_id].parent;
-    const SE3 & oMframe = data.oMof[frame_id];
+    const SE3 & oMframe = data.oMf[frame_id];
     const Frame & frame = model.frames[frame_id];
     
     const int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
@@ -117,8 +125,8 @@ namespace se3
     const SE3::Vector3 lever(data.oMi[parent].rotation() * (frame.placement.translation()));
     
     getJacobian<localFrame>(model, data, parent, J);
-    
-    if (frame.type == JOINT)
+
+    if (frame.placement == SE3::Identity())
     {
       // do nothing
     }

src/multibody/model.hpp

@@ -423,7 +423,7 @@ namespace se3
     Eigen::VectorXd nle;
 
     /// \brief Vector of absolute operationnel frame placements (wrt the world).
-    std::vector<SE3> oMof;
+    std::vector<SE3> oMf;
 
     /// \brief Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the subtree supported by the joint.
     std::vector<Inertia> Ycrb;

src/multibody/model.hxx

@@ -287,7 +287,7 @@ namespace se3
     ,liMi((std::size_t)ref.nbody)
     ,tau(ref.nv)
     ,nle(ref.nv)
-    ,oMof((std::size_t)ref.nFrames)
+    ,oMf((std::size_t)ref.nFrames)
     ,Ycrb((std::size_t)ref.nbody)
     ,M(ref.nv,ref.nv)
     ,ddq(ref.nv)

src/python/data.hpp

@@ -74,7 +74,7 @@ namespace se3
         .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)")
-        .ADD_DATA_PROPERTY(std::vector<SE3>,oMof,"frames absolute placement (wrt world)")
+        .ADD_DATA_PROPERTY(std::vector<SE3>,oMf,"frames absolute placement (wrt world)")
         .ADD_DATA_PROPERTY(std::vector<SE3>,liMi,"Body relative placement (wrt parent)")
         .ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,tau,"Joint forces")
         .ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,nle,"Non Linear Effects")
@@ -120,7 +120,7 @@ namespace se3
       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)")
-      IMPL_DATA_PROPERTY(std::vector<SE3>,oMof,"frames absolute placement (wrt world)")
+      IMPL_DATA_PROPERTY(std::vector<SE3>,oMf,"frames absolute placement (wrt world)")
       IMPL_DATA_PROPERTY(std::vector<SE3>,liMi,"Body relative placement (wrt parent)")
       IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,tau,"Joint forces")
       IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,nle,"Non Linear Effects")

unittest/frames.cpp

@@ -49,7 +49,7 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
   q.middleRows<4> (3).normalize();
   framesForwardKinematics(model, data, q);
 
-  BOOST_CHECK(data.oMof[model.getFrameId(frame_name)].isApprox(data.oMi[parent_idx]*framePlacement));
+  BOOST_CHECK(data.oMf[model.getFrameId(frame_name)].isApprox(data.oMi[parent_idx]*framePlacement));
 
 }