src/parsers/urdf/model.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2020 CNRS
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #include "pinocchio/parsers/urdf.hpp"
7 #include "pinocchio/parsers/urdf/utils.hpp"
8 #include "pinocchio/parsers/urdf/model.hxx"
9 
10 #include <urdf_model/model.h>
11 #include <urdf_parser/urdf_parser.h>
12 
13 #include <sstream>
14 #include <boost/foreach.hpp>
15 #include <limits>
16 
17 namespace pinocchio
18 {
19  namespace urdf
20  {
21  namespace details
22  {
30  static Inertia convertFromUrdf(const ::urdf::Inertial & Y)
31  {
32  const ::urdf::Vector3 & p = Y.origin.position;
33  const ::urdf::Rotation & q = Y.origin.rotation;
34 
35  const Inertia::Vector3 com(p.x, p.y, p.z);
36  const Inertia::Matrix3 & R =
37  Eigen::Quaterniond(q.w, q.x, q.y, q.z).cast<Inertia::Scalar>().matrix();
38 
39  Inertia::Matrix3 I;
40  I << Y.ixx, Y.ixy, Y.ixz, Y.ixy, Y.iyy, Y.iyz, Y.ixz, Y.iyz, Y.izz;
41  return Inertia(Y.mass, com, R * I * R.transpose());
42  }
43 
44  static Inertia convertFromUrdf(const ::urdf::InertialSharedPtr & Y)
45  {
46  if (Y)
47  return convertFromUrdf(*Y);
48  return Inertia::Zero();
49  }
50 
51  static FrameIndex
52  getParentLinkFrame(const ::urdf::LinkConstSharedPtr link, UrdfVisitorBase & model)
53  {
54  PINOCCHIO_CHECK_INPUT_ARGUMENT(link && link->getParent());
55  FrameIndex id = model.getBodyId(link->getParent()->name);
56  return id;
57  }
58 
67  void parseTree(::urdf::LinkConstSharedPtr link, UrdfVisitorBase & model, const bool mimic)
68  {
69  typedef UrdfVisitorBase::Scalar Scalar;
70  typedef UrdfVisitorBase::SE3 SE3;
71  typedef UrdfVisitorBase::Vector Vector;
72  typedef UrdfVisitorBase::Vector3 Vector3;
73  typedef Model::FrameIndex FrameIndex;
74 
75  // Parent joint of the current body
76  const ::urdf::JointConstSharedPtr joint =
77  ::urdf::const_pointer_cast<::urdf::Joint>(link->parent_joint);
78 
79  if (joint) // if the link is not the root of the tree
80  {
81  PINOCCHIO_CHECK_INPUT_ARGUMENT(link->getParent());
82 
83  const std::string & joint_name = joint->name;
84  const std::string & link_name = link->name;
85  const std::string & parent_link_name = link->getParent()->name;
86  std::ostringstream joint_info;
87 
88  FrameIndex parentFrameId = getParentLinkFrame(link, model);
89 
90  // Transformation from the parent link to the joint origin
91  const SE3 jointPlacement =
92  convertFromUrdf(joint->parent_to_joint_origin_transform).cast<Scalar>();
93 
94  const Inertia Y = convertFromUrdf(link->inertial).cast<Scalar>();
95 
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);
99 
100  const Scalar infty = std::numeric_limits<Scalar>::infinity();
101 
102  switch (joint->type)
103  {
104  case ::urdf::Joint::FLOATING:
105  joint_info << "joint FreeFlyer";
106 
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);
113 
114  friction = Vector::Constant(6, 0.);
115  damping = Vector::Constant(6, 0.);
116 
117  model.addJointAndBody(
118  UrdfVisitorBase::FLOATING, axis, parentFrameId, jointPlacement, joint->name, Y,
119  link->name, max_effort, max_velocity, min_config, max_config, friction, damping);
120  break;
121 
122  case ::urdf::Joint::REVOLUTE:
123  joint_info << "joint REVOLUTE with axis";
124 
125  // TODO I think the URDF standard forbids REVOLUTE with no limits.
126  assert(joint->limits);
127  if (joint->limits)
128  {
129  max_effort << joint->limits->effort;
130  max_velocity << joint->limits->velocity;
131  min_config << joint->limits->lower;
132  max_config << joint->limits->upper;
133  }
134 
135  if (joint->dynamics)
136  {
137  friction << joint->dynamics->friction;
138  damping << joint->dynamics->damping;
139  }
140  if (joint->mimic && mimic)
141  {
142  max_effort = Vector::Constant(0, infty);
143  max_velocity = Vector::Constant(0, infty);
144  min_config = Vector::Constant(0, -infty);
145  max_config = Vector::Constant(0, infty);
146 
147  friction = Vector::Constant(0, 0.);
148  damping = Vector::Constant(0, 0.);
149 
150  MimicInfo_ mimic_info(
151  joint->mimic->joint_name, joint->mimic->multiplier, joint->mimic->offset, axis,
152  UrdfVisitorBase::REVOLUTE);
153 
154  model.addJointAndBody(
155  UrdfVisitorBase::MIMIC, axis, parentFrameId, jointPlacement, joint->name, Y,
156  link->name, max_effort, max_velocity, min_config, max_config, friction, damping,
157  mimic_info);
158  }
159  else
160  model.addJointAndBody(
161  UrdfVisitorBase::REVOLUTE, axis, parentFrameId, jointPlacement, joint->name, Y,
162  link->name, max_effort, max_velocity, min_config, max_config, friction, damping);
163  break;
164 
165  case ::urdf::Joint::CONTINUOUS: // Revolute joint with no joint limits
166  joint_info << "joint CONTINUOUS with axis";
167 
168  min_config.resize(2);
169  max_config.resize(2);
170  min_config << -1.01, -1.01;
171  max_config << 1.01, 1.01;
172 
173  if (joint->limits)
174  {
175  max_effort << joint->limits->effort;
176  max_velocity << joint->limits->velocity;
177  }
178  else
179  {
180  max_effort << infty;
181  max_velocity << infty;
182  }
183 
184  if (joint->dynamics)
185  {
186  friction << joint->dynamics->friction;
187  damping << joint->dynamics->damping;
188  }
189 
190  model.addJointAndBody(
191  UrdfVisitorBase::CONTINUOUS, axis, parentFrameId, jointPlacement, joint->name, Y,
192  link->name, max_effort, max_velocity, min_config, max_config, friction, damping);
193  break;
194 
195  case ::urdf::Joint::PRISMATIC:
196  joint_info << "joint PRISMATIC with axis";
197 
198  // TODO I think the URDF standard forbids REVOLUTE with no limits.
199  assert(joint->limits);
200  if (joint->limits)
201  {
202  max_effort << joint->limits->effort;
203  max_velocity << joint->limits->velocity;
204  min_config << joint->limits->lower;
205  max_config << joint->limits->upper;
206  }
207 
208  if (joint->dynamics)
209  {
210  friction << joint->dynamics->friction;
211  damping << joint->dynamics->damping;
212  }
213  if (joint->mimic && mimic)
214  {
215  max_effort = Vector::Constant(0, infty);
216  max_velocity = Vector::Constant(0, infty);
217  min_config = Vector::Constant(0, -infty);
218  max_config = Vector::Constant(0, infty);
219 
220  friction = Vector::Constant(0, 0.);
221  damping = Vector::Constant(0, 0.);
222 
223  MimicInfo_ mimic_info(
224  joint->mimic->joint_name, joint->mimic->multiplier, joint->mimic->offset, axis,
225  UrdfVisitorBase::PRISMATIC);
226 
227  model.addJointAndBody(
228  UrdfVisitorBase::MIMIC, axis, parentFrameId, jointPlacement, joint->name, Y,
229  link->name, max_effort, max_velocity, min_config, max_config, friction, damping,
230  mimic_info);
231  }
232  else
233  model.addJointAndBody(
234  UrdfVisitorBase::PRISMATIC, axis, parentFrameId, jointPlacement, joint->name, Y,
235  link->name, max_effort, max_velocity, min_config, max_config, friction, damping);
236  break;
237 
238  case ::urdf::Joint::PLANAR:
239  joint_info << "joint PLANAR with normal axis along Z";
240 
241  max_effort = Vector::Constant(3, infty);
242  max_velocity = Vector::Constant(3, infty);
243  min_config = Vector::Constant(4, -infty);
244  max_config = Vector::Constant(4, infty);
245  min_config.tail<2>().setConstant(-1.01);
246  max_config.tail<2>().setConstant(1.01);
247 
248  friction = Vector::Constant(3, 0.);
249  damping = Vector::Constant(3, 0.);
250 
251  model.addJointAndBody(
252  UrdfVisitorBase::PLANAR, axis, parentFrameId, jointPlacement, joint->name, Y,
253  link->name, max_effort, max_velocity, min_config, max_config, friction, damping);
254  break;
255 
256  case ::urdf::Joint::FIXED:
257  // In case of fixed joint, if link has inertial tag:
258  // -add the inertia of the link to his parent in the model
259  // Otherwise do nothing.
260  // In all cases:
261  // -let all the children become children of parent
262  // -inform the parser of the offset to apply
263  // -add fixed body in model to display it in gepetto-viewer
264 
265  joint_info << "fixed joint";
266  model.addFixedJointAndBody(parentFrameId, jointPlacement, joint_name, Y, link_name);
267  break;
268 
269  default:
270  throw std::invalid_argument("The type of joint " + joint_name + " is not supported.");
271  break;
272  }
273 
274  model << "Adding Body" << '\n'
275  << '\"' << link_name << "\" connected to \"" << parent_link_name
276  << "\" through joint \"" << joint_name << "\"\n"
277  << "joint type: " << joint_info.str() << '\n'
278  << "joint placement:\n"
279  << jointPlacement << '\n'
280  << "body info: " << '\n'
281  << " mass: " << Y.mass() << '\n'
282  << " lever: " << Y.lever().transpose() << '\n'
283  << " inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): "
284  << Y.inertia().data().transpose() << '\n'
285  << '\n';
286  }
287  else if (link->getParent())
288  throw std::invalid_argument(link->name + " - joint information missing.");
289 
290  BOOST_FOREACH (::urdf::LinkConstSharedPtr child, link->child_links)
291  {
292  parseTree(child, model, mimic);
293  }
294  }
295 
304  void parseRootTree(
305  const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model, const bool mimic)
306  {
307  model.setName(urdfTree->getName());
308 
309  ::urdf::LinkConstSharedPtr root_link = urdfTree->getRoot();
310  model.addRootJoint(convertFromUrdf(root_link->inertial).cast<double>(), root_link->name);
311 
312  BOOST_FOREACH (::urdf::LinkConstSharedPtr child, root_link->child_links)
313  {
314  parseTree(child, model, mimic);
315  }
316  }
317 
318  void parseRootTree(const std::string & filename, UrdfVisitorBase & model, const bool mimic)
319  {
320  ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
321  if (urdfTree)
322  return parseRootTree(urdfTree.get(), model, mimic);
323  else
324  throw std::invalid_argument(
325  "The file " + filename
326  + " does not "
327  "contain a valid URDF model.");
328  }
329 
330  void
331  parseRootTreeFromXML(const std::string & xmlString, UrdfVisitorBase & model, const bool mimic)
332  {
333  ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlString);
334  if (urdfTree)
335  return parseRootTree(urdfTree.get(), model, mimic);
336  else
337  throw std::invalid_argument("The XML stream does not contain a valid "
338  "URDF model.");
339  }
340  } // namespace details
341  } // namespace urdf
342 } // namespace pinocchio
pinocchio::FrameIndex
Index FrameIndex
Definition: multibody/fwd.hpp:28
PINOCCHIO_CHECK_INPUT_ARGUMENT
#define PINOCCHIO_CHECK_INPUT_ARGUMENT(...)
Macro to check an assert-like condition and throw a std::invalid_argument exception (with a message) ...
Definition: include/pinocchio/macros.hpp:196
pinocchio::id
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain.
pinocchio::SE3
context::SE3 SE3
Definition: spatial/fwd.hpp:59
pinocchio::urdf::details::parseRootTree
void parseRootTree(const ::urdf::ModelInterface *urdfTree, UrdfVisitorBase &model, const bool mimic)
Parse a tree with a specific root joint linking the model to the environment. The function returns an...
Definition: src/parsers/urdf/model.cpp:304
pinocchio::urdf::details::convertFromUrdf
SE3 convertFromUrdf(const ::urdf::Pose &M)
Convert URDF Pose quantity to SE3.
Definition: utils.cpp:10
R
R
dpendulum.p
p
Definition: dpendulum.py:7
details
filename
filename
urdf.hpp
pinocchio::urdf::details::parseRootTreeFromXML
void parseRootTreeFromXML(const std::string &xmlString, UrdfVisitorBase &model, const bool mimic)
Definition: src/parsers/urdf/model.cpp:331
pinocchio::ModelTpl< context::Scalar, context::Options >::FrameIndex
pinocchio::FrameIndex FrameIndex
Definition: multibody/model.hpp:69
pinocchio::urdf::details::parseTree
void parseTree(::urdf::LinkConstSharedPtr link, UrdfVisitorBase &model, const bool mimic)
Recursive procedure for reading the URDF tree. The function returns an exception as soon as a necessa...
Definition: src/parsers/urdf/model.cpp:67
pinocchio::Inertia
context::Inertia Inertia
Definition: spatial/fwd.hpp:64
axis
axis
utils.hpp
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
append-urdf-model-with-another-model.joint_name
string joint_name
Definition: append-urdf-model-with-another-model.py:33
urdf
Definition: types.hpp:29
Y
Y
pinocchio::urdf::details::getParentLinkFrame
static FrameIndex getParentLinkFrame(const ::urdf::LinkConstSharedPtr link, UrdfVisitorBase &model)
Definition: src/parsers/urdf/model.cpp:52
pinocchio::python::context::SE3
SE3Tpl< Scalar, Options > SE3
Definition: bindings/python/context/generic.hpp:53
Scalar
double Scalar
Definition: timings-cppad-jit.cpp:37
robot-wrapper-viewer.com
com
Definition: robot-wrapper-viewer.py:45
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:33

pinocchio
Author(s):
autogenerated on Wed Apr 16 2025 02:41:50