6 #include "pinocchio/parsers/urdf.hpp" 7 #include "pinocchio/parsers/urdf/utils.hpp" 8 #include "pinocchio/parsers/urdf/model.hxx" 10 #include <urdf_model/model.h> 11 #include <urdf_parser/urdf_parser.h> 14 #include <boost/foreach.hpp> 32 const ::urdf::Vector3 &
p = Y.origin.position;
33 const ::urdf::Rotation &
q = Y.origin.rotation;
35 const Inertia::Vector3
com(p.x,p.y,p.z);
36 const Inertia::Matrix3 &
R = Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix();
39 I << Y.ixx,Y.ixy,Y.ixz,
42 return Inertia(Y.mass,com,R*I*R.transpose());
52 UrdfVisitorBase &
model)
55 FrameIndex id = model.getBodyId(link->getParent()->name);
67 UrdfVisitorBase &
model)
71 typedef UrdfVisitorBase::Vector Vector;
72 typedef UrdfVisitorBase::Vector3 Vector3;
76 const ::urdf::JointConstSharedPtr joint =
77 ::urdf::const_pointer_cast<
::urdf::Joint>(link->parent_joint);
84 const std::string & link_name = link->name;
85 const std::string & parent_link_name = link->getParent()->name;
86 std::ostringstream joint_info;
91 const SE3 jointPlacement
96 Vector max_effort(1), max_velocity(1), min_config(1), max_config(1);
97 Vector friction(Vector::Constant(1,0.)), damping(Vector::Constant(1,0.));
98 Vector3
axis (joint->axis.x, joint->axis.y, joint->axis.z);
100 const Scalar infty = std::numeric_limits<Scalar>::infinity();
104 case ::urdf::Joint::FLOATING:
105 joint_info <<
"joint FreeFlyer";
107 max_effort = Vector::Constant(6, infty);
108 max_velocity = Vector::Constant(6, infty);
109 min_config = Vector::Constant(7,-infty);
110 max_config = Vector::Constant(7, infty);
111 min_config.tail<4>().setConstant(-1.01);
112 max_config.tail<4>().setConstant( 1.01);
114 friction = Vector::Constant(6, 0.);
115 damping = Vector::Constant(6, 0.);
117 model.addJointAndBody(UrdfVisitorBase::FLOATING, axis,
118 parentFrameId,jointPlacement,joint->name,
120 max_effort,max_velocity,min_config,max_config,
124 case ::urdf::Joint::REVOLUTE:
125 joint_info <<
"joint REVOLUTE with axis";
128 assert(joint->limits);
131 max_effort << joint->limits->effort;
132 max_velocity << joint->limits->velocity;
133 min_config << joint->limits->lower;
134 max_config << joint->limits->upper;
139 friction << joint->dynamics->friction;
140 damping << joint->dynamics->damping;
143 model.addJointAndBody(UrdfVisitorBase::REVOLUTE, axis,
144 parentFrameId,jointPlacement,joint->name,
146 max_effort,max_velocity,min_config,max_config,
150 case ::urdf::Joint::CONTINUOUS:
151 joint_info <<
"joint CONTINUOUS with axis";
153 min_config.resize(2);
154 max_config.resize(2);
155 min_config << -1.01, -1.01;
156 max_config << 1.01, 1.01;
160 max_effort << joint->limits->effort;
161 max_velocity << joint->limits->velocity;
166 max_velocity << infty;
171 friction << joint->dynamics->friction;
172 damping << joint->dynamics->damping;
175 model.addJointAndBody(UrdfVisitorBase::CONTINUOUS, axis,
176 parentFrameId,jointPlacement,joint->name,
178 max_effort,max_velocity,min_config,max_config,
182 case ::urdf::Joint::PRISMATIC:
183 joint_info <<
"joint PRISMATIC with axis";
186 assert(joint->limits);
189 max_effort << joint->limits->effort;
190 max_velocity << joint->limits->velocity;
191 min_config << joint->limits->lower;
192 max_config << joint->limits->upper;
197 friction << joint->dynamics->friction;
198 damping << joint->dynamics->damping;
201 model.addJointAndBody(UrdfVisitorBase::PRISMATIC, axis,
202 parentFrameId,jointPlacement,joint->name,
204 max_effort,max_velocity,min_config,max_config,
208 case ::urdf::Joint::PLANAR:
209 joint_info <<
"joint PLANAR with normal axis along Z";
211 max_effort = Vector::Constant(3, infty);
212 max_velocity = Vector::Constant(3, infty);
213 min_config = Vector::Constant(4,-infty);
214 max_config = Vector::Constant(4, infty);
215 min_config.tail<2>().setConstant(-1.01);
216 max_config.tail<2>().setConstant( 1.01);
218 friction = Vector::Constant(3, 0.);
219 damping = Vector::Constant(3, 0.);
221 model.addJointAndBody(UrdfVisitorBase::PLANAR, axis,
222 parentFrameId,jointPlacement,joint->name,
224 max_effort,max_velocity,min_config,max_config,
228 case ::urdf::Joint::FIXED:
237 joint_info <<
"fixed joint";
238 model.addFixedJointAndBody(parentFrameId, jointPlacement,
239 joint_name, Y, link_name);
243 throw std::invalid_argument(
"The type of joint " + joint_name +
" is not supported.");
248 <<
"Adding Body" <<
'\n' 249 <<
'\"' << link_name <<
"\" connected to \"" << parent_link_name <<
"\" through joint \"" << joint_name <<
"\"\n" 250 <<
"joint type: " << joint_info.str() <<
'\n' 251 <<
"joint placement:\n" << jointPlacement <<
'\n' 252 <<
"body info: " <<
'\n' 253 <<
" mass: " << Y.
mass() <<
'\n' 254 <<
" lever: " << Y.
lever().transpose() <<
'\n' 255 <<
" inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): " << Y.
inertia().
data().transpose() <<
'\n' <<
'\n';
257 else if (link->getParent())
258 throw std::invalid_argument(link->name +
" - joint information missing.");
260 BOOST_FOREACH(::urdf::LinkConstSharedPtr child, link->child_links)
274 UrdfVisitorBase&
model)
276 model.setName(urdfTree->getName());
278 ::urdf::LinkConstSharedPtr root_link = urdfTree->getRoot();
279 model.addRootJoint(
convertFromUrdf(root_link->inertial), root_link->name);
281 BOOST_FOREACH(::urdf::LinkConstSharedPtr child, root_link->child_links)
288 UrdfVisitorBase&
model)
290 ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);
294 throw std::invalid_argument(
"The file " + filename +
" does not " 295 "contain a valid URDF model.");
299 UrdfVisitorBase&
model)
301 ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlString);
305 throw std::invalid_argument(
"The XML stream does not contain a valid " InertiaTpl< double, 0 > Inertia
const Symmetric3 & inertia() const
static FrameIndex getParentLinkFrame(const ::urdf::LinkConstSharedPtr link, UrdfVisitorBase &model)
const Vector3 & lever() const
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain...
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
static Inertia convertFromUrdf(const ::urdf::Inertial &Y)
Convert URDF Inertial quantity to Spatial Inertia.
pinocchio::FrameIndex FrameIndex
void parseTree(::urdf::LinkConstSharedPtr link, UrdfVisitorBase &model)
Recursive procedure for reading the URDF tree. The function returns an exception as soon as a necessa...
void parseRootTree(const ::urdf::ModelInterface *urdfTree, UrdfVisitorBase &model)
Parse a tree with a specific root joint linking the model to the environment. The function returns an...
void parseRootTreeFromXML(const std::string &xmlString, UrdfVisitorBase &model)
Main pinocchio namespace.
const Vector6 & data() const
#define PINOCCHIO_CHECK_INPUT_ARGUMENT(...)
Macro to check an assert-like condition and throw a std::invalid_argument exception (with a message) ...
JointCollectionTpl & model