Renamed operational_frames to frames in Model

CMakeLists.txt

@@ -159,7 +159,7 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
   algorithm/center-of-mass.hxx
   algorithm/joint-configuration.hpp
   algorithm/energy.hpp
-  algorithm/operational-frames.hpp
+  algorithm/frames.hpp
   algorithm/compute-all-terms.hpp
   )
 

python/bindings_frame.py

@@ -24,19 +24,19 @@ class TestFrameBindings(unittest.TestCase):
     robot = RobotWrapper(romeo_model_path, hint_list, se3.JointModelFreeFlyer())
 
     def test_type_get_set(self):
-        f = self.robot.model.operational_frames[1]
+        f = self.robot.model.frames[1]
         self.assertTrue(f.type == se3.FrameType.JOINT)
         f.type = se3.FrameType.BODY
         self.assertTrue(f.type == se3.FrameType.BODY)
 
     def test_name_get_set(self):
-        f = self.robot.model.operational_frames[1]
+        f = self.robot.model.frames[1]
         self.assertTrue(f.name == 'LHipYaw')
         f.name = 'new_hip_frame'
         self.assertTrue(f.name == 'new_hip_frame')
 
     def test_parent_get_set(self):
-        f = self.robot.model.operational_frames[1]
+        f = self.robot.model.frames[1]
         self.assertTrue(f.parent == 2)
         f.parent = 5
         self.assertTrue(f.parent == 5)
@@ -44,7 +44,7 @@ class TestFrameBindings(unittest.TestCase):
     def test_placement_get_set(self):
         m = se3.SE3(self.m3ones, np.array([0,0,0],np.double))
         new_m = se3.SE3(rand([3,3]), rand(3))
-        f = self.robot.model.operational_frames[1]
+        f = self.robot.model.frames[1]
         self.assertTrue(np.allclose(f.placement.homogeneous, m.homogeneous))
         f.placement = new_m
         self.assertTrue(np.allclose(f.placement.homogeneous, new_m.homogeneous))

src/algorithm/operational-frames.hpp → src/algorithm/frames.hpp

@@ -15,8 +15,8 @@
 // Pinocchio If not, see
 // <http://www.gnu.org/licenses/>.
 
-#ifndef __se3_operational_frames_hpp__
-#define __se3_operational_frames_hpp__
+#ifndef __se3_frames_hpp__
+#define __se3_frames_hpp__
 
 #include "pinocchio/multibody/model.hpp"
 
@@ -80,10 +80,10 @@ namespace se3
                                       Data & data
                                       )
   {
-    for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nOperationalFrames; ++i)
+    for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nFrames; ++i)
     {
-      const Model::JointIndex & parent = model.operational_frames[i].parent;
-      data.oMof[i] = (data.oMi[parent] * model.operational_frames[i].placement);
+      const Model::JointIndex & parent = model.frames[i].parent;
+      data.oMof[i] = (data.oMi[parent] * model.frames[i].placement);
     }
   }
   
@@ -107,9 +107,9 @@ namespace se3
     assert( J.rows() == data.J.rows() );
     assert( J.cols() == data.J.cols() );
     
-    const Model::JointIndex & parent = model.operational_frames[frame_id].parent;
+    const Model::JointIndex & parent = model.frames[frame_id].parent;
     const SE3 & oMframe = data.oMof[frame_id];
-    const Frame & frame = model.operational_frames[frame_id];
+    const Frame & frame = model.frames[frame_id];
     
     const int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
     
@@ -117,16 +117,24 @@ namespace se3
     const SE3::Vector3 lever(data.oMi[parent].rotation() * (frame.placement.translation()));
     
     getJacobian<localFrame>(model, data, parent, J);
-    for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
+    
+    if (frame.type == JOINT)
+    {
+      // do nothing
+    }
+    else
     {
-      if(! localFrame )
-        J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
-      else
-        J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
+      for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
+      {
+        if(! localFrame )
+          J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
+        else
+          J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
+      }
     }
   }
 
 } // namespace se3
 
-#endif // ifndef __se3_operational_frames_hpp__
+#endif // ifndef __se3_frames_hpp__
 

src/multibody/model.hpp

@@ -67,7 +67,7 @@ namespace se3
     
     
     /// \brief Number of operational frames.
-    int nOperationalFrames;
+    int nFrames;
 
     /// \brief Spatial inertias of the body <i> expressed in the supporting joint frame <i>.
     std::vector<Inertia> inertias;
