diff --git a/CMakeLists.txt b/CMakeLists.txt
index 93988c81df2ea454b1fd753585881cd5e1de95f4..00917e2003ad69622beb41ef159c59fc94b7d09b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -157,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
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index e5ad22eb491692f0fc6a02d88620be6419580d2d..4ffd5c8bd1e65e3ed07485343201548afbab2796 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -39,6 +39,26 @@ namespace se3
{
class Model;
class Data;
+ struct Frame
+ {
+ typedef std::size_t Index;
+
+ 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;
+ };
class Model
{
@@ -49,6 +69,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 nExtraFrames; // 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 +84,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> extra_frames;
+
Motion gravity; // Spatial gravity
static const Eigen::Vector3d gravity981; // Default 3D gravity (=(0,0,-9.81))
@@ -71,6 +94,7 @@ namespace se3
, nv(0)
, nbody(1)
, nFixBody(0)
+ , nExtraFrames(0)
, inertias(1)
, jointPlacements(1)
, joints(1)
@@ -107,6 +131,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 +164,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
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index be4aa1980dfea392d4cef02f9e789289de0a9544..b7255ab95085023ace1097bc3223a43a25077194 100644
--- a/src/multibody/model.hxx
+++ b/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( extra_frames.begin()
+ , extra_frames.end()
+ , boost::bind(&Frame::name, _1) == name
+ );
+ return Model::Index(it - extra_frames.begin());
+ }
+
+ inline bool Model::existFrame ( const std::string & name ) const
+ {
+ return std::find_if( extra_frames.begin(), extra_frames.end(), boost::bind(&Frame::name, _1) == name) != extra_frames.end();
+ }
+
+ inline const std::string & Model::getFrameName ( Index index ) const
+ {
+ return extra_frames[index].name;
+ }
+
+ inline const Model::Index& Model::getFrameParent( const std::string & name ) const
+ {
+ std::vector<Frame>::const_iterator it = std::find_if( extra_frames.begin()
+ , extra_frames.end()
+ , boost::bind(&Frame::name, _1) == name
+ );
+ return it->parent_id;
+ }
+
+ inline const Model::Index& Model::getFrameParent( Model::Index index ) const
+ {
+ return extra_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( extra_frames.begin()
+ , extra_frames.end()
+ , boost::bind(&Frame::name, _1) == name
+ );
+ return it->frame_placement;
+ }
+
+ inline const SE3 & Model::getJointToFrameTransform( Model::Index index ) const
+ {
+ return extra_frames[index].frame_placement;
+ }
+
+ inline void Model::addFrame ( const Frame & frame )
+ {
+ if( !existFrame(frame.name) )
+ extra_frames.push_back(frame);nExtraFrames++;
+ }
+
+ 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)
diff --git a/src/multibody/parser/urdf.hxx b/src/multibody/parser/urdf.hxx
index ebf258f2c8bb05057b6d439cae06f2b9383b90e1..2cbdd1bd1c5e6595717d5a00baa63ab9ead9004a 100644
--- a/src/multibody/parser/urdf.hxx
+++ b/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.
diff --git a/src/python/frame.hpp b/src/python/frame.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a76df1697a845902b3fbe20196961cb5e877634c
--- /dev/null
+++ b/src/python/frame.hpp
@@ -0,0 +1,83 @@
+//
+// 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__
+
diff --git a/src/python/model.hpp b/src/python/model.hpp
index 96162c6badede77df3e79878727da537d8775d20..6caba7c4bc47c067f273361234fe8621363ca5e3 100644
--- a/src/python/model.hpp
+++ b/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("extra_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->extra_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)",
diff --git a/src/python/python.cpp b/src/python/python.cpp
index eddca903957f9c6fcdbc6d933f3c858721a7c6bc..36b4a99b3f0cc8f6ed6afa79b91006deb5a76625 100644
--- a/src/python/python.cpp
+++ b/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