model.cpp 11.7 KB
Newer Older
1
2
3
4
5
6
//
// Copyright (c) 2015-2020 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#include "pinocchio/parsers/urdf.hpp"
7
#include "pinocchio/parsers/urdf/utils.hpp"
8
#include "pinocchio/parsers/urdf/model.hxx"
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

#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.
      ///
30
      static Inertia convertFromUrdf(const ::urdf::Inertial & Y)
31
32
33
34
      {
        const ::urdf::Vector3 & p = Y.origin.position;
        const ::urdf::Rotation & q = Y.origin.rotation;

35
36
        const Inertia::Vector3 com(p.x,p.y,p.z);
        const Inertia::Matrix3 & R = Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix();
37

38
39
40
41
        Inertia::Matrix3 I;
        I << Y.ixx,Y.ixy,Y.ixz,
             Y.ixy,Y.iyy,Y.iyz,
             Y.ixz,Y.iyz,Y.izz;
42
43
44
        return Inertia(Y.mass,com,R*I*R.transpose());
      }

45
      static Inertia convertFromUrdf(const ::urdf::InertialSharedPtr & Y)
46
      {
47
        if (Y) return convertFromUrdf(*Y);
48
49
50
        return Inertia::Zero();
      }

51
52
      static FrameIndex getParentLinkFrame(const ::urdf::LinkConstSharedPtr link,
                                           UrdfVisitorBase & model)
53
54
      {
        PINOCCHIO_CHECK_INPUT_ARGUMENT(link && link->getParent());
Joseph Mirabel's avatar
Joseph Mirabel committed
55
        FrameIndex id = model.getBodyId(link->getParent()->name);
56
57
        return id;
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
58

59
60
61
62
63
64
65
66
      ///
      /// \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,
67
                     UrdfVisitorBase & model)
68
      {
69
70
71
72
73
        typedef UrdfVisitorBase::Scalar Scalar;
        typedef UrdfVisitorBase::SE3 SE3;
        typedef UrdfVisitorBase::Vector Vector;
        typedef UrdfVisitorBase::Vector3 Vector3;
        typedef Model::FrameIndex FrameIndex;
74
75
76
77
78
79
80
81
82
83
84
85
86
87

        // 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;

Joseph Mirabel's avatar
Joseph Mirabel committed
88
          FrameIndex parentFrameId = getParentLinkFrame(link, model);
89
90
91
92
93
94
95
96

          // 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);
97
          Vector friction(Vector::Constant(1,0.)), damping(Vector::Constant(1,0.));
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
          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);
113
114
115
              
              friction = Vector::Constant(6, 0.);
              damping = Vector::Constant(6, 0.);
116
117

              model.addJointAndBody(UrdfVisitorBase::FLOATING, axis,
118
119
120
121
                                    parentFrameId,jointPlacement,joint->name,
                                    Y,link->name,
                                    max_effort,max_velocity,min_config,max_config,
                                    friction,damping);
122
123
124
125
126
127
128
              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);
129
              if(joint->limits)
130
131
132
133
134
135
              {
                max_effort << joint->limits->effort;
                max_velocity << joint->limits->velocity;
                min_config << joint->limits->lower;
                max_config << joint->limits->upper;
              }
136
137
138
139
140
141
              
              if(joint->dynamics)
              {
                friction << joint->dynamics->friction;
                damping << joint->dynamics->damping;
              }
142
143

              model.addJointAndBody(UrdfVisitorBase::REVOLUTE, axis,
144
145
146
147
                                    parentFrameId,jointPlacement,joint->name,
                                    Y,link->name,
                                    max_effort,max_velocity,min_config,max_config,
                                    friction,damping);
148
149
150
151
152
153
154
155
156
157
158
159
160
161
              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;
162
163
164
              }
              else
              {
165
166
                max_effort << infty;
                max_velocity << infty;
167
              }
168
169
170
171
172
173
              
              if(joint->dynamics)
              {
                friction << joint->dynamics->friction;
                damping << joint->dynamics->damping;
              }
174
175

              model.addJointAndBody(UrdfVisitorBase::CONTINUOUS, axis,
176
177
178
179
                                    parentFrameId,jointPlacement,joint->name,
                                    Y,link->name,
                                    max_effort,max_velocity,min_config,max_config,
                                    friction,damping);
180
181
182
183
184
185
186
187
188
189
190
191
192
193
              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;
              }
194
195
196
197
198
199
              
              if(joint->dynamics)
              {
                friction << joint->dynamics->friction;
                damping << joint->dynamics->damping;
              }
200
201

              model.addJointAndBody(UrdfVisitorBase::PRISMATIC, axis,
202
203
204
205
                                    parentFrameId,jointPlacement,joint->name,
                                    Y,link->name,
                                    max_effort,max_velocity,min_config,max_config,
                                    friction,damping);
206
207
208
209
210
211
212
213
214
215
216
              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);
217
218
219
              
              friction = Vector::Constant(3, 0.);
              damping = Vector::Constant(3, 0.);
220
221

              model.addJointAndBody(UrdfVisitorBase::PLANAR, axis,
222
223
224
225
                                    parentFrameId,jointPlacement,joint->name,
                                    Y,link->name,
                                    max_effort,max_velocity,min_config,max_config,
                                    friction,damping);
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
              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);
Joseph Mirabel's avatar
Joseph Mirabel committed
280

281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
        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