@@ -95,7 +95,7 @@ namespace se3
     Eigen::VectorXd upperPositionLimit;
 
     /// \brief Vector of operational frames registered on the model.
-    std::vector<Frame> operational_frames;
+    std::vector<Frame> frames;
 
     /// \brief Spatial gravity
     Motion gravity;
@@ -108,7 +108,7 @@ namespace se3
       , nv(0)
       , njoint(1)
       , nbody(1)
-      , nOperationalFrames(0)
+      , nFrames(0)
       , inertias(1)
       , jointPlacements(1)
       , joints(1)

src/multibody/model.hxx

@@ -189,80 +189,80 @@ namespace se3
 
   inline Model::FrameIndex Model::getFrameId ( const std::string & name ) const
   {
-    std::vector<Frame>::const_iterator it = std::find_if( operational_frames.begin()
-                                                        , operational_frames.end()
+    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
+                                                        , frames.end()
                                                         , boost::bind(&Frame::name, _1) == name
                                                         );
-    return Model::FrameIndex(it - operational_frames.begin());
+    return Model::FrameIndex(it - frames.begin());
   }
 
   inline bool Model::existFrame ( const std::string & name ) const
   {
-    return std::find_if( operational_frames.begin(), operational_frames.end(), boost::bind(&Frame::name, _1) == name) != operational_frames.end();
+    return std::find_if( frames.begin(), frames.end(), boost::bind(&Frame::name, _1) == name) != frames.end();
   }
 
   inline const std::string & Model::getFrameName ( const FrameIndex index ) const
   {
-    return operational_frames[index].name;
+    return frames[index].name;
   }
 
   inline Model::JointIndex Model::getFrameParent( const std::string & name ) const
   {
     assert(existFrame(name) && "The Frame you requested does not exist");
-    std::vector<Frame>::const_iterator it = std::find_if( operational_frames.begin()
-                                                        , operational_frames.end()
+    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
+                                                        , frames.end()
                                                         , boost::bind(&Frame::name, _1) == name
                                                         );
     
-    std::vector<Frame>::iterator::difference_type it_diff = it - operational_frames.begin();
+    std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
     return getFrameParent(Model::JointIndex(it_diff));
   }
 
   inline Model::JointIndex Model::getFrameParent( const FrameIndex index ) const
   {
-    return operational_frames[index].parent;
+    return frames[index].parent;
   }
 
   inline FrameType Model::getFrameType( const std::string & name ) const
   {
     assert(existFrame(name) && "The Frame you requested does not exist");
-    std::vector<Frame>::const_iterator it = std::find_if( operational_frames.begin()
-                                                        , operational_frames.end()
+    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
+                                                        , frames.end()
                                                         , boost::bind(&Frame::name, _1) == name
                                                         );
     
-    std::vector<Frame>::iterator::difference_type it_diff = it - operational_frames.begin();
+    std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
     return getFrameType(Model::JointIndex(it_diff));
   }
 
   inline FrameType Model::getFrameType( const FrameIndex index ) const
   {
-    return operational_frames[index].type;
+    return frames[index].type;
   }
 
   inline const SE3 & Model::getFramePlacement( const std::string & name) const
   {
     assert(existFrame(name) && "The Frame you requested does not exist");
-    std::vector<Frame>::const_iterator it = std::find_if( operational_frames.begin()
-                                                        , operational_frames.end()
+    std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
+                                                        , frames.end()
                                                         , boost::bind(&Frame::name, _1) == name
                                                         );
     
-    std::vector<Frame>::iterator::difference_type it_diff = it - operational_frames.begin();
+    std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
     return getFramePlacement(Model::Index(it_diff));
   }
 
   inline const SE3 & Model::getFramePlacement( const FrameIndex index ) const
   {
-    return operational_frames[index].placement;
+    return frames[index].placement;
   }
 
   inline bool Model::addFrame ( const Frame & frame )
   {
     if( !existFrame(frame.name) )
     {
-      operational_frames.push_back(frame);
-      nOperationalFrames++;
+      frames.push_back(frame);
+      nFrames++;
       return true;
     }
     else
@@ -291,7 +291,7 @@ namespace se3
     ,liMi((std::size_t)ref.nbody)
     ,tau(ref.nv)
     ,nle(ref.nv)
-    ,oMof((std::size_t)ref.nOperationalFrames)
+    ,oMof((std::size_t)ref.nFrames)
     ,Ycrb((std::size_t)ref.nbody)
     ,M(ref.nv,ref.nv)
     ,ddq(ref.nv)

src/python/algorithms.hpp

@@ -30,7 +30,7 @@
 #include "pinocchio/algorithm/dynamics.hpp"
 #include "pinocchio/algorithm/kinematics.hpp"
 #include "pinocchio/algorithm/jacobian.hpp"
-#include "pinocchio/algorithm/operational-frames.hpp"
+#include "pinocchio/algorithm/frames.hpp"
 #include "pinocchio/algorithm/center-of-mass.hpp"
 #include "pinocchio/algorithm/energy.hpp"
 #include "pinocchio/algorithm/joint-configuration.hpp"

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,"operational_frames absolute placement (wrt world)")
+        .ADD_DATA_PROPERTY(std::vector<SE3>,oMof,"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,"operational_frames absolute placement (wrt world)")
+      IMPL_DATA_PROPERTY(std::vector<SE3>,oMof,"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")

src/python/model.hpp

@@ -130,7 +130,7 @@ namespace se3
           .def("getFrameParent", &ModelPythonVisitor::getFrameParent)
           .def("getFramePlacement", &ModelPythonVisitor::getFramePlacement)
           .def("addFrame", &ModelPythonVisitor::addFrame)
-          .add_property("operational_frames", bp::make_function(&ModelPythonVisitor::operationalFrames, bp::return_internal_reference<>()) )
+          .add_property("frames", bp::make_function(&ModelPythonVisitor::operationalFrames, bp::return_internal_reference<>()) )
 
           .add_property("gravity",&ModelPythonVisitor::gravity,&ModelPythonVisitor::setGravity)
           .def("BuildEmptyModel",&ModelPythonVisitor::maker_empty)
@@ -180,7 +180,7 @@ namespace se3
       {
         m->addFrame(frameName, parent, placementWrtParent);
       }
-      static std::vector<Frame> & operationalFrames (ModelHandler & m ) { return m->operational_frames;}
+      static std::vector<Frame> & operationalFrames (ModelHandler & m ) { return m->frames;}
 
       static Motion gravity( ModelHandler & m ) { return m->gravity; }
       static void setGravity( ModelHandler & m,const Motion_fx & g ) { m->gravity = g; }

unittest/CMakeLists.txt

@@ -88,6 +88,6 @@ ADD_UNIT_TEST(constraint eigen3)
 ADD_UNIT_TEST(joints eigen3)
 ADD_UNIT_TEST(compute-all-terms eigen3)
 ADD_UNIT_TEST(energy eigen3)
-ADD_UNIT_TEST(operational-frames eigen3)
+ADD_UNIT_TEST(frames eigen3)
 ADD_UNIT_TEST(joint-configurations eigen3)
 ADD_UNIT_TEST(joint-accessor eigen3)
\ No newline at end of file

unittest/operational-frames.cpp → unittest/frames.cpp

@@ -17,7 +17,7 @@
 
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/algorithm/jacobian.hpp"
-#include "pinocchio/algorithm/operational-frames.hpp"
+#include "pinocchio/algorithm/frames.hpp"
 #include "pinocchio/algorithm/rnea.hpp"
 #include "pinocchio/spatial/act-on-set.hpp"
 #include "pinocchio/multibody/parser/sample-models.hpp"
@@ -26,11 +26,11 @@
 #include <iostream>
 
 #define BOOST_TEST_DYN_LINK
-#define BOOST_TEST_MODULE OperationalFramesTest
+#define BOOST_TEST_MODULE FramesTest
 #include <boost/test/unit_test.hpp>
 #include <boost/utility/binary.hpp>
 
-BOOST_AUTO_TEST_SUITE ( OperationalFramesTest)
+BOOST_AUTO_TEST_SUITE ( FramesTest)
 
 BOOST_AUTO_TEST_CASE ( test_kinematics )
 {

unittest/joint-accessor.cpp

@@ -17,7 +17,7 @@
 
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/algorithm/jacobian.hpp"
-#include "pinocchio/algorithm/operational-frames.hpp"
+#include "pinocchio/algorithm/frames.hpp"
 #include "pinocchio/algorithm/rnea.hpp"
 #include "pinocchio/algorithm/joint-configuration.hpp"
 #include "pinocchio/algorithm/compute-all-terms.hpp"