util.cc 10.9 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// Copyright (c) 2016, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-pinocchio.
// hpp-pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-pinocchio. If not, see <http://www.gnu.org/licenses/>.

#include <hpp/pinocchio/urdf/util.hh>

19
#include <pinocchio/parsers/utils.hpp>
20
21
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/multibody/geometry.hpp>
Joseph Mirabel's avatar
Joseph Mirabel committed
22
#include <pinocchio/algorithm/geometry.hpp>
23
24
25
26
#include <pinocchio/parsers/srdf.hpp>

#include <hpp/util/debug.hh>

Joseph Mirabel's avatar
Joseph Mirabel committed
27
#include <hpp/pinocchio/joint.hh>
28
29
30
31
32
33
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/humanoid-robot.hh>

namespace hpp {
  namespace pinocchio {
    namespace urdf {
Joseph Mirabel's avatar
Joseph Mirabel committed
34
35
      using se3::FrameIndex;

36
      namespace {
37
38
39
40
41
42
#ifdef HPP_DEBUG
        const bool verbose = false;
#else
        const bool verbose = true;
#endif

43
44
45
46
47
        JointPtr_t findSpecialJoint (const HumanoidRobotPtr_t& robot, const std::string& linkName)
        {
          return robot->getJointByBodyName (linkName);
        }

Joseph Mirabel's avatar
Joseph Mirabel committed
48
        void setSpecialJoints (const HumanoidRobotPtr_t& robot, const std::string& prefix) {
49
          try {
Joseph Mirabel's avatar
Joseph Mirabel committed
50
            robot->waist (robot->getJointByName(prefix + "root_joint"));
51
52
53
54
          } catch (const std::exception&) {
            hppDout (notice, "No waist joint found");
          }
          try {
Joseph Mirabel's avatar
Joseph Mirabel committed
55
            robot->chest (findSpecialJoint (robot, prefix + "chest"));
56
57
58
59
          } catch (const std::exception&) {
            hppDout (notice, "No chest joint found");
          }
          try {
Joseph Mirabel's avatar
Joseph Mirabel committed
60
            robot->leftWrist (findSpecialJoint (robot, prefix + "l_wrist"));
61
62
63
64
          } catch (const std::exception&) {
            hppDout (notice, "No left wrist joint found");
          }
          try {
Joseph Mirabel's avatar
Joseph Mirabel committed
65
            robot->rightWrist (findSpecialJoint (robot, prefix + "r_wrist"));
66
67
68
69
          } catch (const std::exception&) {
            hppDout (notice, "No right wrist joint found");
          }
          try {
Joseph Mirabel's avatar
Joseph Mirabel committed
70
            robot->leftAnkle (findSpecialJoint (robot, prefix + "l_ankle"));
71
72
73
74
          } catch (const std::exception&) {
            hppDout (notice, "No left ankle joint found");
          }
          try {
Joseph Mirabel's avatar
Joseph Mirabel committed
75
            robot->rightAnkle (findSpecialJoint (robot, prefix + "r_ankle"));
76
77
78
79
          } catch (const std::exception&) {
            hppDout (notice, "No right ankle joint found");
          }
          try {
Joseph Mirabel's avatar
Joseph Mirabel committed
80
            robot->gazeJoint (findSpecialJoint (robot, prefix + "gaze"));
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
          } catch (const std::exception&) {
            hppDout (notice, "No gaze joint found");
          }
        }

        void fillGaze (const HumanoidRobotPtr_t robot)
        {
          vector3_t dir, origin;
          // Gaze direction is defined by the gaze joint local
          // orientation.
          dir[0] = 1;
          dir[1] = 0;
          dir[2] = 0;
          // Gaze position should is defined by the gaze joint local
          // origin.
          origin[0] = 0;
          origin[1] = 0;
          origin[2] = 0;
          robot->gaze (dir, origin);
        }
101
102
103
104
105
106
107
108

