[C++][Python][parser] Added operational_frames in Model as a vector of Frame...

[C++][Python][parser] Added operational_frames in Model as a vector of Frame {name, parent id, placementWrtParentJoint} and exposed it in python. When parsing a fixed joint, a frame is added to the model with the name of the fixed joint

CMakeLists.txt

@@ -95,6 +95,7 @@ SET(${PROJECT_NAME}_SPATIAL_HEADERS
   spatial/skew.hpp
   spatial/act-on-set.hpp
   spatial/explog.hpp
+  spatial/frame.hpp
   )
 
 SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
@@ -156,6 +157,7 @@ SET(${PROJECT_NAME}_PYTHON_HEADERS
   python/joint-dense.hpp
   python/joints-models.hpp
   python/joints-variant.hpp
+  python/frame.hpp
   python/model.hpp
   python/data.hpp
   python/algorithms.hpp

src/multibody/model.hpp

@@ -25,6 +25,7 @@
 #include "pinocchio/spatial/force.hpp"
 #include "pinocchio/spatial/motion.hpp"
 #include "pinocchio/spatial/inertia.hpp"
+#include "pinocchio/spatial/frame.hpp"
 #include "pinocchio/multibody/joint/joint-variant.hpp"
 #include <iostream>
 
@@ -34,11 +35,13 @@ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Force)
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Motion)
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,Eigen::Dynamic>)
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3::Vector3)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Frame)
 
 namespace se3
 {
   class Model;
   class Data;
+  
 
   class Model
   {
@@ -49,6 +52,7 @@ namespace se3
     int nv;                               // Dimension of the velocity vector space
     int nbody;                            // Number of bodies (= number of joints + 1)
     int nFixBody;                         // Number of fixed-bodies (= number of fixed-joints)
+    int nOperationalFrames;                     // Number of extra frames
 
     std::vector<Inertia> inertias;        // Spatial inertias of the body <i> in the supporting joint frame <i>
     std::vector<SE3> jointPlacements;     // Placement (SE3) of the input of joint <i> in parent joint output <li>
@@ -63,6 +67,8 @@ namespace se3
     std::vector<bool> fix_hasVisual;      // True iff fixed-body <i> has a visual mesh.
     std::vector<std::string> fix_bodyNames;// Name of fixed-joint <i>
 
+    std::vector<Frame> operational_frames;
+
     Motion gravity;                       // Spatial gravity
     static const Eigen::Vector3d gravity981; // Default 3D gravity (=(0,0,-9.81))
 
@@ -71,6 +77,7 @@ namespace se3
       , nv(0)
       , nbody(1)
       , nFixBody(0)
+      , nOperationalFrames(0)
       , inertias(1)
       , jointPlacements(1)
       , joints(1)
@@ -107,6 +114,18 @@ namespace se3
     Index getJointId( const std::string & name ) const;
     bool existJointName( const std::string & name ) const;
     const std::string& getJointName( Index index ) const;
+
+    Index getFrameId ( const std::string & name ) const;
+    bool existFrame ( const std::string & name ) const;
+    const std::string & getFrameName ( Index index ) const;
+    const Index& getFrameParent( const std::string & name ) const;
+    const Index& getFrameParent( Index index ) const;
+    const SE3 & getJointToFrameTransform( const std::string & name ) const;
+    const SE3 & getJointToFrameTransform( Index index ) const;
+
+    void addFrame ( const Frame & frame );
+    void addFrame ( const std::string & name, Index index, const SE3 & placement );
+
   };
 
   class Data
@@ -128,6 +147,8 @@ namespace se3
     Eigen::VectorXd tau;                  // Joint forces
     Eigen::VectorXd nle;                  // Non linear effects
 
+    std::vector<SE3> oMef;                // Absolute position of extra frames
+
     std::vector<Inertia> Ycrb;            // Inertia of the sub-tree composit rigid body
     Eigen::MatrixXd M;                    // Joint Inertia
 

src/multibody/model.hxx

@@ -27,6 +27,8 @@
 #include "pinocchio/multibody/joint/joint-variant.hpp"
 #include <iostream>
 
+#include <boost/bind.hpp>
+
 namespace se3
 {
   inline std::ostream& operator<< (std::ostream & os, const Model & model)
@@ -173,6 +175,67 @@ namespace se3
     return names[index];
   }
 
+  inline Model::Index Model::getFrameId ( const std::string & name ) const
+  {
+    std::vector<Frame>::const_iterator it = std::find_if( operational_frames.begin()
+                                                        , operational_frames.end()
+                                                        , boost::bind(&Frame::name, _1) == name
+                                                        );
+    return Model::Index(it - operational_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();
+  }
+
+  inline const std::string & Model::getFrameName ( Index index ) const
+  {
+    return operational_frames[index].name;
+  }
+
+  inline const Model::Index& Model::getFrameParent( const std::string & name ) const
+  {
+    std::vector<Frame>::const_iterator it = std::find_if( operational_frames.begin()
+                                                        , operational_frames.end()
+                                                        , boost::bind(&Frame::name, _1) == name
+                                                        );
+    return it->parent_id;
+  }
+
+  inline const Model::Index& Model::getFrameParent( Model::Index index ) const
+  {
+    return operational_frames[index].parent_id;
+  }
+
+  inline const SE3 & Model::getJointToFrameTransform( 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()
+                                                        , boost::bind(&Frame::name, _1) == name
+                                                        );
+    return it->frame_placement;
+  }
+
+  inline const SE3 & Model::getJointToFrameTransform( Model::Index index ) const
+  {
+    return operational_frames[index].frame_placement;
+  }
+
+  inline void Model::addFrame ( const Frame & frame )
+  {
+    if( !existFrame(frame.name) )
+      operational_frames.push_back(frame);nOperationalFrames++;
+  }
+
+  inline void Model::addFrame ( const std::string & name, Index index, const SE3 & placement)
+  {
+    if( !existFrame(name) )
+      addFrame(Frame(name, index, placement));
+  }
+
+
   inline Data::Data (const Model & ref)
     :model(ref)
     ,joints(0)

src/multibody/parser/urdf.hxx

@@ -345,6 +345,9 @@ namespace se3
           
           //add the fixed Body in the model for the viewer
           model.addFixedBody(parent_joint_id, nextPlacementOffset, link->name, has_visual);
+
+          // Add a frame in the model to keep trace of this fixed joint
+          model.addFrame(joint->name, parent_joint_id, nextPlacementOffset);
           
           //for the children of the current link, set their parent to be
           //the the parent of the current link.

src/python/frame.hpp

+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_python_frame_hpp__
+#define __se3_python_frame_hpp__
+
+#include <eigenpy/exception.hpp>
+#include <eigenpy/eigenpy.hpp>
+#include "pinocchio/multibody/model.hpp"
+
+
+namespace se3
+{
+  namespace python
+  {
+    namespace bp = boost::python;
+
+    struct FramePythonVisitor
+      : public boost::python::def_visitor< FramePythonVisitor >
+    {
+      typedef eigenpy::UnalignedEquivalent<SE3>::type SE3_fx;
+      typedef Model::Index Index;
+
+    public:
+
+      static PyObject* convert(Frame const& f)
+      {
+        return boost::python::incref(boost::python::object(f).ptr());
+      }
+
+      template<class PyClass>
+      void visit(PyClass& cl) const 
+      {
+        cl
+          .def(bp::init<const std::string&,Index, const SE3_fx&> ((bp::arg("name"),bp::arg("parent id"), bp::arg("placement")),
+                "Initialize from name, parent id and placement wrt parent joint."))
+
+          .def("name", &FramePythonVisitor::getName)
+          .def("parent_id", &FramePythonVisitor::getParentId)
+          .def("placement", &FramePythonVisitor::getPlcaementWrtParentJoint)
+          ;
+      }
+
+
+      static std::string getName( const Frame & self) { return self.name; }
+      static Index getParentId( const Frame & self) { return self.parent_id; }
+      static SE3_fx getPlcaementWrtParentJoint( const Frame & self) { return self.frame_placement; }
+
+      static void expose()
+      {
+        bp::class_<Frame>("Frame",
+                           "ExtraFrames.\n\n",
+	                         bp::no_init
+                         )
+	                       .def(FramePythonVisitor())
+	                       ;
+    
+        bp::to_python_converter< Frame,FramePythonVisitor >();
+      }
+
+
+    };
+    
+
+  } // namespace python
+} // namespace se3
+
+#endif // ifndef __se3_python_frame_hpp__
+

src/python/model.hpp

@@ -132,6 +132,10 @@ namespace se3
       .add_property("fix_hasVisual", bp::make_function(&ModelPythonVisitor::fix_hasVisual, bp::return_internal_reference<>())  )
       .add_property("fix_bodyNames", bp::make_function(&ModelPythonVisitor::fix_bodyNames, bp::return_internal_reference<>())  )
 
+      .def("getFrameParentJoint", &ModelPythonVisitor::getFrameParent)
+      .def("getFramePositionInParent", &ModelPythonVisitor::getJointToFrameTransform)
+      .def("addExtraFrame", &ModelPythonVisitor::addExtraFrame)
+      .add_property("operational_frames", bp::make_function(&ModelPythonVisitor::extraFrames, bp::return_internal_reference<>()) )
 
       .add_property("gravity",&ModelPythonVisitor::gravity,&ModelPythonVisitor::setGravity)
 	  .def("BuildEmptyModel",&ModelPythonVisitor::maker_empty)
@@ -175,6 +179,14 @@ namespace se3
       static std::vector<bool> & fix_hasVisual ( ModelHandler & m ) { return m->fix_hasVisual; }
       static std::vector<std::string> & fix_bodyNames ( ModelHandler & m ) { return m->fix_bodyNames; }
 
+      static Model::Index  getFrameParent( ModelHandler & m, const std::string & name ) { return m->getFrameParent(name); }
+      static SE3  getJointToFrameTransform( ModelHandler & m, const std::string & name ) { return m->getJointToFrameTransform(name); }
+      static void  addExtraFrame( ModelHandler & m, const std::string & frameName, Index parent, const SE3_fx & placementWrtParent )
+      {
+        m->addFrame(frameName, parent, placementWrtParent);
+      }
+      static std::vector<Frame> & extraFrames (ModelHandler & m ) { return m->operational_frames;}
+
       static Motion gravity( ModelHandler & m ) { return m->gravity; }
       static void setGravity( ModelHandler & m,const Motion_fx & g ) { m->gravity = g; }
 
@@ -232,6 +244,8 @@ namespace se3
           .def(bp::vector_indexing_suite< std::vector<double> >());
         bp::class_< JointModelVector >("StdVec_JointModelVector")
           .def(bp::vector_indexing_suite< JointModelVector, true >());
+        bp::class_< std::vector<Frame> >("StdVec_FrameVector")
+          .def(bp::vector_indexing_suite< std::vector<Frame> >());
 
         bp::class_<ModelHandler>("Model",
                                  "Articulated rigid body model (const)",

src/python/python.cpp

@@ -24,6 +24,7 @@
 #include "pinocchio/python/joint-dense.hpp"
 #include "pinocchio/python/joints-variant.hpp"
 
+#include "pinocchio/python/frame.hpp"
 #include "pinocchio/python/model.hpp"
 #include "pinocchio/python/data.hpp"
 #include "pinocchio/python/algorithms.hpp"
@@ -65,6 +66,7 @@ namespace se3
     }
     void exposeModel()
     {
+      FramePythonVisitor::expose();
       ModelPythonVisitor::expose();
       DataPythonVisitor::expose();
 #ifdef WITH_HPP_FCL      

src/spatial/frame.hpp

+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_frame_hpp__
+#define __se3_frame_hpp__
+
+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/spatial/force.hpp"
+#include "pinocchio/spatial/motion.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+#include <iostream>
+
+namespace se3
+{
+struct Frame
+{
+  typedef std::size_t Index;
+  
+  Frame() : name(), parent_id(), frame_placement()
+  {
+
+  }
+
+  Frame(const std::string & name, Index parent, const SE3 & placement): name(name)
+                                                                      , parent_id(parent)
+                                                                      , frame_placement(placement)
+  {
+  }
+
+  bool operator == (const Frame & other) const
+  {
+    return name == other.name && parent_id == other.parent_id
+            && frame_placement == other.frame_placement ;
+  }
+
+  std::string name;
+  Index parent_id;
+  SE3 frame_placement;
+};
+
+}
+#endif // ifndef __se3_frame_hpp__