diff --git a/CMakeLists.txt b/CMakeLists.txt
index b3798ed0d515edcf81fdef8c037986fed2aa7ec7..6421351d8c39bc360c77a26ce3f3e92e77331483 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -119,6 +119,7 @@ SET(${PROJECT_NAME}_MULTIBODY_HEADERS
multibody/force-set.hpp
multibody/joint.hpp
multibody/model.hpp
+ multibody/model.hxx
multibody/visitor.hpp
)
@@ -173,6 +174,7 @@ IF(URDFDOM_FOUND)
SET(${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
${${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS}
multibody/parser/urdf.hpp
+ multibody/parser/urdf.hxx
)
LIST(APPEND HEADERS
${${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS}
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index a2ace9e4d5120319736c251836c62a91c7affc1c..de7d12a8aecf20589d0bb66f97ee7eb80cc0684d 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -162,251 +162,11 @@ namespace se3
};
-
-
} // namespace se3
-
-
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
-namespace se3
-{
- inline std::ostream& operator<< ( std::ostream & os, const Model& model )
- {
- os << "Nb bodies = " << model.nbody << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
- for(Model::Index i=0;i<(Model::Index)(model.nbody);++i)
- {
- os << " Joint "<<model.names[i] << ": parent=" << model.parents[i]
- << ( model.hasVisual[i] ? " (has visual) " : "(doesnt have visual)" ) << std::endl;
- }
-
- return os;
- }
-
- inline std::string random(const int len)
- {
- std::string res;
- static const char alphanum[] =
- "0123456789"
- "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
- "abcdefghijklmnopqrstuvwxyz";
-
- for (int i=0; i<len;++i)
- res += alphanum[((size_t)std::rand() % (sizeof(alphanum) - 1))];
- return res;
-}
-
- template<typename D>
- Model::Index Model::addBody( Index parent,const JointModelBase<D> & j,const SE3 & placement,
- const Inertia & Y,const std::string & jointName,
- const std::string & bodyName, bool visual)
- {
- assert( (nbody==(int)joints.size())&&(nbody==(int)inertias.size())
- &&(nbody==(int)parents.size())&&(nbody==(int)jointPlacements.size()) );
- assert( (j.nq()>=0)&&(j.nv()>=0) );
-
- Model::Index idx = (Model::Index) (nbody ++);
-
- joints .push_back(j.derived());
- boost::get<D>(joints.back()).setIndexes((int)idx,nq,nv);
-
- inertias .push_back(Y);
- parents .push_back(parent);
- jointPlacements.push_back(placement);
- names .push_back( (jointName!="")?jointName:random(8) );
- hasVisual .push_back(visual);
- bodyNames .push_back( (bodyName!="")?bodyName:random(8));
- nq += j.nq();
- nv += j.nv();
- return idx;
- }
-
- template<typename D>
- Model::Index Model::addBody( Index parent,const JointModelBase<D> & j,const SE3 & placement,
- const Inertia & Y,
- const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
- const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
- const std::string & jointName,
- const std::string & bodyName, bool visual)
- {
- assert( (nbody==(int)joints.size())&&(nbody==(int)inertias.size())
- &&(nbody==(int)parents.size())&&(nbody==(int)jointPlacements.size()) );
- assert( (j.nq()>=0)&&(j.nv()>=0) );
-
- Model::Index idx = (Model::Index) (nbody ++);
-
- joints .push_back(j.derived());
- boost::get<D>(joints.back()).setIndexes((int)idx,nq,nv);
-
- boost::get<D>(joints.back()).setMaxEffortLimit(effort);
- boost::get<D>(joints.back()).setMaxVelocityLimit(velocity);
-
- boost::get<D>(joints.back()).setLowerPositionLimit(lowPos);
- boost::get<D>(joints.back()).setUpperPositionLimit(upPos);
-
- inertias .push_back(Y);
- parents .push_back(parent);
- jointPlacements.push_back(placement);
- names .push_back( (jointName!="")?jointName:random(8) );
- hasVisual .push_back(visual);
- bodyNames .push_back( (bodyName!="")?bodyName:random(8));
- nq += j.nq();
- nv += j.nv();
- return idx;
- }
-
- inline Model::Index Model::addFixedBody( Index lastMovingParent,
- const SE3 & placementFromLastMoving,
- const std::string & bodyName,
- bool visual )
- {
-
- Model::Index idx = (Model::Index) (nFixBody++);
- fix_lastMovingParent.push_back(lastMovingParent);
- fix_lmpMi .push_back(placementFromLastMoving);
- fix_hasVisual .push_back(visual);
- fix_bodyNames .push_back( (bodyName!="")?bodyName:random(8));
- return idx;
- }
-
- inline void Model::mergeFixedBody(Index parent, const SE3 & placement, const Inertia & Y)
- {
- const Inertia & iYf = Y.se3Action(placement); //TODO
- inertias[parent] += iYf;
- }
-
- inline Model::Index Model::getBodyId( const std::string & name ) const
- {
- std::vector<std::string>::iterator::difference_type
- res = std::find(bodyNames.begin(),bodyNames.end(),name) - bodyNames.begin();
- assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
- assert( (res>=0)&&(res<nbody)&&"The body name you asked do not exist" );
- return Model::Index(res);
- }
- inline bool Model::existBodyName( const std::string & name ) const
- {
- return (bodyNames.end() != std::find(bodyNames.begin(),bodyNames.end(),name));
- }
-
- inline const std::string& Model::getBodyName( Model::Index index ) const
- {
- assert( index < (Model::Index)nbody );
- return bodyNames[index];
- }
-
- inline Model::Index Model::getJointId( const std::string & name ) const
- {
- std::vector<std::string>::iterator::difference_type
- res = std::find(names.begin(),names.end(),name) - names.begin();
- assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
- assert( (res>=0)&&(res<joints.size())&&"The joint name you asked do not exist" );
- return Model::Index(res);
- }
- inline bool Model::existJointName( const std::string & name ) const
- {
- return (names.end() != std::find(names.begin(),names.end(),name));
- }
-
- inline const std::string& Model::getJointName( Model::Index index ) const
- {
- assert( index < (Model::Index)joints.size() );
- return names[index];
- }
-
- inline Data::Data( const Model& ref )
- :model(ref)
- ,joints(0)
- ,a((std::size_t)ref.nbody)
- ,v((std::size_t)ref.nbody)
- ,f((std::size_t)ref.nbody)
- ,oMi((std::size_t)ref.nbody)
- ,liMi((std::size_t)ref.nbody)
- ,tau(ref.nv)
- ,nle(ref.nv)
- ,Ycrb((std::size_t)ref.nbody)
- ,M(ref.nv,ref.nv)
- ,Fcrb((std::size_t)ref.nbody)
- ,lastChild((std::size_t)ref.nbody)
- ,nvSubtree((std::size_t)ref.nbody)
- ,U(ref.nv,ref.nv)
- ,D(ref.nv)
- ,tmp(ref.nv)
- ,parents_fromRow((std::size_t)ref.nv)
- ,nvSubtree_fromRow((std::size_t)ref.nv)
- ,J(6,ref.nv)
- ,iMf((std::size_t)ref.nbody)
- ,com((std::size_t)ref.nbody)
- ,vcom((std::size_t)ref.nbody)
- ,acom((std::size_t)ref.nbody)
- ,mass((std::size_t)ref.nbody)
- ,Jcom(3,ref.nv)
- ,effortLimit(ref.nq)
- ,velocityLimit(ref.nv)
- ,lowerPositionLimit(ref.nq)
- ,upperPositionLimit(ref.nq)
- {
- /* Create data strcture associated to the joints */
- for(Model::Index i=0;i<(Model::Index)(model.nbody);++i)
- joints.push_back(CreateJointData::run(model.joints[i]));
-
- /* Init for CRBA */
- for(Model::Index i=0;i<(Model::Index)(ref.nbody);++i ) { Fcrb[i].resize(6,model.nv); }
- computeLastChild(ref);
-
- /* Init for Cholesky */
- U = Eigen::MatrixXd::Identity(ref.nv,ref.nv);
- computeParents_fromRow(ref);
-
- /* Init Jacobian */
- J.fill(0);
-
- /* Init universe states relatively to itself */
-
- a[0].setZero();
- v[0].setZero();
- f[0].setZero();
- oMi[0].setIdentity();
- liMi[0].setIdentity();
- }
-
- inline void Data::computeLastChild(const Model& model)
- {
- typedef Model::Index Index;
- std::fill(lastChild.begin(),lastChild.end(),-1);
- for( int i=model.nbody-1;i>=0;--i )
- {
- if(lastChild[(Model::Index)i] == -1) lastChild[(Model::Index)i] = i;
- const Index & parent = model.parents[(Model::Index)i];
- lastChild[parent] = std::max(lastChild[(Model::Index)i],lastChild[parent]);
-
- nvSubtree[(Model::Index)i]
- = idx_v(model.joints[(Model::Index)lastChild[(Model::Index)i]]) + nv(model.joints[(Model::Index)lastChild[(Model::Index)i]])
- - idx_v(model.joints[(Model::Index)i]);
- }
- }
-
- inline void Data::computeParents_fromRow( const Model& model )
- {
- for( Model::Index joint=1;joint<(Model::Index)(model.nbody);joint++)
- {
- const Model::Index & parent = model.parents[joint];
- const int nvj = nv (model.joints[joint]);
- const int idx_vj = idx_v(model.joints[joint]);
-
- if(parent>0) parents_fromRow[(Model::Index)idx_vj] = idx_v(model.joints[parent])+nv(model.joints[parent])-1;
- else parents_fromRow[(Model::Index)idx_vj] = -1;
- nvSubtree_fromRow[(Model::Index)idx_vj] = nvSubtree[joint];
-
- for( int row=1;row<nvj;++row)
- {
- parents_fromRow[(Model::Index)(idx_vj+row)] = idx_vj+row-1;
- nvSubtree_fromRow[(Model::Index)(idx_vj+row)] = nvSubtree[joint]-row;
- }
- }
- }
-
-} // namespace se3
+#include "pinocchio/multibody/model.hxx"
#endif // ifndef __se3_model_hpp__
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
new file mode 100644
index 0000000000000000000000000000000000000000..8c91f6787359bc49f275c2257b1c27d720c11f48
--- /dev/null
+++ b/src/multibody/model.hxx
@@ -0,0 +1,271 @@
+//
+// Copyright (c) 2015 CNRS
+// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
+//
+// 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_model_hxx__
+#define __se3_model_hxx__
+
+#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 "pinocchio/multibody/joint/joint-variant.hpp"
+#include <iostream>
+
+namespace se3
+{
+ inline std::ostream& operator<< (std::ostream & os, const Model & model)
+ {
+ os << "Nb bodies = " << model.nbody << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
+ for(Model::Index i=0;i<(Model::Index)(model.nbody);++i)
+ {
+ os << " Joint "<<model.names[i] << ": parent=" << model.parents[i]
+ << ( model.hasVisual[i] ? " (has visual) " : "(doesnt have visual)" ) << std::endl;
+ }
+
+ return os;
+ }
+
+ inline std::string random (const int len)
+ {
+ std::string res;
+ static const char alphanum[] =
+ "0123456789"
+ "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
+ "abcdefghijklmnopqrstuvwxyz";
+
+ for (int i=0; i<len;++i)
+ res += alphanum[((size_t)std::rand() % (sizeof(alphanum) - 1))];
+ return res;
+ }
+
+ template<typename D>
+ Model::Index Model::addBody (Index parent, const JointModelBase<D> & j, const SE3 & placement,
+ const Inertia & Y, const std::string & jointName,
+ const std::string & bodyName, bool visual)
+ {
+ assert( (nbody==(int)joints.size())&&(nbody==(int)inertias.size())
+ &&(nbody==(int)parents.size())&&(nbody==(int)jointPlacements.size()) );
+ assert( (j.nq()>=0)&&(j.nv()>=0) );
+
+ Model::Index idx = (Model::Index) (nbody ++);
+
+ joints .push_back(j.derived());
+ boost::get<D>(joints.back()).setIndexes((int)idx,nq,nv);
+
+ inertias .push_back(Y);
+ parents .push_back(parent);
+ jointPlacements.push_back(placement);
+ names .push_back( (jointName!="")?jointName:random(8) );
+ hasVisual .push_back(visual);
+ bodyNames .push_back( (bodyName!="")?bodyName:random(8));
+ nq += j.nq();
+ nv += j.nv();
+ return idx;
+ }
+
+ template<typename D>
+ Model::Index Model::addBody (Index parent, const JointModelBase<D> & j, const SE3 & placement,
+ const Inertia & Y,
+ const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
+ const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
+ const std::string & jointName,
+ const std::string & bodyName, bool visual)
+ {
+ assert( (nbody==(int)joints.size())&&(nbody==(int)inertias.size())
+ &&(nbody==(int)parents.size())&&(nbody==(int)jointPlacements.size()) );
+ assert( (j.nq()>=0)&&(j.nv()>=0) );
+
+ Model::Index idx = (Model::Index) (nbody ++);
+
+ joints .push_back(j.derived());
+ boost::get<D>(joints.back()).setIndexes((int)idx,nq,nv);
+
+ boost::get<D>(joints.back()).setMaxEffortLimit(effort);
+ boost::get<D>(joints.back()).setMaxVelocityLimit(velocity);
+
+ boost::get<D>(joints.back()).setLowerPositionLimit(lowPos);
+ boost::get<D>(joints.back()).setUpperPositionLimit(upPos);
+
+ inertias .push_back(Y);
+ parents .push_back(parent);
+ jointPlacements.push_back(placement);
+ names .push_back( (jointName!="")?jointName:random(8) );
+ hasVisual .push_back(visual);
+ bodyNames .push_back( (bodyName!="")?bodyName:random(8));
+ nq += j.nq();
+ nv += j.nv();
+ return idx;
+ }
+
+ inline Model::Index Model::addFixedBody (Index lastMovingParent,
+ const SE3 & placementFromLastMoving,
+ const std::string & bodyName,
+ bool visual)
+ {
+
+ Model::Index idx = (Model::Index) (nFixBody++);
+ fix_lastMovingParent.push_back(lastMovingParent);
+ fix_lmpMi .push_back(placementFromLastMoving);
+ fix_hasVisual .push_back(visual);
+ fix_bodyNames .push_back( (bodyName!="")?bodyName:random(8));
+ return idx;
+ }
+
+ inline void Model::mergeFixedBody (Index parent, const SE3 & placement, const Inertia & Y)
+ {
+ const Inertia & iYf = Y.se3Action(placement); //TODO
+ inertias[parent] += iYf;
+ }
+
+ inline Model::Index Model::getBodyId (const std::string & name) const
+ {
+ std::vector<std::string>::iterator::difference_type
+ res = std::find(bodyNames.begin(),bodyNames.end(),name) - bodyNames.begin();
+ assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
+ assert( (res>=0)&&(res<nbody) && "The body name you asked does not exist" );
+ return Model::Index(res);
+ }
+
+ inline bool Model::existBodyName (const std::string & name) const
+ {
+ return (bodyNames.end() != std::find(bodyNames.begin(),bodyNames.end(),name));
+ }
+
+ inline const std::string& Model::getBodyName (Model::Index index) const
+ {
+ assert( index < (Model::Index)nbody );
+ return bodyNames[index];
+ }
+
+ inline Model::Index Model::getJointId (const std::string & name) const
+ {
+ std::vector<std::string>::iterator::difference_type
+ res = std::find(names.begin(),names.end(),name) - names.begin();
+ assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
+ assert( (res>=0)&&(res<joints.size()) && "The joint name you asked does not exist" );
+ return Model::Index(res);
+ }
+
+ inline bool Model::existJointName (const std::string & name) const
+ {
+ return (names.end() != std::find(names.begin(),names.end(),name));
+ }
+
+ inline const std::string& Model::getJointName (Model::Index index) const
+ {
+ assert( index < (Model::Index)joints.size() );
+ return names[index];
+ }
+
+ inline Data::Data (const Model & ref)
+ :model(ref)
+ ,joints(0)
+ ,a((std::size_t)ref.nbody)
+ ,v((std::size_t)ref.nbody)
+ ,f((std::size_t)ref.nbody)
+ ,oMi((std::size_t)ref.nbody)
+ ,liMi((std::size_t)ref.nbody)
+ ,tau(ref.nv)
+ ,nle(ref.nv)
+ ,Ycrb((std::size_t)ref.nbody)
+ ,M(ref.nv,ref.nv)
+ ,Fcrb((std::size_t)ref.nbody)
+ ,lastChild((std::size_t)ref.nbody)
+ ,nvSubtree((std::size_t)ref.nbody)
+ ,U(ref.nv,ref.nv)
+ ,D(ref.nv)
+ ,tmp(ref.nv)
+ ,parents_fromRow((std::size_t)ref.nv)
+ ,nvSubtree_fromRow((std::size_t)ref.nv)
+ ,J(6,ref.nv)
+ ,iMf((std::size_t)ref.nbody)
+ ,com((std::size_t)ref.nbody)
+ ,vcom((std::size_t)ref.nbody)
+ ,acom((std::size_t)ref.nbody)
+ ,mass((std::size_t)ref.nbody)
+ ,Jcom(3,ref.nv)
+ ,effortLimit(ref.nq)
+ ,velocityLimit(ref.nv)
+ ,lowerPositionLimit(ref.nq)
+ ,upperPositionLimit(ref.nq)
+ {
+ /* Create data strcture associated to the joints */
+ for(Model::Index i=0;i<(Model::Index)(model.nbody);++i)
+ joints.push_back(CreateJointData::run(model.joints[i]));
+
+ /* Init for CRBA */
+ M.fill(0);
+ for(Model::Index i=0;i<(Model::Index)(ref.nbody);++i ) { Fcrb[i].resize(6,model.nv); }
+ computeLastChild(ref);
+
+ /* Init for Cholesky */
+ U = Eigen::MatrixXd::Identity(ref.nv,ref.nv);
+ computeParents_fromRow(ref);
+
+ /* Init Jacobian */
+ J.fill(0);
+
+ /* Init universe states relatively to itself */
+
+ a[0].setZero();
+ v[0].setZero();
+ f[0].setZero();
+ oMi[0].setIdentity();
+ liMi[0].setIdentity();
+ }
+
+ inline void Data::computeLastChild (const Model & model)
+ {
+ typedef Model::Index Index;
+ std::fill(lastChild.begin(),lastChild.end(),-1);
+ for( int i=model.nbody-1;i>=0;--i )
+ {
+ if(lastChild[(Index)i] == -1) lastChild[(Index)i] = i;
+ const Index & parent = model.parents[(Index)i];
+ lastChild[parent] = std::max(lastChild[(Index)i],lastChild[parent]);
+
+ nvSubtree[(Index)i]
+ = idx_v(model.joints[(Index)lastChild[(Index)i]]) + nv(model.joints[(Index)lastChild[(Index)i]])
+ - idx_v(model.joints[(Index)i]);
+ }
+ }
+
+ inline void Data::computeParents_fromRow (const Model & model)
+ {
+ for( Model::Index joint=1;joint<(Model::Index)(model.nbody);joint++)
+ {
+ const Model::Index & parent = model.parents[joint];
+ const int nvj = nv (model.joints[joint]);
+ const int idx_vj = idx_v(model.joints[joint]);
+
+ if(parent>0) parents_fromRow[(Model::Index)idx_vj] = idx_v(model.joints[parent])+nv(model.joints[parent])-1;
+ else parents_fromRow[(Model::Index)idx_vj] = -1;
+ nvSubtree_fromRow[(Model::Index)idx_vj] = nvSubtree[joint];
+
+ for(int row=1;row<nvj;++row)
+ {
+ parents_fromRow[(Model::Index)(idx_vj+row)] = idx_vj+row-1;
+ nvSubtree_fromRow[(Model::Index)(idx_vj+row)] = nvSubtree[joint]-row;
+ }
+ }
+ }
+
+} // namespace se3
+
+#endif // ifndef __se3_model_hxx__
diff --git a/src/multibody/parser/urdf.hpp b/src/multibody/parser/urdf.hpp
index 6f9915636aaee9ecf850080b85fa088256f3627e..652279dfd046c78990272fc695fe04266a90e5da 100644
--- a/src/multibody/parser/urdf.hpp
+++ b/src/multibody/parser/urdf.hpp
@@ -52,19 +52,7 @@ namespace se3
///
/// \return The converted Spatial Inertia se3::Inertia.
///
- inline Inertia convertFromUrdf( const ::urdf::Inertial& Y )
- {
- const ::urdf::Vector3 & p = Y.origin.position;
- const ::urdf::Rotation & q = Y.origin.rotation;
-
- const Eigen::Vector3d com(p.x,p.y,p.z);
- const Eigen::Matrix3d & R = Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix();
-
- Eigen::Matrix3d I; I << Y.ixx,Y.ixy,Y.ixz
- , Y.ixy,Y.iyy,Y.iyz
- , Y.ixz,Y.iyz,Y.izz;
- return Inertia(Y.mass,com,R*I*R.transpose());
- }
+ inline Inertia convertFromUrdf (const ::urdf::Inertial & Y);
///
/// \brief Convert URDF Pose quantity to SE3.
@@ -73,12 +61,7 @@ namespace se3
///
/// \return The converted pose/transform se3::SE3.
///
- inline SE3 convertFromUrdf( const ::urdf::Pose & M )
- {
- const ::urdf::Vector3 & p = M.position;
- const ::urdf::Rotation & q = M.rotation;
- return SE3( Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix(), Eigen::Vector3d(p.x,p.y,p.z));
- }
+ inline SE3 convertFromUrdf (const ::urdf::Pose & M);
///
/// \brief The four possible cartesian types of an 3D axis.
@@ -93,17 +76,7 @@ namespace se3
///
/// \return The property of the particular axis se3::urdf::AxisCartesian.
///
- inline AxisCartesian extractCartesianAxis( const ::urdf::Vector3 & axis )
- {
- if( (axis.x==1.0)&&(axis.y==0.0)&&(axis.z==0.0) )
- return AXIS_X;
- else if( (axis.x==0.0)&&(axis.y==1.0)&&(axis.z==0.0) )
- return AXIS_Y;
- else if( (axis.x==0.0)&&(axis.y==0.0)&&(axis.z==1.0) )
- return AXIS_Z;
- else
- return AXIS_UNALIGNED;
- }
+ inline AxisCartesian extractCartesianAxis (const ::urdf::Vector3 & axis);
///
/// \brief Recursive procedure for reading the URDF tree.
@@ -113,347 +86,27 @@ namespace se3
/// \param[in] model The model where the link must be added.
/// \param[in] placementOffset The relative placement of the link relative to the closer non fixed joint in the tree.
///
- inline void parseTree(::urdf::LinkConstPtr link, Model & model, const SE3 & placementOffset = SE3::Identity(), bool verbose = false) throw (std::invalid_argument)
-{
-
- // Parent joint of the current body
- ::urdf::JointConstPtr joint = link->parent_joint;
-
- // OffSet of the next link. In case we encounter a fixed joint, we need to propagate the offset placement of its attached body to next joint.
- SE3 nextPlacementOffset = SE3::Identity();
-
- if(joint != NULL) // if the link is not the root of the tree
- {
- assert(link->getParent()!=NULL);
-
- const std::string & joint_name = joint->name;
- const std::string & link_name = link->name;
- const std::string & parent_link_name = link->getParent()->name;
- std::string joint_info = "";
-
- // check if inertial information is provided
- if (!link->inertial && joint->type != ::urdf::Joint::FIXED)
- {
- const std::string exception_message (link->name + " - spatial inertial information missing.");
- throw std::invalid_argument(exception_message);
- }
-
- Model::Index parent_joint_id = (link->getParent()->parent_joint==NULL) ? (model.existJointName("root_joint") ? model.getJointId("root_joint") : 0) :
- model.getJointId( link->getParent()->parent_joint->name );
-
- // Transformation from the parent link to the joint origin
- const SE3 & jointPlacement = placementOffset*convertFromUrdf(joint->parent_to_joint_origin_transform);
-
- const Inertia & Y = (link->inertial) ? convertFromUrdf(*link->inertial) :
- Inertia::Zero();
-
- const bool has_visual = (link->visual) ? true : false;
+ inline void parseTree (::urdf::LinkConstPtr link,
+ Model & model,
+ const SE3 & placementOffset = SE3::Identity(),
+ bool verbose = false) throw (std::invalid_argument);
- switch(joint->type)
- {
- case ::urdf::Joint::FLOATING:
- {
- joint_info = "joint FreeFlyer";
-
- typedef JointModelFreeFlyer::ConfigVector_t ConfigVector_t;
- typedef JointModelFreeFlyer::TangentVector_t TangentVector_t;
- typedef ConfigVector_t::Scalar Scalar_t;
-
-
- ConfigVector_t lower_position;
- lower_position.fill(std::numeric_limits<Scalar_t>::min());
-
- ConfigVector_t upper_position;
- upper_position.fill(std::numeric_limits<Scalar_t>::max());
-
- TangentVector_t max_effort;
- max_effort.fill(std::numeric_limits<Scalar_t>::max());
-
- TangentVector_t max_velocity;
- max_velocity.fill(std::numeric_limits<Scalar_t>::max());
-
- model.addBody(parent_joint_id, JointModelFreeFlyer(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name, link->name, has_visual);
- break;
- }
- case ::urdf::Joint::REVOLUTE:
- {
- joint_info = "joint REVOLUTE with axis";
-
- typedef JointModelRX::ConfigVector_t ConfigVector_t;
- typedef JointModelRX::TangentVector_t TangentVector_t;
-
- TangentVector_t max_effort;
- TangentVector_t max_velocity;
- ConfigVector_t lower_position;
- ConfigVector_t upper_position;
-
- if (joint->limits)
- {
- max_effort << joint->limits->effort;
- max_velocity << joint->limits->velocity;
- lower_position << joint->limits->lower;
- upper_position << joint->limits->upper;
- }
-
- Eigen::Vector3d jointAxis(Eigen::Vector3d::Zero());
- AxisCartesian axis = extractCartesianAxis(joint->axis);
-
- switch(axis)
- {
- case AXIS_X:
- joint_info += " along X";
- model.addBody( parent_joint_id, JointModelRX(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name, has_visual );
- break;
- case AXIS_Y:
- joint_info += " along Y";
- model.addBody( parent_joint_id, JointModelRY(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name, has_visual );
- break;
- case AXIS_Z:
- joint_info += " along Z";
- model.addBody( parent_joint_id, JointModelRZ(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name, has_visual );
- break;
- case AXIS_UNALIGNED:
- {
-
- std::stringstream axis_value;
- axis_value << std::setprecision(5);
- axis_value << "(" << joint->axis.x <<"," << joint->axis.y << "," << joint->axis.z << ")";
- joint_info += " unaligned " + axis_value.str();
-
- jointAxis = Eigen::Vector3d( joint->axis.x,joint->axis.y,joint->axis.z );
- jointAxis.normalize();
- model.addBody( parent_joint_id, JointModelRevoluteUnaligned(jointAxis),
- jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name, has_visual );
- break;
- }
- default:
- assert( false && "Fatal Error while extracting revolute joint axis");
- break;
- }
- break;
- }
- case ::urdf::Joint::CONTINUOUS: // Revolute with no joint limits
- {
- joint_info = "joint CONTINUOUS with axis";
-
- typedef JointModelRX::ConfigVector_t ConfigVector_t;
- typedef JointModelRX::TangentVector_t TangentVector_t;
-
- TangentVector_t max_effort;
- TangentVector_t max_velocity;
- ConfigVector_t lower_position;
- ConfigVector_t upper_position;
-
- if (joint->limits)
- {
- max_effort << joint->limits->effort;
- max_velocity << joint->limits->velocity;
- lower_position << joint->limits->lower;
- upper_position << joint->limits->upper;
- }
-
- Eigen::Vector3d jointAxis(Eigen::Vector3d::Zero());
- AxisCartesian axis = extractCartesianAxis(joint->axis);
-
- switch(axis)
- {
- case AXIS_X:
- joint_info += " along X";
- model.addBody( parent_joint_id, JointModelRX(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name, has_visual );
- break;
- case AXIS_Y:
- joint_info += " along Y";
- model.addBody( parent_joint_id, JointModelRY(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name, has_visual );
- break;
- case AXIS_Z:
- joint_info += " along Z";
- model.addBody( parent_joint_id, JointModelRZ(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name, has_visual );
- break;
- case AXIS_UNALIGNED:
- {
- std::stringstream axis_value;
- axis_value << std::setprecision(5);
- axis_value << "(" << joint->axis.x <<"," << joint->axis.y << "," << joint->axis.z << ")";
- joint_info += " unaligned " + axis_value.str();
-
- jointAxis = Eigen::Vector3d( joint->axis.x,joint->axis.y,joint->axis.z );
- jointAxis.normalize();
- model.addBody( parent_joint_id, JointModelRevoluteUnaligned(jointAxis),
- jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name, has_visual );
- break;
- }
- default:
- assert( false && "Fatal Error while extracting revolute joint axis");
- break;
- }
- break;
- }
- case ::urdf::Joint::PRISMATIC:
- {
- joint_info = "joint PRISMATIC with axis";
-
- typedef JointModelRX::ConfigVector_t ConfigVector_t;
- typedef JointModelRX::TangentVector_t TangentVector_t;
-
- TangentVector_t max_effort;
- TangentVector_t max_velocity;
- ConfigVector_t lower_position;
- ConfigVector_t upper_position;
-
- if (joint->limits)
- {
- max_effort << joint->limits->effort;
- max_velocity << joint->limits->velocity;
- lower_position << joint->limits->lower;
- upper_position << joint->limits->upper;
- }
-
- AxisCartesian axis = extractCartesianAxis(joint->axis);
- switch(axis)
- {
- case AXIS_X:
- joint_info += " along X";
- model.addBody( parent_joint_id, JointModelPX(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name, has_visual );
- break;
- case AXIS_Y:
- joint_info += " along Y";
- model.addBody( parent_joint_id, JointModelPY(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name, has_visual );
- break;
- case AXIS_Z:
- joint_info += " along Z";
- model.addBody( parent_joint_id, JointModelPZ(), jointPlacement, Y,
- max_effort, max_velocity, lower_position, upper_position,
- joint->name,link->name, has_visual );
- break;
- case AXIS_UNALIGNED:
- {
- std::stringstream axis_value;
- axis_value << std::setprecision(5);
- axis_value << "(" << joint->axis.x <<"," << joint->axis.y << "," << joint->axis.z << ")";
- joint_info += " unaligned " + axis_value.str();
- std::cerr << "Bad axis = " << axis_value << std::endl;
- assert(false && "Only X, Y or Z axis are accepted." );
- break;
- }
- default:
- assert( false && "Fatal Error while extracting prismatic joint axis");
- break;
- }
- break;
- }
- case ::urdf::Joint::FIXED:
- {
- // In case of fixed joint, if link has inertial tag:
- // -add the inertia of the link to his parent in the model
- // Otherwise do nothing.
- // In all cases:
- // -let all the children become children of parent
- // -inform the parser of the offset to apply
- // -add fixed body in model to display it in gepetto-viewer
-
- joint_info = "fixed joint";
- if (link->inertial)
- {
- model.mergeFixedBody(parent_joint_id, jointPlacement, Y); //Modify the parent inertia in the model
- }
-
- //transformation of the current placement offset
- nextPlacementOffset = jointPlacement;
-
- //add the fixed Body in the model for the viewer
- model.addFixedBody(parent_joint_id, nextPlacementOffset, link->name, has_visual);
-
- //for the children of the current link, set their parent to be
- //the the parent of the current link.
- BOOST_FOREACH(::urdf::LinkPtr child_link, link->child_links)
- {
- child_link->setParent(link->getParent() );
- }
- break;
- }
- default:
- {
- std::cerr << "The joint type " << joint->type << " is not supported." << std::endl;
- assert(false && "Only revolute, prismatic and fixed joints are accepted." );
- break;
- }
- }
-
- if (verbose)
- {
- std::cout << "Adding Body" << std::endl;
- std::cout << "\"" << link_name << "\" connected to " << "\"" << parent_link_name << "\" throw joint " << "\"" << joint_name << "\"" << std::endl;
- std::cout << "joint type: " << joint_info << std::endl;
- std::cout << "joint placement:\n" << jointPlacement;
- std::cout << "body info: " << std::endl;
- std::cout << " " << "mass: " << Y.mass() << std::endl;
- std::cout << " " << "lever: " << Y.lever().transpose() << std::endl;
- std::cout << " " << "inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): " << Y.inertia().data().transpose() << std::endl << std::endl;
- }
-
- }
- else if (link->getParent() != NULL)
- {
- const std::string exception_message (link->name + " - joint information missing.");
- throw std::invalid_argument(exception_message);
- }
-
- BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
- {
- parseTree(child, model, nextPlacementOffset, verbose);
- }
-}
-
- ///
- /// \brief Parse a tree with a specific root joint linking the model to the environment.
- /// The function returns an exception as soon as a necessary Inertia or Joint information are missing.
- ///
- /// \param[in] link The current URDF link.
- /// \param[in] model The model where the link must be added.
- /// \param[in] placementOffset The relative placement of the link relative to the closer non fixed joint in the tree.
- /// \param[in] root_joint The specific root joint.
- ///
+ ///
+ /// \brief Parse a tree with a specific root joint linking the model to the environment.
+ /// The function returns an exception as soon as a necessary Inertia or Joint information are missing.
+ ///
+ /// \param[in] link The current URDF link.
+ /// \param[in] model The model where the link must be added.
+ /// \param[in] placementOffset The relative placement of the link relative to the closer non fixed joint in the tree.
+ /// \param[in] root_joint The specific root joint.
+ ///
template <typename D>
- void parseTree( ::urdf::LinkConstPtr link, Model & model, const SE3 & placementOffset, const JointModelBase<D> & root_joint, const bool verbose = false) throw (std::invalid_argument)
- {
- if (!link->inertial)
- {
- const std::string exception_message (link->name + " - spatial inertia information missing.");
- throw std::invalid_argument(exception_message);
- }
-
- const Inertia & Y = convertFromUrdf(*link->inertial);
- const bool has_visual = (link->visual) ? true : false;
- model.addBody(0, root_joint, placementOffset, Y , "root_joint", link->name, has_visual);
-
- BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
- {
- parseTree(child, model, SE3::Identity(), verbose);
- }
- }
+ void parseTree (::urdf::LinkConstPtr link,
+ Model & model,
+ const SE3 & placementOffset,
+ const JointModelBase<D> & root_joint,
+ const bool verbose = false) throw (std::invalid_argument);
///
/// \brief Build the model from a URDF file with a particular joint as root of the model tree.
@@ -464,21 +117,9 @@ namespace se3
/// \return The se3::Model of the URDF file.
///
template <typename D>
- Model buildModel(const std::string & filename, const JointModelBase<D> & root_joint, bool verbose = false) throw (std::invalid_argument)
- {
- Model model;
-
- ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
- if (urdfTree)
- parseTree(urdfTree->getRoot(), model, SE3::Identity(), root_joint, verbose);
- else
- {
- const std::string exception_message ("The file " + filename + " does not contain a valid URDF model.");
- throw std::invalid_argument(exception_message);
- }
-
- return model;
- }
+ Model buildModel (const std::string & filename,
+ const JointModelBase<D> & root_joint,
+ bool verbose = false) throw (std::invalid_argument);
///
/// \brief Build the model from a URDF file with a fixed joint as root of the model tree.
@@ -487,23 +128,15 @@ namespace se3
///
/// \return The se3::Model of the URDF file.
///
- inline Model buildModel(const std::string & filename, const bool verbose = false) throw (std::invalid_argument)
- {
- Model model;
-
- ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
- if (urdfTree)
- parseTree(urdfTree->getRoot(), model, SE3::Identity(), verbose);
- else
- {
- const std::string exception_message ("The file " + filename + " does not contain a valid URDF model.");
- throw std::invalid_argument(exception_message);
- }
-
- return model;
- }
+ inline Model buildModel (const std::string & filename,
+ const bool verbose = false) throw (std::invalid_argument);
} // namespace urdf
} // namespace se3
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+#include "pinocchio/multibody/parser/urdf.hxx"
+
#endif // ifndef __se3_urdf_hpp__
diff --git a/src/multibody/parser/urdf.hxx b/src/multibody/parser/urdf.hxx
new file mode 100644
index 0000000000000000000000000000000000000000..6036287b9d57bce48c7affc640b9a3322951ce56
--- /dev/null
+++ b/src/multibody/parser/urdf.hxx
@@ -0,0 +1,444 @@
+//
+// Copyright (c) 2015 CNRS
+// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
+//
+// 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_urdf_hxx__
+#define __se3_urdf_hxx__
+
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
+
+#include <iostream>
+#include <sstream>
+#include <iomanip>
+#include <boost/foreach.hpp>
+#include "pinocchio/multibody/model.hpp"
+
+#include <exception>
+
+namespace se3
+{
+ namespace urdf
+ {
+
+ inline Inertia convertFromUrdf (const ::urdf::Inertial & Y)
+ {
+ const ::urdf::Vector3 & p = Y.origin.position;
+ const ::urdf::Rotation & q = Y.origin.rotation;
+
+ const Eigen::Vector3d com(p.x,p.y,p.z);
+ const Eigen::Matrix3d & R = Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix();
+
+ Eigen::Matrix3d I; I << Y.ixx,Y.ixy,Y.ixz
+ , Y.ixy,Y.iyy,Y.iyz
+ , Y.ixz,Y.iyz,Y.izz;
+ return Inertia(Y.mass,com,R*I*R.transpose());
+ }
+
+ inline SE3 convertFromUrdf (const ::urdf::Pose & M)
+ {
+ const ::urdf::Vector3 & p = M.position;
+ const ::urdf::Rotation & q = M.rotation;
+ return SE3( Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix(), Eigen::Vector3d(p.x,p.y,p.z));
+ }
+
+ inline AxisCartesian extractCartesianAxis (const ::urdf::Vector3 & axis)
+ {
+ if( (axis.x==1.0)&&(axis.y==0.0)&&(axis.z==0.0) )
+ return AXIS_X;
+ else if( (axis.x==0.0)&&(axis.y==1.0)&&(axis.z==0.0) )
+ return AXIS_Y;
+ else if( (axis.x==0.0)&&(axis.y==0.0)&&(axis.z==1.0) )
+ return AXIS_Z;
+ else
+ return AXIS_UNALIGNED;
+ }
+
+
+ inline void parseTree (::urdf::LinkConstPtr link, Model & model, const SE3 & placementOffset, bool verbose) throw (std::invalid_argument)
+ {
+
+ // Parent joint of the current body
+ ::urdf::JointConstPtr joint = link->parent_joint;
+
+ // OffSet of the next link. In case we encounter a fixed joint, we need to propagate the offset placement of its attached body to next joint.
+ SE3 nextPlacementOffset = SE3::Identity();
+
+ if(joint != NULL) // if the link is not the root of the tree
+ {
+ assert(link->getParent()!=NULL);
+
+ const std::string & joint_name = joint->name;
+ const std::string & link_name = link->name;
+ const std::string & parent_link_name = link->getParent()->name;
+ std::string joint_info = "";
+
+ // check if inertial information is provided
+ if (!link->inertial && joint->type != ::urdf::Joint::FIXED)
+ {
+ const std::string exception_message (link->name + " - spatial inertial information missing.");
+ throw std::invalid_argument(exception_message);
+ }
+
+ Model::Index parent_joint_id = (link->getParent()->parent_joint==NULL) ? (model.existJointName("root_joint") ? model.getJointId("root_joint") : 0) :
+ model.getJointId( link->getParent()->parent_joint->name );
+
+ // Transformation from the parent link to the joint origin
+ const SE3 & jointPlacement = placementOffset*convertFromUrdf(joint->parent_to_joint_origin_transform);
+
+ const Inertia & Y = (link->inertial) ? convertFromUrdf(*link->inertial) :
+ Inertia::Zero();
+
+ const bool has_visual = (link->visual) ? true : false;
+
+ switch(joint->type)
+ {
+ case ::urdf::Joint::FLOATING:
+ {
+ joint_info = "joint FreeFlyer";
+
+ typedef JointModelFreeFlyer::ConfigVector_t ConfigVector_t;
+ typedef JointModelFreeFlyer::TangentVector_t TangentVector_t;
+ typedef ConfigVector_t::Scalar Scalar_t;
+
+
+ ConfigVector_t lower_position;
+ lower_position.fill(std::numeric_limits<Scalar_t>::min());
+
+ ConfigVector_t upper_position;
+ upper_position.fill(std::numeric_limits<Scalar_t>::max());
+
+ TangentVector_t max_effort;
+ max_effort.fill(std::numeric_limits<Scalar_t>::max());
+
+ TangentVector_t max_velocity;
+ max_velocity.fill(std::numeric_limits<Scalar_t>::max());
+
+ model.addBody(parent_joint_id, JointModelFreeFlyer(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name, link->name, has_visual);
+ break;
+ }
+ case ::urdf::Joint::REVOLUTE:
+ {
+ joint_info = "joint REVOLUTE with axis";
+
+ typedef JointModelRX::ConfigVector_t ConfigVector_t;
+ typedef JointModelRX::TangentVector_t TangentVector_t;
+
+ TangentVector_t max_effort;
+ TangentVector_t max_velocity;
+ ConfigVector_t lower_position;
+ ConfigVector_t upper_position;
+
+ if (joint->limits)
+ {
+ max_effort << joint->limits->effort;
+ max_velocity << joint->limits->velocity;
+ lower_position << joint->limits->lower;
+ upper_position << joint->limits->upper;
+ }
+
+ Eigen::Vector3d jointAxis(Eigen::Vector3d::Zero());
+ AxisCartesian axis = extractCartesianAxis(joint->axis);
+
+ switch(axis)
+ {
+ case AXIS_X:
+ joint_info += " along X";
+ model.addBody( parent_joint_id, JointModelRX(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name, has_visual );
+ break;
+ case AXIS_Y:
+ joint_info += " along Y";
+ model.addBody( parent_joint_id, JointModelRY(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name, has_visual );
+ break;
+ case AXIS_Z:
+ joint_info += " along Z";
+ model.addBody( parent_joint_id, JointModelRZ(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name, has_visual );
+ break;
+ case AXIS_UNALIGNED:
+ {
+ std::stringstream axis_value;
+ axis_value << std::setprecision(5);
+ axis_value << "(" << joint->axis.x <<"," << joint->axis.y << "," << joint->axis.z << ")";
+ joint_info += " unaligned " + axis_value.str();
+
+ jointAxis = Eigen::Vector3d( joint->axis.x,joint->axis.y,joint->axis.z );
+ jointAxis.normalize();
+ model.addBody( parent_joint_id, JointModelRevoluteUnaligned(jointAxis),
+ jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name, has_visual );
+ break;
+ }
+ default:
+ assert( false && "Fatal Error while extracting revolute joint axis");
+ break;
+ }
+ break;
+ }
+ case ::urdf::Joint::CONTINUOUS: // Revolute with no joint limits
+ {
+ joint_info = "joint CONTINUOUS with axis";
+
+ typedef JointModelRX::ConfigVector_t ConfigVector_t;
+ typedef JointModelRX::TangentVector_t TangentVector_t;
+
+ TangentVector_t max_effort;
+ TangentVector_t max_velocity;
+ ConfigVector_t lower_position;
+ ConfigVector_t upper_position;
+
+ if (joint->limits)
+ {
+ max_effort << joint->limits->effort;
+ max_velocity << joint->limits->velocity;
+ lower_position << joint->limits->lower;
+ upper_position << joint->limits->upper;
+ }
+
+ Eigen::Vector3d jointAxis(Eigen::Vector3d::Zero());
+ AxisCartesian axis = extractCartesianAxis(joint->axis);
+
+ switch(axis)
+ {
+ case AXIS_X:
+ joint_info += " along X";
+ model.addBody( parent_joint_id, JointModelRX(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name, has_visual );
+ break;
+ case AXIS_Y:
+ joint_info += " along Y";
+ model.addBody( parent_joint_id, JointModelRY(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name, has_visual );
+ break;
+ case AXIS_Z:
+ joint_info += " along Z";
+ model.addBody( parent_joint_id, JointModelRZ(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name, has_visual );
+ break;
+ case AXIS_UNALIGNED:
+ {
+ std::stringstream axis_value;
+ axis_value << std::setprecision(5);
+ axis_value << "(" << joint->axis.x <<"," << joint->axis.y << "," << joint->axis.z << ")";
+ joint_info += " unaligned " + axis_value.str();
+
+ jointAxis = Eigen::Vector3d( joint->axis.x,joint->axis.y,joint->axis.z );
+ jointAxis.normalize();
+ model.addBody( parent_joint_id, JointModelRevoluteUnaligned(jointAxis),
+ jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name, has_visual );
+ break;
+ }
+ default:
+ assert( false && "Fatal Error while extracting revolute joint axis");
+ break;
+ }
+ break;
+ }
+ case ::urdf::Joint::PRISMATIC:
+ {
+ joint_info = "joint PRISMATIC with axis";
+
+ typedef JointModelRX::ConfigVector_t ConfigVector_t;
+ typedef JointModelRX::TangentVector_t TangentVector_t;
+
+ TangentVector_t max_effort;
+ TangentVector_t max_velocity;
+ ConfigVector_t lower_position;
+ ConfigVector_t upper_position;
+
+ if (joint->limits)
+ {
+ max_effort << joint->limits->effort;
+ max_velocity << joint->limits->velocity;
+ lower_position << joint->limits->lower;
+ upper_position << joint->limits->upper;
+ }
+
+ AxisCartesian axis = extractCartesianAxis(joint->axis);
+ switch(axis)
+ {
+ case AXIS_X:
+ joint_info += " along X";
+ model.addBody( parent_joint_id, JointModelPX(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name, has_visual );
+ break;
+ case AXIS_Y:
+ joint_info += " along Y";
+ model.addBody( parent_joint_id, JointModelPY(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name, has_visual );
+ break;
+ case AXIS_Z:
+ joint_info += " along Z";
+ model.addBody( parent_joint_id, JointModelPZ(), jointPlacement, Y,
+ max_effort, max_velocity, lower_position, upper_position,
+ joint->name,link->name, has_visual );
+ break;
+ case AXIS_UNALIGNED:
+ {
+ std::stringstream axis_value;
+ axis_value << std::setprecision(5);
+ axis_value << "(" << joint->axis.x <<"," << joint->axis.y << "," << joint->axis.z << ")";
+ joint_info += " unaligned " + axis_value.str();
+ std::cerr << "Bad axis = " << axis_value << std::endl;
+ assert(false && "Only X, Y or Z axis are accepted." );
+ break;
+ }
+ default:
+ assert( false && "Fatal Error while extracting prismatic joint axis");
+ break;
+ }
+ break;
+ }
+ case ::urdf::Joint::FIXED:
+ {
+ // In case of fixed joint, if link has inertial tag:
+ // -add the inertia of the link to his parent in the model
+ // Otherwise do nothing.
+ // In all cases:
+ // -let all the children become children of parent
+ // -inform the parser of the offset to apply
+ // -add fixed body in model to display it in gepetto-viewer
+
+ joint_info = "fixed joint";
+ if (link->inertial)
+ {
+ model.mergeFixedBody(parent_joint_id, jointPlacement, Y); //Modify the parent inertia in the model
+ }
+
+ //transformation of the current placement offset
+ nextPlacementOffset = jointPlacement;
+
+ //add the fixed Body in the model for the viewer
+ model.addFixedBody(parent_joint_id, nextPlacementOffset, link->name, has_visual);
+
+ //for the children of the current link, set their parent to be
+ //the the parent of the current link.
+ BOOST_FOREACH(::urdf::LinkPtr child_link, link->child_links)
+ {
+ child_link->setParent(link->getParent() );
+ }
+ break;
+ }
+ default:
+ {
+ std::cerr << "The joint type " << joint->type << " is not supported." << std::endl;
+ assert(false && "Only revolute, prismatic and fixed joints are accepted." );
+ break;
+ }
+ }
+
+ if (verbose)
+ {
+ std::cout << "Adding Body" << std::endl;
+ std::cout << "\"" << link_name << "\" connected to " << "\"" << parent_link_name << "\" throw joint " << "\"" << joint_name << "\"" << std::endl;
+ std::cout << "joint type: " << joint_info << std::endl;
+ std::cout << "joint placement:\n" << jointPlacement;
+ std::cout << "body info: " << std::endl;
+ std::cout << " " << "mass: " << Y.mass() << std::endl;
+ std::cout << " " << "lever: " << Y.lever().transpose() << std::endl;
+ std::cout << " " << "inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): " << Y.inertia().data().transpose() << std::endl << std::endl;
+ }
+
+ }
+ else if (link->getParent() != NULL)
+ {
+ const std::string exception_message (link->name + " - joint information missing.");
+ throw std::invalid_argument(exception_message);
+ }
+
+
+ BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
+ {
+ parseTree(child, model, nextPlacementOffset, verbose);
+ }
+ }
+
+
+ template <typename D>
+ void parseTree (::urdf::LinkConstPtr link, Model & model, const SE3 & placementOffset, const JointModelBase<D> & root_joint, const bool verbose) throw (std::invalid_argument)
+ {
+ if (!link->inertial)
+ {
+ const std::string exception_message (link->name + " - spatial inertia information missing.");
+ throw std::invalid_argument(exception_message);
+ }
+
+ const Inertia & Y = convertFromUrdf(*link->inertial);
+ const bool has_visual = (link->visual) ? true : false;
+ model.addBody(0, root_joint, placementOffset, Y , "root_joint", link->name, has_visual);
+
+ BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
+ {
+ parseTree(child, model, SE3::Identity(), verbose);
+ }
+ }
+
+
+ template <typename D>
+ Model buildModel (const std::string & filename, const JointModelBase<D> & root_joint, bool verbose) throw (std::invalid_argument)
+ {
+ Model model;
+
+ ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
+ if (urdfTree)
+ parseTree(urdfTree->getRoot(), model, SE3::Identity(), root_joint, verbose);
+ else
+ {
+ const std::string exception_message ("The file " + filename + " does not contain a valid URDF model.");
+ throw std::invalid_argument(exception_message);
+ }
+
+ return model;
+ }
+
+
+ inline Model buildModel (const std::string & filename, const bool verbose) throw (std::invalid_argument)
+ {
+ Model model;
+
+ ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
+ if (urdfTree)
+ parseTree(urdfTree->getRoot(), model, SE3::Identity(), verbose);
+ else
+ {
+ const std::string exception_message ("The file " + filename + " does not contain a valid URDF model.");
+ throw std::invalid_argument(exception_message);
+ }
+
+ return model;
+ }
+
+ } // namespace urdf
+
+} // namespace se3
+
+#endif // ifndef __se3_urdf_hxx__