GCC Code Coverage Report

Directory: ./
File: src/urdf/util.cc
Date: 2025-05-04 12:09:19
Exec Total Coverage
Lines: 180 228 78.9%
Branches: 203 502 40.4%
Line Branch Exec Source
1 // Copyright (c) 2016, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include <coal/mesh_loader/loader.h>
30 #include <urdf_parser/urdf_parser.h>
31
32 #include <hpp/pinocchio/device.hh>
33 #include <hpp/pinocchio/humanoid-robot.hh>
34 #include <hpp/pinocchio/joint-collection.hh>
35 #include <hpp/pinocchio/joint.hh>
36 #include <hpp/pinocchio/urdf/util.hh>
37 #include <hpp/util/debug.hh>
38 #include <pinocchio/algorithm/geometry.hpp>
39 #include <pinocchio/algorithm/model.hpp>
40 #include <pinocchio/multibody/geometry.hpp>
41 #include <pinocchio/parsers/srdf.hpp>
42 #include <pinocchio/parsers/urdf.hpp>
43 #include <pinocchio/parsers/utils.hpp>
44
45 namespace hpp {
46 namespace pinocchio {
47 namespace srdf {
48 12 void removeCollisionPairs(const Model& model, GeomModel& geom_model,
49 const std::string& prefix, std::istream& stream) {
50 // Read xml stream
51 using boost::property_tree::ptree;
52
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 ptree pt;
53
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 read_xml(stream, pt);
54
55 // Iterate over collision pairs
56 12 auto last = geom_model.collisionPairs.end();
57
9/16
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 12 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 69 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 69 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 81 times.
✗ Branch 21 not taken.
✓ Branch 22 taken 69 times.
✓ Branch 23 taken 12 times.
 81 for (const ptree::value_type& v : pt.get_child("robot")) {
58
2/2
✓ Branch 1 taken 27 times.
✓ Branch 2 taken 42 times.
 69 if (v.first == "disable_collisions") {
59 const std::string link1 =
60
3/6
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
 54 prefix + v.second.get<std::string>("<xmlattr>.link1");
61 const std::string link2 =
62
3/6
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
 54 prefix + v.second.get<std::string>("<xmlattr>.link2");
63
64 // Check first if the two bodies exist in model
65
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 27 times.
 27 if (!model.existBodyName(link1)) {
66 hppDout(error, link1 << ", " << link2
67 << ". Link1 does not exist in model. Skip");
68 ✗ continue;
69 }
70
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 27 times.
 27 if (!model.existBodyName(link2)) {
71 hppDout(error, link1 << ", " << link2
72 << ". Link2 does not exist in model. Skip");
73 ✗ continue;
74 }
75
76
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 const typename Model::FrameIndex frame_id1 = model.getBodyId(link1);
77
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 const typename Model::FrameIndex frame_id2 = model.getBodyId(link2);
78
79 // Malformed SRDF
80
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 27 times.
 27 if (frame_id1 == frame_id2) {
81 hppDout(info, "Cannot disable collision between " << link1 << " and "
82 << link2);
83 ✗ continue;
84 }
85
86
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 auto nlast = std::remove_if(
87 54 geom_model.collisionPairs.begin(), last, [&](const auto& colPair) {
88 54 const auto &ga = geom_model.geometryObjects[colPair.first],
89 54 &gb = geom_model.geometryObjects[colPair.second];
90 117 return ((ga.parentFrame == frame_id1) &&
91
4/4
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 27 times.
✓ Branch 2 taken 9 times.
✓ Branch 3 taken 18 times.
 90 (gb.parentFrame == frame_id2)) ||
92
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 27 times.
 36 ((gb.parentFrame == frame_id1) &&
93
1/2
✓ Branch 0 taken 9 times.
✗ Branch 1 not taken.
 63 (ga.parentFrame == frame_id2));
94 });
95 27 if (last != nlast) {
96 hppDout(info,
97 "Remove collision pair (" << link1 << "," << link2 << ")");
98 }
99 27 last = nlast;
100
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
 27 }
101 }
102
2/2
✓ Branch 2 taken 9 times.
✓ Branch 3 taken 3 times.
 12 if (last != geom_model.collisionPairs.end()) {
103 hppDout(info, "Removing "
104 << std::distance(last, geom_model.collisionPairs.end())
105 << " collision pairs.");
106
1/2
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 geom_model.collisionPairs.erase(last, geom_model.collisionPairs.end());
107 }
108 12 }
109
110 11 void removeCollisionPairs(const Model& model, GeomModel& geom_model,
111 const std::string& prefix,
112 const std::string& filename) {
113 // Check extension
114
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 const std::string extension = filename.substr(filename.find_last_of('.') + 1);
115
2/4
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 11 times.
 11 if (extension != "srdf") {
116 const std::string exception_message(filename +
117 ✗ " does not have the right extension.");
118 ✗ throw std::invalid_argument(exception_message);
119 }
120
121 // Open file
122
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 std::ifstream srdf_stream(filename.c_str());
123
2/4
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 11 times.
 11 if (!srdf_stream.is_open()) {
124 const std::string exception_message(filename +
125 ✗ " does not seem to be a valid file.");
126 ✗ throw std::invalid_argument(exception_message);
127 }
128
129
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 removeCollisionPairs(model, geom_model, prefix, srdf_stream);
130 11 }
131
132 1 void removeCollisionPairsFromXML(const Model& model, GeomModel& geom_model,
133 const std::string& prefix,
134 const std::string& xmlString) {
135
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::istringstream srdf_stream(xmlString);
136
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 removeCollisionPairs(model, geom_model, prefix, srdf_stream);
137 1 }
138 } // namespace srdf
139
140 namespace urdf {
141 namespace {
142 #ifdef HPP_DEBUG
143 const bool verbose = true;
144 #else
145 const bool verbose = false;
146 #endif
147
148 12 JointPtr_t findSpecialJoint(const HumanoidRobotPtr_t& robot,
149 const std::string& linkName) {
150 12 return robot->getJointByBodyName(linkName);
151 }
152
153 2 void setSpecialJoints(const HumanoidRobotPtr_t& robot, std::string prefix) {
154
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 2 times.
 2 if (!prefix.empty() && *prefix.rbegin() != '/') prefix += "/";
155 try {
156
3/6
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 2 robot->waist(robot->getJointByName(prefix + "root_joint"));
157 ✗ } catch (const std::exception&) {
158 hppDout(notice, "No waist joint found");
159 }
160 try {
161
2/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 4 robot->chest(findSpecialJoint(robot, prefix + "chest"));
162
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 } catch (const std::exception&) {
163 hppDout(notice, "No chest joint found");
164 2 }
165 try {
166
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 2 robot->leftWrist(findSpecialJoint(robot, prefix + "l_wrist"));
167 ✗ } catch (const std::exception&) {
168 hppDout(notice, "No left wrist joint found");
169 }
170 try {
171
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 2 robot->rightWrist(findSpecialJoint(robot, prefix + "r_wrist"));
172 ✗ } catch (const std::exception&) {
173 hppDout(notice, "No right wrist joint found");
174 }
175 try {
176
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 2 robot->leftAnkle(findSpecialJoint(robot, prefix + "l_ankle"));
177 ✗ } catch (const std::exception&) {
178 hppDout(notice, "No left ankle joint found");
179 }
180 try {
181
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 2 robot->rightAnkle(findSpecialJoint(robot, prefix + "r_ankle"));
182 ✗ } catch (const std::exception&) {
183 hppDout(notice, "No right ankle joint found");
184 }
185 try {
186
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 2 robot->gazeJoint(findSpecialJoint(robot, prefix + "gaze"));
187 ✗ } catch (const std::exception&) {
188 hppDout(notice, "No gaze joint found");
189 }
190 2 }
191
192 2 void fillGaze(const HumanoidRobotPtr_t robot) {
193
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 vector3_t dir, origin;
194 // Gaze direction is defined by the gaze joint local
195 // orientation.
196
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 dir[0] = 1;
197
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 dir[1] = 0;
198
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 dir[2] = 0;
199 // Gaze position should is defined by the gaze joint local
200 // origin.
201
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 origin[0] = 0;
202
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 origin[1] = 0;
203
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 origin[2] = 0;
204
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 robot->gaze(dir, origin);
205 2 }
206
207 11 JointModelVariant buildJoint(const std::string& type) {
208
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 9 times.
 11 if (type == "freeflyer")
209
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 return JointCollection::JointModelFreeFlyer();
210
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 else if (type == "planar")
211
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 18 return JointCollection::JointModelPlanar();
212 ✗ else if (type == "prismatic_x")
213 ✗ return JointCollection::JointModelPX();
214 ✗ else if (type == "prismatic_y")
215 ✗ return JointCollection::JointModelPY();
216 ✗ else if (type == "translation3d")
217 ✗ return JointCollection::JointModelTranslation();
218 else
219 ✗ throw std::invalid_argument("Root joint type \"" + type +
220 ✗ "\" is currently not available.");
221 }
222
223 1 void setPrefix(const std::string& prefix, Model& model, GeomModel& geomModel,
224 const JointIndex& idFirstJoint, const FrameIndex& idFirstFrame) {
225
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
 3 for (JointIndex i = idFirstJoint; i < model.joints.size(); ++i) {
226 2 model.names[i] = prefix + model.names[i];
227 }
228
2/2
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 1 times.
 6 for (FrameIndex i = idFirstFrame; i < model.frames.size(); ++i) {
229 5 ::pinocchio::Frame& f = model.frames[i];
230 5 f.name = prefix + f.name;
231 }
232
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 for (::pinocchio::GeometryObject& go : geomModel.geometryObjects)
233 ✗ go.name = prefix + go.name;
234 1 }
235
236 19 void setRootJointBounds(Model& model, const JointIndex& rtIdx,
237 const std::string& rootType) {
238 19 value_type b = std::numeric_limits<value_type>::infinity();
239
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 17 times.
 19 if (rootType == "freeflyer") {
240 2 const std::size_t idx = model.joints[rtIdx].idx_q();
241
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 model.upperPositionLimit.segment<3>(idx).setConstant(+b);
242
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 model.lowerPositionLimit.segment<3>(idx).setConstant(-b);
243 // Quaternion bounds
244 2 b = 1.01;
245 2 const size_type quat_idx = idx + 3;
246
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 model.upperPositionLimit.segment<4>(quat_idx).setConstant(+b);
247
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 model.lowerPositionLimit.segment<4>(quat_idx).setConstant(-b);
248
2/2
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 8 times.
 17 } else if (rootType == "planar") {
249 9 const std::size_t idx = model.joints[rtIdx].idx_q();
250
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 model.upperPositionLimit.segment<2>(idx).setConstant(+b);
251
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 model.lowerPositionLimit.segment<2>(idx).setConstant(-b);
252 // Unit complex bounds
253 9 b = 1.01;
254 9 const size_type cplx_idx = idx + 2;
255
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 model.upperPositionLimit.segment<2>(cplx_idx).setConstant(+b);
256
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 model.lowerPositionLimit.segment<2>(cplx_idx).setConstant(-b);
257 }
258 19 }
259
260 22 std::string makeModelPath(const std::string& package, const std::string& type,
261 const std::string& modelName,
262 const std::string& suffix = "") {
263
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 std::stringstream ss;
264 ss << "package://" << package << "/" << type << "/" << modelName << suffix
265
9/18
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 22 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 22 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 22 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 22 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 22 times.
✗ Branch 26 not taken.
 22 << "." << type;
266
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 return ss.str();
267 22 }
268
269 template <bool XmlString>
270 1 void _removeCollisionPairs(const Model& model, GeomModel& geomModel,
271 const std::string& prefix, const std::string& srdf) {
272 hppDout(info, "parsing SRDF string:\n" << srdf);
273 1 srdf::removeCollisionPairsFromXML(model, geomModel, prefix, srdf);
274 1 }
275
276 template <>
277 11 void _removeCollisionPairs<false>(const Model& model, GeomModel& geomModel,
278 const std::string& prefix,
279 const std::string& srdf) {
280 hppDout(info, "parsing SRDF file: " << srdf);
281 11 srdf::removeCollisionPairs(model, geomModel, prefix, srdf);
282 11 }
283
284 /// JointPtrType will be a boost or std shared_ptr depending on the
285 /// version of urdfdom.
286 template <typename JointPtrType>
287 19 void addMimicJoints(const std::map<std::string, JointPtrType>& joints,
288 const std::string& prefix, const DevicePtr_t& robot) {
289 typedef std::map<std::string, JointPtrType> UrdfJointMap_t;
290 19 for (typename UrdfJointMap_t::const_iterator _joint = joints.begin();
291
2/2
✓ Branch 3 taken 471 times.
✓ Branch 4 taken 19 times.
 490 _joint != joints.end(); ++_joint) {
292 471 const JointPtrType& joint = _joint->second;
293
7/8
✓ Branch 1 taken 471 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 197 times.
✓ Branch 5 taken 274 times.
✓ Branch 8 taken 13 times.
✓ Branch 9 taken 184 times.
✓ Branch 10 taken 13 times.
✓ Branch 11 taken 458 times.
 471 if (joint && joint->type != ::urdf::Joint::FIXED && joint->mimic) {
294 13 Device::JointLinearConstraint constraint;
295
2/4
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 13 constraint.joint = robot->getJointByName(prefix + joint->name);
296
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 26 constraint.reference =
297
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 13 robot->getJointByName(prefix + joint->mimic->joint_name);
298 13 constraint.multiplier = joint->mimic->multiplier;
299 13 constraint.offset = joint->mimic->offset;
300
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 13 robot->addJointConstraint(constraint);
301 13 }
302 }
303 19 }
304
305 template <bool srdfAsXmlString>
306 38 void _loadModel(const DevicePtr_t& robot, const FrameIndex& baseFrame,
307 const SE3& bMr, std::string prefix, const std::string& rootType,
308 const ::urdf::ModelInterfaceSharedPtr urdfTree,
309 const std::istream& urdfStream, const std::string& srdf) {
310
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 19 times.
 38 if (!urdfTree)
311 ✗ throw std::invalid_argument(
312 "Failed to parse URDF. Use check_urdf command to know what's wrong.");
313
314
3/6
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19 times.
✗ Branch 8 not taken.
 38 ModelPtr_t model = ModelPtr_t(new Model);
315 38 const JointIndex idFirstJoint = model->joints.size();
316 38 const FrameIndex idFirstFrame = model->frames.size();
317
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 11 times.
 38 if (rootType == "anchor")
318
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 16 ::pinocchio::urdf::buildModel(urdfTree, *model, verbose);
319 else
320
3/6
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 11 times.
✗ Branch 10 not taken.
 22 ::pinocchio::urdf::buildModel(urdfTree, buildJoint(rootType), *model,
321 verbose);
322
323 hppDout(notice, "Finished parsing URDF file.");
324
325
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 38 GeomModel geomModel;
326
327
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 38 std::vector<std::string> baseDirs = ::pinocchio::rosPaths();
328
4/8
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 13 times.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 50 static coal::MeshLoaderPtr loader(
329
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 24 new coal::CachedMeshLoader(coal::BV_OBBRSS));
330
1/2
✓ Branch 3 taken 19 times.
✗ Branch 4 not taken.
 38 ::pinocchio::urdf::buildGeom(*model, urdfStream, ::pinocchio::COLLISION,
331 geomModel, baseDirs, loader);
332
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 38 geomModel.addAllCollisionPairs();
333
334
2/2
✓ Branch 1 taken 11 times.
✓ Branch 2 taken 6 times.
 38 if (!srdf.empty()) {
335 if (!srdfAsXmlString)
336
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 22 ::pinocchio::srdf::loadReferenceConfigurations(*model, srdf, verbose);
337 else {
338 hppDout(warning,
339 "Neutral configuration won't be extracted from SRDF string.");
340 // TODO : A method getNeutralConfigurationFromSrdfString must be added in
341 // Pinocchio,
342 // similarly to removeCollisionPairsFromSrdf /
343 // removeCollisionPairsFromSrdfString
344 }
345 }
346
347
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 18 times.
 38 if (!prefix.empty()) {
348
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 if (*prefix.rbegin() != '/') prefix += "/";
349
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 setPrefix(prefix, *model, geomModel, idFirstJoint, idFirstFrame);
350 }
351
352 // Update root joint bounds
353
6/10
✓ Branch 1 taken 11 times.
✓ Branch 2 taken 8 times.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 11 times.
✓ Branch 12 taken 11 times.
✓ Branch 13 taken 8 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
 38 assert((rootType == "anchor") ||
354 (model->names[idFirstJoint] == prefix + "root_joint"));
355
1/2
✓ Branch 2 taken 19 times.
✗ Branch 3 not taken.
 38 setRootJointBounds(*model, idFirstJoint, rootType);
356
357
3/6
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19 times.
✗ Branch 8 not taken.
 38 ModelPtr_t m(new Model);
358
3/6
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19 times.
✗ Branch 8 not taken.
 38 GeomModelPtr_t gm(new GeomModel);
359
1/2
✓ Branch 6 taken 19 times.
✗ Branch 7 not taken.
 38 ::pinocchio::appendModel(robot->model(), *model, robot->geomModel(),
360 38 geomModel, baseFrame, bMr, *m, *gm);
361 38 robot->setModel(m);
362 38 robot->setGeomModel(gm);
363
364
2/2
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 7 times.
 38 if (!srdf.empty()) {
365
1/2
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
 24 _removeCollisionPairs<srdfAsXmlString>(robot->model(), robot->geomModel(),
366 prefix, srdf);
367 }
368
369
1/2
✓ Branch 2 taken 19 times.
✗ Branch 3 not taken.
 38 robot->createData();
370
1/2
✓ Branch 2 taken 19 times.
✗ Branch 3 not taken.
 38 robot->createGeomData();
371
372 // Build mimic joint table.
373
1/2
✓ Branch 2 taken 19 times.
✗ Branch 3 not taken.
 38 addMimicJoints(urdfTree->joints_, prefix, robot);
374
375 hppDout(notice, "Finished parsing SRDF file.");
376 38 }
377
378 } // namespace
379
380 ✗ void loadRobotModel(const DevicePtr_t& robot, const std::string& rootJointType,
381 const std::string& package, const std::string& modelName,
382 const std::string& urdfSuffix,
383 const std::string& srdfSuffix) {
384 ✗ loadModel(robot, 0, "", rootJointType,
385 ✗ makeModelPath(package, "urdf", modelName, urdfSuffix),
386 ✗ makeModelPath(package, "srdf", modelName, srdfSuffix));
387 }
388
389 11 void loadRobotModel(const DevicePtr_t& robot, const FrameIndex& baseFrame,
390 const std::string& prefix, const std::string& rootJointType,
391 const std::string& package, const std::string& modelName,
392 const std::string& urdfSuffix,
393 const std::string& srdfSuffix, const SE3& bMr) {
394
4/12
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 11 taken 11 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 11 times.
✗ Branch 15 not taken.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
 11 loadModel(robot, baseFrame, (prefix.empty() ? "" : prefix + "/"),
395 rootJointType,
396
2/4
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
 22 makeModelPath(package, "urdf", modelName, urdfSuffix),
397
2/4
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
 22 makeModelPath(package, "srdf", modelName, srdfSuffix), bMr);
398 11 }
399
400 2 void setupHumanoidRobot(const HumanoidRobotPtr_t& robot,
401 const std::string& prefix) {
402 // Look for special joints and attach them to the model.
403
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 setSpecialJoints(robot, prefix);
404 // Fill gaze position and direction.
405
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 fillGaze(robot);
406 2 }
407
408 ✗ void loadUrdfModel(const DevicePtr_t& robot, const FrameIndex& baseFrame,
409 const std::string& prefix, const std::string& rootJointType,
410 const std::string& package, const std::string& filename,
411 const SE3& bMr) {
412 ✗ loadModel(robot, baseFrame, (prefix.empty() ? "" : prefix + "/"),
413 ✗ rootJointType, makeModelPath(package, "urdf", filename), "", bMr);
414 }
415
416 ✗ void loadUrdfModel(const DevicePtr_t& robot, const std::string& rootType,
417 const std::string& package, const std::string& filename) {
418 ✗ loadModel(robot, 0, "", rootType, makeModelPath(package, "urdf", filename),
419 "");
420 }
421
422 17 void loadModel(const DevicePtr_t& robot, const FrameIndex& baseFrame,
423 const std::string& prefix, const std::string& rootType,
424 const std::string& urdfPath, const std::string& srdfPath,
425 const SE3& bMr) {
426
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 std::vector<std::string> baseDirs = ::pinocchio::rosPaths();
427
2/4
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
 17 baseDirs.push_back(EXAMPLE_ROBOT_DATA_MODEL_DIR);
428
2/4
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
 17 baseDirs.push_back(EXAMPLE_ROBOT_DATA_MODEL_DIR "/../..");
429
430 std::string urdfFileName =
431
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 ::pinocchio::retrieveResourcePath(urdfPath, baseDirs);
432
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 17 times.
 17 if (urdfFileName == "") {
433 ✗ throw std::invalid_argument(std::string("Unable to retrieve ") + urdfPath);
434 }
435
436 17 std::string srdfFileName;
437
2/2
✓ Branch 1 taken 11 times.
✓ Branch 2 taken 6 times.
 17 if (!srdfPath.empty()) {
438
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 srdfFileName = ::pinocchio::retrieveResourcePath(srdfPath, baseDirs);
439
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 11 times.
 11 if (srdfFileName == "") {
440 ✗ throw std::invalid_argument(std::string("Unable to retrieve ") +
441 ✗ srdfPath);
442 }
443 }
444
445 ::urdf::ModelInterfaceSharedPtr urdfTree =
446
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 ::urdf::parseURDFFile(urdfFileName);
447
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 17 std::ifstream urdfStream(urdfFileName.c_str());
448
2/4
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
 17 _loadModel<false>(robot, baseFrame, bMr, prefix, rootType, urdfTree,
449 urdfStream, srdfFileName);
450 17 }
451
452 2 void loadModelFromString(const DevicePtr_t& robot, const FrameIndex& baseFrame,
453 const std::string& prefix, const std::string& rootType,
454 const std::string& urdfString,
455 const std::string& srdfString, const SE3& bMr) {
456
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(urdfString);
457
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 std::istringstream urdfStream(urdfString);
458
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 _loadModel<true>(robot, baseFrame, bMr, prefix, rootType, urdfTree,
459 urdfStream, srdfString);
460 2 }
461
462 ✗ void loadSRDFModel(const DevicePtr_t& robot, std::string prefix,
463 const std::string& srdfPath) {
464 ✗ std::vector<std::string> baseDirs = ::pinocchio::rosPaths();
465 std::string srdfFileName =
466 ✗ ::pinocchio::retrieveResourcePath(srdfPath, baseDirs);
467 ✗ if (srdfFileName == "") {
468 ✗ throw std::invalid_argument(std::string("Unable to retrieve ") + srdfPath);
469 }
470 ✗ if (!prefix.empty() && *prefix.rbegin() != '/') prefix += "/";
471
472 ✗ _removeCollisionPairs<false>(robot->model(), robot->geomModel(), prefix,
473 srdfFileName);
474 ✗ ::pinocchio::srdf::loadReferenceConfigurations(robot->model(), srdfFileName,
475 verbose);
476 ✗ robot->createGeomData();
477 }
478
479 ✗ void loadSRDFModelFromString(const DevicePtr_t& robot, std::string prefix,
480 const std::string& srdfString) {
481 ✗ if (!prefix.empty() && *prefix.rbegin() != '/') prefix += "/";
482 ✗ _removeCollisionPairs<true>(robot->model(), robot->geomModel(), prefix,
483 srdfString);
484 ✗ robot->createGeomData();
485 }
486 } // end of namespace urdf.
487 } // end of namespace pinocchio.
488 } // end of namespace hpp.
489