Commit 1002170c authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[URDF] Move parser into the library.

parent eb03c616
......@@ -8,7 +8,8 @@
# ----------------------------------------------------
SET(${PROJECT_NAME}_SOURCES
${${PROJECT_NAME}_PARSERS_SOURCES}
parsers/urdf/model.cpp
parsers/urdf/geometry.cpp
)
# Extract the compile definitions of the project for export
......
......@@ -8,9 +8,14 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/geometry.hpp"
#include "pinocchio/parsers/urdf/types.hpp"
/// \cond
// forward declaration of the unique type from urdfdom which is expose (mostly
// for backward compatibility).
namespace urdf {
class ModelInterface;
}
namespace hpp
{
namespace fcl
......@@ -72,7 +77,7 @@ namespace pinocchio
/// or ::urdf::parseURDFFile
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
ModelTpl<Scalar,Options,JointCollectionTpl> &
buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
buildModel(const ::urdf::ModelInterface* urdfTree,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & rootJoint,
ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const bool verbose = false);
......@@ -89,7 +94,7 @@ namespace pinocchio
/// or ::urdf::parseURDFFile
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
ModelTpl<Scalar,Options,JointCollectionTpl> &
buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
buildModel(const ::urdf::ModelInterface* urdfTree,
ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const bool verbose = false);
......
This diff is collapsed.
This diff is collapsed.
//
// Copyright (c) 2015-2020 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#include "pinocchio/parsers/urdf.hpp"
#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>
#include <sstream>
#include <boost/foreach.hpp>
#include <limits>
namespace pinocchio
{
namespace urdf
{
namespace details
{
///
/// \brief Convert URDF Inertial quantity to Spatial Inertia.
///
/// \param[in] Y The input URDF Inertia.
///
/// \return The converted Spatial Inertia pinocchio::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::InertialSharedPtr & Y)
{
if (Y) return convertFromUrdf (*Y);
return Inertia::Zero();
}
///
/// \brief Convert URDF Pose quantity to SE3.
///
/// \param[in] M The input URDF Pose.
///
/// \return The converted pose/transform pinocchio::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));
}
FrameIndex getParentJointFrame(const ::urdf::LinkConstSharedPtr link,
UrdfVisitorBase& model)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(link && link->getParent());
FrameIndex id = -1;
std::string parent_joint_name;
if (!link->getParent()->parent_joint)
{
try {
id = model.getJointFrameId("root_joint");
} catch (const std::invalid_argument&) {
id = 0;
}
}
else
id = model.getJointFrameId(link->getParent()->parent_joint->name);
return id;
}
///
/// \brief Recursive procedure for reading the URDF tree.
/// 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.
///
void parseTree(::urdf::LinkConstSharedPtr link,
UrdfVisitorBase& model)
{
typedef typename UrdfVisitorBase::Scalar Scalar;
typedef typename UrdfVisitorBase::SE3 SE3;
typedef typename UrdfVisitorBase::Vector Vector;
typedef typename UrdfVisitorBase::Vector3 Vector3;
typedef typename Model::FrameIndex FrameIndex;
// Parent joint of the current body
const ::urdf::JointConstSharedPtr joint =
::urdf::const_pointer_cast< ::urdf::Joint>(link->parent_joint);
if(joint) // if the link is not the root of the tree
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(link->getParent());
const std::string & joint_name = joint->name;
const std::string & link_name = link->name;
const std::string & parent_link_name = link->getParent()->name;
std::ostringstream joint_info;
FrameIndex parentFrameId = getParentJointFrame(link, model);
// Transformation from the parent link to the joint origin
const SE3 jointPlacement
= convertFromUrdf(joint->parent_to_joint_origin_transform);
const Inertia Y = convertFromUrdf(link->inertial);
Vector max_effort(1), max_velocity(1), min_config(1), max_config(1);
Vector3 axis (joint->axis.x, joint->axis.y, joint->axis.z);
const Scalar infty = std::numeric_limits<Scalar>::infinity();
switch(joint->type)
{
case ::urdf::Joint::FLOATING:
joint_info << "joint FreeFlyer";
max_effort = Vector::Constant(6, infty);
max_velocity = Vector::Constant(6, infty);
min_config = Vector::Constant(7,-infty);
max_config = Vector::Constant(7, infty);
min_config.tail<4>().setConstant(-1.01);
max_config.tail<4>().setConstant( 1.01);
model.addJointAndBody(UrdfVisitorBase::FLOATING, axis,
parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,min_config,max_config);
break;
case ::urdf::Joint::REVOLUTE:
joint_info << "joint REVOLUTE with axis";
// TODO I think the URDF standard forbids REVOLUTE with no limits.
assert(joint->limits);
if (joint->limits)
{
max_effort << joint->limits->effort;
max_velocity << joint->limits->velocity;
min_config << joint->limits->lower;
max_config << joint->limits->upper;
}
model.addJointAndBody(UrdfVisitorBase::REVOLUTE, axis,
parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,min_config,max_config);
break;
case ::urdf::Joint::CONTINUOUS: // Revolute joint with no joint limits
joint_info << "joint CONTINUOUS with axis";
min_config.resize(2);
max_config.resize(2);
min_config << -1.01, -1.01;
max_config << 1.01, 1.01;
if(joint->limits)
{
max_effort << joint->limits->effort;
max_velocity << joint->limits->velocity;
}
model.addJointAndBody(UrdfVisitorBase::CONTINUOUS, axis,
parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,min_config,max_config);
break;
case ::urdf::Joint::PRISMATIC:
joint_info << "joint PRISMATIC with axis";
// TODO I think the URDF standard forbids REVOLUTE with no limits.
assert(joint->limits);
if (joint->limits)
{
max_effort << joint->limits->effort;
max_velocity << joint->limits->velocity;
min_config << joint->limits->lower;
max_config << joint->limits->upper;
}
model.addJointAndBody(UrdfVisitorBase::PRISMATIC, axis,
parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,min_config,max_config);
break;
case ::urdf::Joint::PLANAR:
joint_info << "joint PLANAR with normal axis along Z";
max_effort = Vector::Constant(3, infty);
max_velocity = Vector::Constant(3, infty);
min_config = Vector::Constant(4,-infty);
max_config = Vector::Constant(4, infty);
min_config.tail<2>().setConstant(-1.01);
max_config.tail<2>().setConstant( 1.01);
model.addJointAndBody(UrdfVisitorBase::PLANAR, axis,
parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,min_config,max_config);
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";
model.addFixedJointAndBody(parentFrameId, jointPlacement,
joint_name, Y, link_name);
break;
default:
throw std::invalid_argument("The type of joint " + joint_name + " is not supported.");
break;
}
model
<< "Adding Body" << '\n'
<< '\"' << link_name << "\" connected to \"" << parent_link_name << "\" through joint \"" << joint_name << "\"\n"
<< "joint type: " << joint_info.str() << '\n'
<< "joint placement:\n" << jointPlacement << '\n'
<< "body info: " << '\n'
<< " mass: " << Y.mass() << '\n'
<< " lever: " << Y.lever().transpose() << '\n'
<< " inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): " << Y.inertia().data().transpose() << '\n' << '\n';
}
else if (link->getParent())
throw std::invalid_argument(link->name + " - joint information missing.");
BOOST_FOREACH(::urdf::LinkConstSharedPtr child, link->child_links)
{
parseTree(child, model);
}
}
///
/// \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.
///
void parseRootTree(const ::urdf::ModelInterface * urdfTree,
UrdfVisitorBase& model)
{
model.setName(urdfTree->getName());
::urdf::LinkConstSharedPtr root_link = urdfTree->getRoot();
model.addRootJoint(convertFromUrdf(root_link->inertial), root_link->name);
BOOST_FOREACH(::urdf::LinkConstSharedPtr child, root_link->child_links)
{
parseTree(child, model);
}
}
void parseRootTree(const std::string & filename,
UrdfVisitorBase& model)
{
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);
if (urdfTree)
return parseRootTree (urdfTree.get(), model);
else
throw std::invalid_argument("The file " + filename + " does not "
"contain a valid URDF model.");
}
void parseRootTreeFromXML(const std::string & xmlString,
UrdfVisitorBase& model)
{
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlString);
if (urdfTree)
return parseRootTree (urdfTree.get(), model);
else
throw std::invalid_argument("The XML stream does not contain a valid "
"URDF model.");
}
} // namespace details
} // namespace urdf
} // namespace pinocchio
This diff is collapsed.
......@@ -5,76 +5,6 @@
#ifndef __pinocchio_parsers_urdf_utils_hpp__
#define __pinocchio_parsers_urdf_utils_hpp__
#include <urdf_model/model.h>
#warning "This file is not deprecated and will be removed in future releases. There is no replacement."
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/inertia.hpp"
namespace pinocchio
{
namespace urdf
{
///
/// \brief Convert URDF Inertial quantity to Spatial Inertia.
///
/// \param[in] Y The input URDF Inertia.
///
/// \return The converted Spatial Inertia pinocchio::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());
}
///
/// \brief Convert URDF Pose quantity to SE3.
///
/// \param[in] M The input URDF Pose.
///
/// \return The converted pose/transform pinocchio::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));
}
///
/// \brief The four possible cartesian types of an 3D axis.
///
enum CartesianAxis { AXIS_X=0, AXIS_Y=1, AXIS_Z=2, AXIS_UNALIGNED };
///
/// \brief Extract the cartesian property of a particular 3D axis.
///
/// \param[in] axis The input URDF axis.
///
/// \return The property of the particular axis pinocchio::urdf::CartesianAxis.
///
inline CartesianAxis 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;
}
} //urdf
} // se3
#endif // __pinocchio_parsers_urdf_utils_hpp__
Markdown is supported
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