From a215dbe6fb8cb38d51f5aa5f7af2ccf5a9cb34c6 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Sun, 24 Aug 2014 23:09:21 +0200
Subject: [PATCH] Copy urdf parser in multibody/parser.
---
CMakeLists.txt | 2 +
src/multibody/parser/urdf.hpp | 142 ++++++++++++++++++++++++++++++++++
unittest/urdf.cpp | 131 +------------------------------
3 files changed, 147 insertions(+), 128 deletions(-)
create mode 100644 src/multibody/parser/urdf.hpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 051344854..b5759b14b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -45,6 +45,7 @@ SET(${PROJECT_NAME}_HEADERS
multibody/joint/joint-variant.hpp
multibody/model.hpp
multibody/visitor.hpp
+ multibody/parser/urdf.hpp
tools/timer.hpp
algorithm/rnea.hpp
)
@@ -53,6 +54,7 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/spatial")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/joint")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/parser")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/tools")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm")
diff --git a/src/multibody/parser/urdf.hpp b/src/multibody/parser/urdf.hpp
new file mode 100644
index 000000000..17ab1fc95
--- /dev/null
+++ b/src/multibody/parser/urdf.hpp
@@ -0,0 +1,142 @@
+#ifndef __se3_urdf_hpp__
+#define __se3_urdf_hpp__
+
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
+
+#include <iostream>
+#include <boost/foreach.hpp>
+#include "pinocchio/multibody/model.hpp"
+
+namespace urdf
+{
+ typedef boost::shared_ptr<ModelInterface> ModelInterfacePtr;
+ typedef boost::shared_ptr<const Joint> JointConstPtr;
+ typedef boost::shared_ptr<const Link> LinkConstPtr;
+ typedef boost::shared_ptr<Link> LinkPtr;
+
+ typedef boost::shared_ptr<const Inertial> InertialConstPtr;
+}
+
+namespace se3
+{
+ 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());
+ }
+
+ 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));
+ }
+
+ enum AxisCartesian { AXIS_X, AXIS_Y, AXIS_Z, AXIS_ERROR };
+ 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;
+
+ std::cerr << "Axis = (" <<axis.x<<","<<axis.y<<","<<axis.z<<")" << std::endl;
+ assert( false && "Only cartesian axis are accepted." );
+ return AXIS_ERROR;
+ }
+
+ void parseTree( urdf::LinkConstPtr link, Model & model )
+ {
+ urdf::JointConstPtr joint = link->parent_joint;
+
+ // std::cout << " *** " << link->name << " < attached by joint ";
+ // if(joint)
+ // std::cout << "#" << link->parent_joint->name << std::endl;
+ // else std::cout << "###ROOT" << std::endl;
+
+ assert(link->inertial && "The parser cannot accept trivial mass");
+ const Inertia & Y = convertFromUrdf(*link->inertial);
+ //std::cout << "Inertia: " << Y << std::endl;
+
+ if(joint!=NULL)
+ {
+ assert(link->getParent()!=NULL);
+ Model::Index parent
+ = (link->getParent()->parent_joint==NULL) ?
+ 1 : model.getBodyId( link->getParent()->parent_joint->name );
+
+ const SE3 & jointPlacement = convertFromUrdf(joint->parent_to_joint_origin_transform);
+
+ //std::cout << "Parent = " << parent << std::endl;
+ //std::cout << "Placement = " << (Matrix4)jointPlacement << std::endl;
+
+ switch(joint->type)
+ {
+ case urdf::Joint::REVOLUTE:
+ case urdf::Joint::CONTINUOUS: // Revolute with no joint limits
+ {
+ AxisCartesian axis = extractCartesianAxis(joint->axis);
+ switch(axis)
+ {
+ case AXIS_X:
+ model.addBody( parent, JointModelRX(), jointPlacement, Y, joint->name );
+ break;
+ case AXIS_Y:
+ model.addBody( parent, JointModelRY(), jointPlacement, Y, joint->name );
+ break;
+ case AXIS_Z:
+ model.addBody( parent, JointModelRZ(), jointPlacement, Y, joint->name );
+ break;
+ default:
+ std::cerr << "Bad axis = (" <<joint->axis.x<<","<<joint->axis.y
+ <<","<<joint->axis.z<<")" << std::endl;
+ assert(false && "Only X, Y or Z axis are accepted." );
+ break;
+ }
+ break;
+ }
+ default:
+ {
+ std::cerr << "The joint type " << joint->type << " is not supported." << std::endl;
+ assert(false && "Only revolute joint are accepted." );
+ break;
+ }
+ }
+ }
+ else /* (joint==NULL) */
+ { /* The link is the root of the body. */
+ //std::cout << "Parent = 0 (universe)" << std::endl;
+ model.addBody( 0, JointModelFreeFlyer(), SE3::Identity(), Y, "root" );
+ }
+
+ BOOST_FOREACH(urdf::LinkConstPtr child,link->child_links)
+ {
+ parseTree( child,model );
+ }
+ }
+
+ Model buildModel( const std::string & filename )
+ {
+ Model model;
+
+ urdf::ModelInterfacePtr urdfTree = urdf::parseURDFFile (filename);
+ parseTree(urdfTree->getRoot(),model);
+ return model;
+ }
+
+} // namespace se3
+
+#endif // ifndef __se3_urdf_hpp__
+
diff --git a/unittest/urdf.cpp b/unittest/urdf.cpp
index 3e077859f..9b08df746 100644
--- a/unittest/urdf.cpp
+++ b/unittest/urdf.cpp
@@ -1,139 +1,14 @@
-#include <urdf_model/model.h>
-#include <urdf_parser/urdf_parser.h>
-
#include <iostream>
-#include <boost/foreach.hpp>
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-#include "pinocchio/multibody/model.hpp"
-
-namespace urdf
-{
- typedef boost::shared_ptr<ModelInterface> ModelInterfacePtr;
- typedef boost::shared_ptr<const Joint> JointConstPtr;
- typedef boost::shared_ptr<const Link> LinkConstPtr;
- typedef boost::shared_ptr<Link> LinkPtr;
-
- typedef boost::shared_ptr<const Inertial> InertialConstPtr;
-}
-
-se3::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 se3::Inertia(Y.mass,com,R*I*R.transpose());
-}
-se3::SE3 convertFromUrdf( const urdf::Pose & M )
-{
- const urdf::Vector3 & p = M.position;
- const urdf::Rotation & q = M.rotation;
- return se3::SE3( Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix(), Eigen::Vector3d(p.x,p.y,p.z));
-}
-
-enum AxisCartesian { AXIS_X, AXIS_Y, AXIS_Z };
-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;
- std::cerr << "Axis = (" <<axis.x<<","<<axis.y<<","<<axis.z<<")" << std::endl;
- assert( false && "Only cartesian axis are accepted." );
-}
-
-
-void parseTree( urdf::LinkConstPtr link, const urdf::ModelInterfacePtr & model, se3::Model & multibody )
-{
- urdf::JointConstPtr joint = link->parent_joint;
-
- std::cout << " *** " << link->name << " < attached by joint ";
- if(joint)
- std::cout << "#" << link->parent_joint->name << std::endl;
- else std::cout << "###ROOT" << std::endl;
-
- assert(link->inertial && "The parser cannot accept trivial mass");
- const se3::Inertia & Y = convertFromUrdf(*link->inertial);
- //std::cout << "Inertia: " << Y << std::endl;
-
- if(joint!=NULL)
- {
- assert(link->getParent()!=NULL);
- se3::Model::Index parent
- = (link->getParent()->parent_joint==NULL) ?
- 1 : multibody.getBodyId( link->getParent()->parent_joint->name );
- const se3::SE3 & jointPlacement = convertFromUrdf(joint->parent_to_joint_origin_transform);
-
- std::cout << "Parent = " << parent << std::endl;
- std::cout << "Placement = " << (se3::SE3::Matrix4)jointPlacement << std::endl;
-
- switch(joint->type)
- {
- case urdf::Joint::REVOLUTE:
- case urdf::Joint::CONTINUOUS: // Revolute with no joint limits
- {
- AxisCartesian axis = extractCartesianAxis(joint->axis);
- switch(axis)
- {
- case AXIS_X:
- multibody.addBody( parent, se3::JointModelRX(), jointPlacement, Y, joint->name );
- break;
- case AXIS_Y:
- multibody.addBody( parent, se3::JointModelRY(), jointPlacement, Y, joint->name );
- break;
- case AXIS_Z:
- multibody.addBody( parent, se3::JointModelRZ(), jointPlacement, Y, joint->name );
- break;
- default:
- std::cerr << "Bad axis = (" <<joint->axis.x<<","<<joint->axis.y
- <<","<<joint->axis.z<<")" << std::endl;
- assert(false && "Only X axis are accepted." );
- }
- break;
- }
- default:
- std::cerr << "The joint type " << joint->type << " is not supported." << std::endl;
- }
- }
- else // (joint==NULL)
- { // The link is the root of the body.
- std::cout << "Parent = 0 (universe)" << std::endl;
- multibody.addBody( 0, se3::JointModelFreeFlyer(), se3::SE3::Identity(), Y, "root" );
- }
-
-
- BOOST_FOREACH(urdf::LinkConstPtr child,link->child_links)
- {
- parseTree( child,model,multibody );
- }
-}
-
-
-void buildModel( const std::string & filename )
-{
- urdf::ModelInterfacePtr model = urdf::parseURDFFile (filename);
- urdf::LinkConstPtr root = model->getRoot();
- se3::Model multibody;
-
- parseTree(root,model,multibody);
-}
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/parser/urdf.hpp"
int main(int argc, const char**argv)
{
std::string filename = "/home/nmansard/src/rbdl/rbdl_evaluate_performances/models/simple_humanoid.urdf";
if(argc>1) filename = argv[1];
- buildModel(filename);
+ se3::Model model = se3::buildModel(filename);
return 0;
}
--
GitLab