        se3::JointModelVariant buildJoint (const std::string& type)
        {
          if (type == "freeflyer")   return se3::JointModelFreeFlyer();
          else if (type == "planar") return se3::JointModelPlanar();
          else                       throw  std::invalid_argument
            ("Root joint type is currently not available.");
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
109
110
111

        void setPrefix (const std::string& prefix,
            Model& model, GeomModel& geomModel,
Joseph Mirabel's avatar
Joseph Mirabel committed
112
113
            const JointIndex& idFirstJoint,
            const FrameIndex& idFirstFrame)
Joseph Mirabel's avatar
Joseph Mirabel committed
114
115
116
117
        {
          for (JointIndex i = idFirstJoint; i < model.joints.size(); ++i) {
            model.names[i] = prefix + model.names[i];
          }
Joseph Mirabel's avatar
Joseph Mirabel committed
118
119
120
          for (FrameIndex i = idFirstFrame; i < model.frames.size(); ++i) {
            se3::Frame& f = model.frames[i];
            f.name = prefix + f.name;
Joseph Mirabel's avatar
Joseph Mirabel committed
121
122
          }
          BOOST_FOREACH(se3::GeometryObject& go, geomModel.geometryObjects) {
Joseph Mirabel's avatar
Joseph Mirabel committed
123
            go.name = prefix + go.name;
Joseph Mirabel's avatar
Joseph Mirabel committed
124
125
126
          }
        }

127
128
129
130
131
        void setRootJointBounds(Model& model,
            const JointIndex& rtIdx,
            const std::string& rootType)
        {
          if (rootType == "freeflyer") {
Joseph Mirabel's avatar
Joseph Mirabel committed
132
133
134
            value_type b = std::numeric_limits<value_type>::infinity();
            model.upperPositionLimit.segment<3>(rtIdx).setConstant(+b);
            model.lowerPositionLimit.segment<3>(rtIdx).setConstant(-b);
135
            // Quaternion bounds
Joseph Mirabel's avatar
Joseph Mirabel committed
136
            b = 1.01;
137
138
139
140
            const size_type quat_idx = model.joints[rtIdx].idx_q() + 3;
            model.upperPositionLimit.segment<4>(quat_idx).setConstant(+b);
            model.lowerPositionLimit.segment<4>(quat_idx).setConstant(-b);
          } else if (rootType == "planar") {
Joseph Mirabel's avatar
Joseph Mirabel committed
141
142
143
144
145
146
147
148
            value_type b = std::numeric_limits<value_type>::infinity();
            model.upperPositionLimit.segment<2>(rtIdx).setConstant(+b);
            model.lowerPositionLimit.segment<2>(rtIdx).setConstant(-b);
            // Unit complex bounds
            b = 1.01;
            const size_type cplx_idx = model.joints[rtIdx].idx_q() + 2;
            model.upperPositionLimit.segment<2>(cplx_idx).setConstant(+b);
            model.lowerPositionLimit.segment<2>(cplx_idx).setConstant(-b);
149
150
151
          }
        }

Joseph Mirabel's avatar
Joseph Mirabel committed
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
        template <bool LoadSRDF> void loadModel
          (const DevicePtr_t& robot,
           const JointIndex&  baseJoint,
           const std::string& prefix,
           const std::string& rootType,
           const std::string& package,
           const std::string& urdfName,
           const std::string& srdfName)
          {
            if (baseJoint != 0)
              throw std::invalid_argument ("Only appending robots at the world is supported.");
            std::vector<std::string> baseDirs = se3::rosPaths();

            std::string urdfPath =
              "package://" + package + "/urdf/" + urdfName + ".urdf";
            std::string urdfFileName = se3::retrieveResourcePath(urdfPath, baseDirs);
168
169
170
171
	    if (urdfFileName == "") {
	      throw std::invalid_argument (std::string ("Unable to retrieve ") +
					   urdfPath);
	    }
Joseph Mirabel's avatar
Joseph Mirabel committed
172
            Model& model = robot->model();
Joseph Mirabel's avatar
Joseph Mirabel committed
173
174
            const JointIndex idFirstJoint = model.joints.size();
            const FrameIndex idFirstFrame = model.frames.size();
Joseph Mirabel's avatar
Joseph Mirabel committed
175
            if (rootType == "anchor")
176
              se3::urdf::buildModel(urdfFileName, model, verbose);
Joseph Mirabel's avatar
Joseph Mirabel committed
177
            else
178
              se3::urdf::buildModel(urdfFileName, buildJoint(rootType), model, verbose);
Joseph Mirabel's avatar
Joseph Mirabel committed
179
180
181
182
183
184
            robot->createData();

            hppDout (notice, "Finished parsing URDF file.");

            GeomModel geomModel;

185
            se3::urdf::buildGeom(model, urdfFileName, se3::COLLISION, geomModel, baseDirs);
Joseph Mirabel's avatar
Joseph Mirabel committed
186
187
188
189
190
191
            geomModel.addAllCollisionPairs();

            if (LoadSRDF) {
              std::string srdfPath =
                "package://" + package + "/srdf/" + srdfName + ".srdf";
              std::string srdfFileName = se3::retrieveResourcePath(srdfPath, baseDirs);
192
193
194
195
	    if (srdfFileName == "") {
	      throw std::invalid_argument (std::string ("Unable to retrieve ") +
					   srdfPath);
	    }
Joseph Mirabel's avatar
Joseph Mirabel committed
196
              se3::srdf::removeCollisionPairsFromSrdf
197
                (model, geomModel, srdfFileName, verbose);
Joseph Mirabel's avatar
Joseph Mirabel committed
198
199
            }

200
201
202
203
204
205
            if (!prefix.empty()) setPrefix(prefix, model, geomModel, idFirstJoint, idFirstFrame);

            // Update root joint bounds
            assert((rootType == "anchor")
                || (model.names[idFirstJoint] == prefix + "root_joint"));
            setRootJointBounds(model, idFirstJoint, rootType);
Joseph Mirabel's avatar
Joseph Mirabel committed
206
207
208
209
210
211

            se3::appendGeometryModel(robot->geomModel(), geomModel);
            robot->createGeomData();

            hppDout (notice, "Finished parsing SRDF file.");
          }
212
213
214
215
216
217
218
219
220
      }

