Commit 64dcf1ff authored by Valenza Florian's avatar Valenza Florian
Browse files

[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
parent 4194d995
......@@ -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
......
......@@ -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
......
......@@ -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)
......
......@@ -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.
......
//
// 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__
......@@ -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)",
......
......@@ -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
......
//
// 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__
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment