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 = Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix();
37 
38  Inertia::Matrix3 I;
39  I << Y.ixx,Y.ixy,Y.ixz,
40  Y.ixy,Y.iyy,Y.iyz,
41  Y.ixz,Y.iyz,Y.izz;
42  return Inertia(Y.mass,com,R*I*R.transpose());
43  }
44 
45  static Inertia convertFromUrdf(const ::urdf::InertialSharedPtr & Y)
46  {
47  if (Y) return convertFromUrdf(*Y);
48  return Inertia::Zero();
49  }
50 
51  static FrameIndex getParentLinkFrame(const ::urdf::LinkConstSharedPtr link,
52  UrdfVisitorBase & model)
53  {
54  PINOCCHIO_CHECK_INPUT_ARGUMENT(link && link->getParent());
55  FrameIndex id = model.getBodyId(link->getParent()->name);
56  return id;
57  }
58 
66  void parseTree(::urdf::LinkConstSharedPtr link,
67  UrdfVisitorBase & model)
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);
93 
94  const Inertia Y = convertFromUrdf(link->inertial);
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(UrdfVisitorBase::FLOATING, axis,
118  parentFrameId,jointPlacement,joint->name,
119  Y,link->name,
120  max_effort,max_velocity,min_config,max_config,
121  friction,damping);
122  break;
123 
124  case ::urdf::Joint::REVOLUTE:
125  joint_info << "joint REVOLUTE with axis";
126 
127  // TODO I think the URDF standard forbids REVOLUTE with no limits.
128  assert(joint->limits);
129  if(joint->limits)
130  {
131  max_effort << joint->limits->effort;
132  max_velocity << joint->limits->velocity;
133  min_config << joint->limits->lower;
134  max_config << joint->limits->upper;
135  }
136 
137  if(joint->dynamics)
138  {
139  friction << joint->dynamics->friction;
140  damping << joint->dynamics->damping;
141  }
142 
143  model.addJointAndBody(UrdfVisitorBase::REVOLUTE, axis,
144  parentFrameId,jointPlacement,joint->name,
145  Y,link->name,
146  max_effort,max_velocity,min_config,max_config,
147  friction,damping);
148  break;
149 
150  case ::urdf::Joint::CONTINUOUS: // Revolute joint with no joint limits
151  joint_info << "joint CONTINUOUS with axis";
152 
153  min_config.resize(2);
154  max_config.resize(2);
155  min_config << -1.01, -1.01;
156  max_config << 1.01, 1.01;
157 
158  if(joint->limits)
159  {
160  max_effort << joint->limits->effort;
161  max_velocity << joint->limits->velocity;
162  }
163  else
164  {
165  max_effort << infty;
166  max_velocity << infty;
167  }
168 
169  if(joint->dynamics)
170  {
171  friction << joint->dynamics->friction;
172  damping << joint->dynamics->damping;
173  }
174 
175  model.addJointAndBody(UrdfVisitorBase::CONTINUOUS, axis,
176  parentFrameId,jointPlacement,joint->name,
177  Y,link->name,
178  max_effort,max_velocity,min_config,max_config,
179  friction,damping);
180  break;
181 
182  case ::urdf::Joint::PRISMATIC:
183  joint_info << "joint PRISMATIC with axis";
184 
185  // TODO I think the URDF standard forbids REVOLUTE with no limits.
186  assert(joint->limits);
187  if (joint->limits)
188  {
189  max_effort << joint->limits->effort;
190  max_velocity << joint->limits->velocity;
191  min_config << joint->limits->lower;
192  max_config << joint->limits->upper;
193  }
194 
195  if(joint->dynamics)
196  {
197  friction << joint->dynamics->friction;
198  damping << joint->dynamics->damping;
199  }
200 
201  model.addJointAndBody(UrdfVisitorBase::PRISMATIC, axis,
202  parentFrameId,jointPlacement,joint->name,
203  Y,link->name,
204  max_effort,max_velocity,min_config,max_config,
205  friction,damping);
206  break;
207 
208  case ::urdf::Joint::PLANAR:
209  joint_info << "joint PLANAR with normal axis along Z";
210 
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);
217 
218  friction = Vector::Constant(3, 0.);
219  damping = Vector::Constant(3, 0.);
220 
221  model.addJointAndBody(UrdfVisitorBase::PLANAR, axis,
222  parentFrameId,jointPlacement,joint->name,
223  Y,link->name,
224  max_effort,max_velocity,min_config,max_config,
225  friction,damping);
226  break;
227 
228  case ::urdf::Joint::FIXED:
229  // In case of fixed joint, if link has inertial tag:
230  // -add the inertia of the link to his parent in the model
231  // Otherwise do nothing.
232  // In all cases:
233  // -let all the children become children of parent
234  // -inform the parser of the offset to apply
235  // -add fixed body in model to display it in gepetto-viewer
236 
237  joint_info << "fixed joint";
238  model.addFixedJointAndBody(parentFrameId, jointPlacement,
239  joint_name, Y, link_name);
240  break;
241 
242  default:
243  throw std::invalid_argument("The type of joint " + joint_name + " is not supported.");
244  break;
245  }
246 
247  model
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';
256  }
257  else if (link->getParent())
258  throw std::invalid_argument(link->name + " - joint information missing.");
259 
260  BOOST_FOREACH(::urdf::LinkConstSharedPtr child, link->child_links)
261  {
262  parseTree(child, model);
263  }
264  }
265 
273  void parseRootTree(const ::urdf::ModelInterface * urdfTree,
274  UrdfVisitorBase& model)
275  {
276  model.setName(urdfTree->getName());
277 
278  ::urdf::LinkConstSharedPtr root_link = urdfTree->getRoot();
279  model.addRootJoint(convertFromUrdf(root_link->inertial), root_link->name);
280 
281  BOOST_FOREACH(::urdf::LinkConstSharedPtr child, root_link->child_links)
282  {
283  parseTree(child, model);
284  }
285  }
286 
287  void parseRootTree(const std::string & filename,
288  UrdfVisitorBase& model)
289  {
290  ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);
291  if (urdfTree)
292  return parseRootTree (urdfTree.get(), model);
293  else
294  throw std::invalid_argument("The file " + filename + " does not "
295  "contain a valid URDF model.");
296  }
297 
298  void parseRootTreeFromXML(const std::string & xmlString,
299  UrdfVisitorBase& model)
300  {
301  ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlString);
302  if (urdfTree)
303  return parseRootTree (urdfTree.get(), model);
304  else
305  throw std::invalid_argument("The XML stream does not contain a valid "
306  "URDF model.");
307  }
308  } // namespace details
309  } // namespace urdf
310 } // namespace pinocchio
pinocchio::Inertia
InertiaTpl< double, 0 > Inertia
Definition: src/spatial/fwd.hpp:59
axis
axis
pinocchio::FrameIndex
Index FrameIndex
Definition: src/multibody/fwd.hpp:30
pinocchio::urdf::details::getParentLinkFrame
static FrameIndex getParentLinkFrame(const ::urdf::LinkConstSharedPtr link, UrdfVisitorBase &model)
Definition: src/parsers/urdf/model.cpp:51
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::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:746
pinocchio::urdf::details::convertFromUrdf
static Inertia convertFromUrdf(const ::urdf::Inertial &Y)
Convert URDF Inertial quantity to Spatial Inertia.
Definition: src/parsers/urdf/model.cpp:30
pinocchio::python::Scalar
SE3::Scalar Scalar
Definition: conversions.cpp:13
pinocchio::InertiaTpl::Zero
static InertiaTpl Zero()
Definition: src/spatial/inertia.hpp:212
pinocchio::InertiaTpl::inertia
const Symmetric3 & inertia() const
Definition: src/spatial/inertia.hpp:545
pinocchio::InertiaTpl::lever
const Vector3 & lever() const
Definition: src/spatial/inertia.hpp:544
pinocchio::ModelTpl::FrameIndex
pinocchio::FrameIndex FrameIndex
Definition: src/multibody/model.hpp:51
urdf
Definition: types.hpp:29
pinocchio::urdf::details::parseTree
void parseTree(::urdf::LinkConstSharedPtr link, UrdfVisitorBase &model)
Recursive procedure for reading the URDF tree. The function returns an exception as soon as a necessa...
Definition: src/parsers/urdf/model.cpp:66
R
R
pinocchio::urdf::details::parseRootTree
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...
Definition: src/parsers/urdf/model.cpp:273
pinocchio::urdf::details::parseRootTreeFromXML
void parseRootTreeFromXML(const std::string &xmlString, UrdfVisitorBase &model)
Definition: src/parsers/urdf/model.cpp:298
pinocchio::Symmetric3Tpl::data
const Vector6 & data() const
Definition: spatial/symmetric3.hpp:160
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:30
pinocchio::Joint
JointTpl< double > Joint
Definition: joint-generic.hpp:19
pinocchio::SE3
SE3Tpl< double, 0 > SE3
Definition: src/spatial/fwd.hpp:53
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: src/macros.hpp:127
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:746

pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:04