      void loadRobotModel (const DevicePtr_t& robot,
			   const std::string& rootJointType,
			   const std::string& package,
			   const std::string& modelName,
			   const std::string& urdfSuffix,
                           const std::string& srdfSuffix)
      {
Joseph Mirabel's avatar
Joseph Mirabel committed
221
222
        loadModel <true> (robot, 0, "", rootJointType,
            package, modelName + urdfSuffix, modelName + srdfSuffix);
223
224
225
      }

      void loadRobotModel (const DevicePtr_t& robot,
Joseph Mirabel's avatar
Joseph Mirabel committed
226
                           const JointIndex&  baseJoint,
227
228
229
230
231
232
233
			   const std::string& prefix,
			   const std::string& rootJointType,
			   const std::string& package,
			   const std::string& modelName,
			   const std::string& urdfSuffix,
                           const std::string& srdfSuffix)
      {
Joseph Mirabel's avatar
Joseph Mirabel committed
234
235
236
237
        loadModel <true> (robot, baseJoint, 
            (prefix.empty() ? "" : prefix + "/"),
            rootJointType,
            package, modelName + urdfSuffix, modelName + srdfSuffix);
238
239
240
241
242
243
244
245
246
247
248
249
      }

      void loadHumanoidModel (const HumanoidRobotPtr_t& robot,
			      const std::string& rootJointType,
			      const std::string& package,
			      const std::string& modelName,
			      const std::string& urdfSuffix,
			      const std::string& srdfSuffix)
      {
        loadRobotModel(robot, rootJointType, package, modelName, urdfSuffix, srdfSuffix);

	// Look for special joints and attach them to the model.
Joseph Mirabel's avatar
Joseph Mirabel committed
250
	setSpecialJoints (robot, "");
251
252
253
254
255
	// Fill gaze position and direction.
	fillGaze (robot);
      }

      void loadHumanoidModel (const HumanoidRobotPtr_t& robot,
Joseph Mirabel's avatar
Joseph Mirabel committed
256
                              const JointIndex&  baseJoint,
257
258
259
260
261
262
263
			      const std::string& prefix,
			      const std::string& rootJointType,
			      const std::string& package,
			      const std::string& modelName,
			      const std::string& urdfSuffix,
			      const std::string& srdfSuffix)
      {
Joseph Mirabel's avatar
Joseph Mirabel committed
264
        loadRobotModel(robot, baseJoint, prefix, rootJointType, package, modelName, urdfSuffix, srdfSuffix);
265

Joseph Mirabel's avatar
Joseph Mirabel committed
266
267
268
269
	// Look for special joints and attach them to the model.
	setSpecialJoints (robot, prefix);
	// Fill gaze position and direction.
	fillGaze (robot);
270
271
272
      }

      void loadUrdfModel (const DevicePtr_t& robot,
Joseph Mirabel's avatar
Joseph Mirabel committed
273
274
                          const JointIndex&  baseJoint,
                          const std::string& prefix,
275
276
277
278
			  const std::string& rootJointType,
			  const std::string& package,
			  const std::string& filename)
      {
Joseph Mirabel's avatar
Joseph Mirabel committed
279
280
281
        loadModel<false> (robot, baseJoint,
            (prefix.empty() ? "" : prefix + "/"),
            rootJointType, package, filename, "");
282
      }
283

Joseph Mirabel's avatar
Joseph Mirabel committed
284
285
      void loadUrdfModel (const DevicePtr_t& robot,
			  const std::string& rootType,
286
287
288
			  const std::string& package,
			  const std::string& filename)
      {
Joseph Mirabel's avatar
Joseph Mirabel committed
289
        loadModel<false> (robot, 0, "", rootType, package, filename, "");
290
      }
291
292
293
    } // end of namespace urdf.
  } // end of namespace pinocchio.
} // end of namespace  hpp.