GCC Code Coverage Report

Directory: ./
File: include/pinocchio/parsers/srdf.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 110 145 75.9%
Branches: 212 508 41.7%
Line Branch Exec Source
1 //
2 // Copyright (c) 2017-2022 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_parser_srdf_hxx__
6 #define __pinocchio_parser_srdf_hxx__
7
8 #include "pinocchio/parsers/srdf.hpp"
9
10 #include "pinocchio/multibody/model.hpp"
11 #include "pinocchio/multibody/geometry.hpp"
12 #include "pinocchio/algorithm/joint-configuration.hpp"
13 #include <iostream>
14
15 // Read XML file with boost
16 #include <boost/property_tree/xml_parser.hpp>
17 #include <boost/property_tree/ptree.hpp>
18 #include <fstream>
19 #include <sstream>
20 #include <boost/foreach.hpp>
21
22 namespace pinocchio
23 {
24 namespace srdf
25 {
26 namespace details
27 {
28 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
29 8 void removeCollisionPairs(
30 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
31 GeometryModel & geom_model,
32 std::istream & stream,
33 const bool verbose = false)
34 {
35 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
36
37 // Read xml stream
38 using boost::property_tree::ptree;
39
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 ptree pt;
40
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 read_xml(stream, pt);
41
42 // Iterate over collision pairs
43
20/34
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 8 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 8 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 8 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 8 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 8 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 4776 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 4776 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 4776 times.
✓ Branch 32 taken 4776 times.
✓ Branch 33 taken 4776 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 4776 times.
✗ Branch 37 not taken.
✓ Branch 38 taken 4784 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 4784 times.
✗ Branch 42 not taken.
✓ Branch 43 taken 4776 times.
✓ Branch 44 taken 8 times.
✓ Branch 45 taken 4776 times.
✓ Branch 46 taken 8 times.
 9560 BOOST_FOREACH (const ptree::value_type & v, pt.get_child("robot"))
44 {
45
2/2
✓ Branch 1 taken 4610 times.
✓ Branch 2 taken 166 times.
 4776 if (v.first == "disable_collisions")
46 {
47
2/4
✓ Branch 1 taken 4610 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4610 times.
✗ Branch 5 not taken.
 4610 const std::string link1 = v.second.get<std::string>("<xmlattr>.link1");
48
2/4
✓ Branch 1 taken 4610 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4610 times.
✗ Branch 5 not taken.
 4610 const std::string link2 = v.second.get<std::string>("<xmlattr>.link2");
49
50 // Check first if the two bodies exist in model
51
8/10
✓ Branch 1 taken 4610 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1684 times.
✓ Branch 4 taken 2926 times.
✓ Branch 6 taken 1684 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 385 times.
✓ Branch 9 taken 1299 times.
✓ Branch 10 taken 3311 times.
✓ Branch 11 taken 1299 times.
 4610 if (!model.existBodyName(link1) || !model.existBodyName(link2))
52 {
53
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3311 times.
 3311 if (verbose)
54 std::cout << "It seems that " << link1 << " or " << link2
55 ✗ << " do not exist in model. Skip." << std::endl;
56 3311 continue;
57 }
58
59
1/2
✓ Branch 1 taken 1299 times.
✗ Branch 2 not taken.
 1299 const typename Model::FrameIndex frame_id1 = model.getBodyId(link1);
60
1/2
✓ Branch 1 taken 1299 times.
✗ Branch 2 not taken.
 1299 const typename Model::FrameIndex frame_id2 = model.getBodyId(link2);
61
62 // Malformed SRDF
63
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1299 times.
 1299 if (frame_id1 == frame_id2)
64 {
65 ✗ if (verbose)
66 ✗ std::cout << "Cannot disable collision between " << link1 << " and " << link2
67 ✗ << std::endl;
68 ✗ continue;
69 }
70
71 typedef GeometryModel::CollisionPairVector CollisionPairVector;
72 1299 bool didRemove = false;
73 1299 for (CollisionPairVector::const_iterator cp_iterator =
74 2598 geom_model.collisionPairs.begin();
75
2/2
✓ Branch 2 taken 590180 times.
✓ Branch 3 taken 1299 times.
 591479 cp_iterator != geom_model.collisionPairs.end();)
76 {
77 590180 const CollisionPair & cp = *cp_iterator;
78
1/2
✓ Branch 1 taken 590180 times.
✗ Branch 2 not taken.
 590180 const PairIndex cp_index = geom_model.findCollisionPair(cp);
79 590180 const bool remove =
80 590180 ((geom_model.geometryObjects[cp.first].parentFrame == frame_id1)
81
2/2
✓ Branch 1 taken 12526 times.
✓ Branch 2 taken 720 times.
 13246 && (geom_model.geometryObjects[cp.second].parentFrame == frame_id2))
82
4/4
✓ Branch 0 taken 13246 times.
✓ Branch 1 taken 576934 times.
✓ Branch 3 taken 12719 times.
✓ Branch 4 taken 576741 times.
 616145 || ((geom_model.geometryObjects[cp.second].parentFrame == frame_id1)
83
2/2
✓ Branch 1 taken 468 times.
✓ Branch 2 taken 12251 times.
 12719 && (geom_model.geometryObjects[cp.first].parentFrame == frame_id2));
84
85
2/2
✓ Branch 0 taken 1188 times.
✓ Branch 1 taken 588992 times.
 590180 if (remove)
86 {
87
1/2
✓ Branch 1 taken 1188 times.
✗ Branch 2 not taken.
 1188 geom_model.removeCollisionPair(cp);
88 1188 cp_iterator = geom_model.collisionPairs.begin() + (long)cp_index;
89 1188 didRemove = true;
90 }
91 else
92 {
93 588992 ++cp_iterator;
94 }
95 }
96
3/4
✓ Branch 0 taken 1188 times.
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1188 times.
 1299 if (didRemove && verbose)
97 ✗ std::cout << "Remove collision pair (" << link1 << "," << link2 << ")" << std::endl;
98
4/4
✓ Branch 1 taken 1299 times.
✓ Branch 2 taken 3311 times.
✓ Branch 4 taken 1299 times.
✓ Branch 5 taken 3311 times.
 7921 }
99 } // BOOST_FOREACH
100 8 }
101 } // namespace details
102
103 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
104 8 void removeCollisionPairs(
105 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
106 GeometryModel & geom_model,
107 const std::string & filename,
108 const bool verbose)
109 {
110 // Check extension
111
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 const std::string extension = filename.substr(filename.find_last_of('.') + 1);
112
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8 times.
 8 if (extension != "srdf")
113 {
114 ✗ const std::string exception_message(filename + " does not have the right extension.");
115 ✗ throw std::invalid_argument(exception_message);
116 }
117
118 // Open file
119
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 std::ifstream srdf_stream(filename.c_str());
120
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8 times.
 8 if (!srdf_stream.is_open())
121 {
122 ✗ const std::string exception_message(filename + " does not seem to be a valid file.");
123 ✗ throw std::invalid_argument(exception_message);
124 }
125
126
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 details::removeCollisionPairs(model, geom_model, srdf_stream, verbose);
127 8 }
128
129 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
130 ✗ void removeCollisionPairsFromXML(
131 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
132 GeometryModel & geom_model,
133 const std::string & xmlString,
134 const bool verbose)
135 {
136 ✗ std::istringstream srdf_stream(xmlString);
137 ✗ details::removeCollisionPairs(model, geom_model, srdf_stream, verbose);
138 }
139
140 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
141 1 bool loadRotorParameters(
142 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
143 const std::string & filename,
144 const bool verbose)
145 {
146 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
147 typedef typename Model::JointModel JointModel;
148
149 // Check extension
150
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 const std::string extension = filename.substr(filename.find_last_of('.') + 1);
151
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (extension != "srdf")
152 {
153 ✗ const std::string exception_message(filename + " does not have the right extension.");
154 ✗ throw std::invalid_argument(exception_message);
155 }
156
157 // Open file
158
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 std::ifstream srdf_stream(filename.c_str());
159
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (!srdf_stream.is_open())
160 {
161 ✗ const std::string exception_message(filename + " does not seem to be a valid file.");
162 ✗ throw std::invalid_argument(exception_message);
163 }
164
165 // Read xml stream
166 using boost::property_tree::ptree;
167
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ptree pt;
168
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 read_xml(srdf_stream, pt);
169
170 // Iterate over all tags directly children of robot
171
18/34
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 4 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 4 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 4 times.
✓ Branch 32 taken 3 times.
✓ Branch 33 taken 3 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 3 times.
✗ Branch 37 not taken.
✓ Branch 38 taken 4 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 4 times.
✗ Branch 42 not taken.
✓ Branch 43 taken 4 times.
✗ Branch 44 not taken.
✓ Branch 45 taken 4 times.
✗ Branch 46 not taken.
 7 BOOST_FOREACH (const ptree::value_type & v, pt.get_child("robot"))
172 {
173 // if we encounter a tag rotor_params
174
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 3 times.
 4 if (v.first == "rotor_params")
175 {
176 // Iterate over all the joint tags
177
18/30
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 29 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 29 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 29 times.
✓ Branch 25 taken 29 times.
✓ Branch 26 taken 29 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 29 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 30 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 30 times.
✗ Branch 35 not taken.
✓ Branch 36 taken 29 times.
✓ Branch 37 taken 1 times.
✓ Branch 38 taken 29 times.
✓ Branch 39 taken 1 times.
 59 BOOST_FOREACH (const ptree::value_type & joint, v.second)
178 {
179
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
 29 if (joint.first == "joint")
180 {
181
2/4
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29 times.
✗ Branch 5 not taken.
 29 const std::string joint_name = joint.second.get<std::string>("<xmlattr>.name");
182
2/4
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29 times.
✗ Branch 5 not taken.
 29 const Scalar rotor_inertia = (Scalar)joint.second.get<double>("<xmlattr>.mass");
183 const Scalar rotor_gear_ratio =
184
2/4
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29 times.
✗ Branch 5 not taken.
 29 (Scalar)joint.second.get<double>("<xmlattr>.gear_ratio");
185
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 29 times.
 29 if (verbose)
186 {
187 ✗ std::cout << "(" << joint_name << " , " << rotor_inertia << " , "
188 ✗ << rotor_gear_ratio << ")" << std::endl;
189 }
190 // Search in model the joint and its config id
191
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
 29 typename Model::JointIndex joint_id = model.getJointId(joint_name);
192
193
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
 29 if (joint_id != model.joints.size()) // != model.njoints
194 {
195 29 const JointModel & joint = model.joints[joint_id];
196
2/6
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 29 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 29 PINOCCHIO_CHECK_INPUT_ARGUMENT(joint.nv() == 1);
197
198
2/4
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29 times.
✗ Branch 5 not taken.
 29 model.armature[joint.idx_v()] +=
199 29 rotor_inertia * rotor_gear_ratio * rotor_gear_ratio;
200
2/4
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29 times.
✗ Branch 5 not taken.
 29 model.rotorInertia(joint.idx_v()) = rotor_inertia;
201
2/4
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29 times.
✗ Branch 5 not taken.
 29 model.rotorGearRatio(joint.idx_v()) = rotor_gear_ratio; // joint with 1 dof
202 }
203 else
204 {
205 ✗ if (verbose)
206 ✗ std::cout << "The Joint " << joint_name << " was not found in model" << std::endl;
207 }
208 29 }
209 }
210 1 return true;
211 }
212 }
213 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "no rotor params found in the SRDF file");
214 return false; // warning : uninitialized vector is returned
215 1 }
216
217 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
218 struct LoadReferenceConfigurationStep
219 : fusion::JointUnaryVisitorBase<
220 LoadReferenceConfigurationStep<Scalar, Options, JointCollectionTpl>>
221 {
222 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
223 typedef typename Model::ConfigVectorType ConfigVectorType;
224
225 typedef boost::fusion::
226 vector<const std::string &, const ConfigVectorType &, ConfigVectorType &>
227 ArgsType;
228
229 template<typename JointModel>
230 552 static void algo(
231 const JointModelBase<JointModel> & joint,
232 const std::string & joint_name,
233 const ConfigVectorType & fromXML,
234 ConfigVectorType & config)
235 {
236 552 algo_impl(joint.derived(), joint_name, fromXML, config);
237 }
238
239 private:
240 template<int axis>
241 2 static void algo_impl(
242 const JointModelRevoluteUnboundedTpl<Scalar, Options, axis> & joint,
243 const std::string & joint_name,
244 const ConfigVectorType & fromXML,
245 ConfigVectorType & config)
246 {
247 typedef JointModelRevoluteUnboundedTpl<Scalar, Options, axis> JointModelRUB;
248 PINOCCHIO_STATIC_ASSERT(
249 JointModelRUB::NQ == 2, JOINT_MODEL_REVOLUTE_SHOULD_HAVE_2_PARAMETERS);
250
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 2 if (fromXML.size() != 1)
251 ✗ std::cerr << "Could not read joint config (" << joint_name << " , " << fromXML.transpose()
252 ✗ << ")" << std::endl;
253 else
254 {
255 2 SINCOS(fromXML[0], &config[joint.idx_q() + 1], &config[joint.idx_q() + 0]);
256 }
257 }
258
259 template<typename JointModel>
260 550 static void algo_impl(
261 const JointModel & joint,
262 const std::string & joint_name,
263 const ConfigVectorType & fromXML,
264 ConfigVectorType & config)
265 {
266
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 275 times.
 550 if (joint.nq() != fromXML.size())
267 ✗ std::cerr << "Could not read joint config (" << joint_name << " , " << fromXML.transpose()
268 ✗ << ")" << std::endl;
269 else
270
1/2
✓ Branch 4 taken 275 times.
✗ Branch 5 not taken.
 550 config.segment(joint.idx_q(), joint.nq()) = fromXML;
271 }
272 };
273
274 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
275 8 void loadReferenceConfigurations(
276 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
277 const std::string & filename,
278 const bool verbose)
279 {
280 // Check extension
281
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 const std::string extension = filename.substr(filename.find_last_of('.') + 1);
282
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8 times.
 8 if (extension != "srdf")
283 {
284 ✗ const std::string exception_message(filename + " does not have the right extension.");
285 ✗ throw std::invalid_argument(exception_message);
286 }
287
288 // Open file
289
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 std::ifstream srdf_stream(filename.c_str());
290
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8 times.
 8 if (!srdf_stream.is_open())
291 {
292 ✗ const std::string exception_message(filename + " does not seem to be a valid file.");
293 ✗ throw std::invalid_argument(exception_message);
294 }
295
296
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 loadReferenceConfigurationsFromXML(model, srdf_stream, verbose);
297 8 }
298
299 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
300 10 void loadReferenceConfigurationsFromXML(
301 ModelTpl<Scalar, Options, JointCollectionTpl> & model,
302 std::istream & xmlStream,
303 const bool verbose)
304 {
305 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
306 typedef typename Model::JointModel JointModel;
307
308 // Read xml stream
309 using boost::property_tree::ptree;
310
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 ptree pt;
311
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 read_xml(xmlStream, pt);
312
313 // Iterate over all tags directly children of robot
314
20/34
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 10 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 10 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 10 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 4052 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 4052 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 4052 times.
✓ Branch 32 taken 4052 times.
✓ Branch 33 taken 4052 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 4052 times.
✗ Branch 37 not taken.
✓ Branch 38 taken 4062 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 4062 times.
✗ Branch 42 not taken.
✓ Branch 43 taken 4052 times.
✓ Branch 44 taken 10 times.
✓ Branch 45 taken 4052 times.
✓ Branch 46 taken 10 times.
 8114 BOOST_FOREACH (const ptree::value_type & v, pt.get_child("robot"))
315 {
316 // if we encounter a tag group_state
317
2/2
✓ Branch 1 taken 11 times.
✓ Branch 2 taken 4041 times.
 4052 if (v.first == "group_state")
318 {
319
2/4
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
 11 const std::string name = v.second.get<std::string>("<xmlattr>.name");
320
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 typename Model::ConfigVectorType ref_config(model.nq);
321
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 neutral(model, ref_config);
322
323 // Iterate over all the joint tags
324
18/30
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 11 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 11 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 11 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 440 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 440 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 440 times.
✓ Branch 25 taken 440 times.
✓ Branch 26 taken 440 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 440 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 451 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 451 times.
✗ Branch 35 not taken.
✓ Branch 36 taken 440 times.
✓ Branch 37 taken 11 times.
✓ Branch 38 taken 440 times.
✓ Branch 39 taken 11 times.
 891 BOOST_FOREACH (const ptree::value_type & joint_tag, v.second)
325 {
326
2/2
✓ Branch 1 taken 429 times.
✓ Branch 2 taken 11 times.
 440 if (joint_tag.first == "joint")
327 {
328
2/4
✓ Branch 1 taken 429 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 429 times.
✗ Branch 5 not taken.
 429 std::string joint_name = joint_tag.second.get<std::string>("<xmlattr>.name");
329
1/2
✓ Branch 1 taken 429 times.
✗ Branch 2 not taken.
 429 typename Model::JointIndex joint_id = model.getJointId(joint_name);
330
331 // Search in model the joint and its config id
332
2/2
✓ Branch 1 taken 276 times.
✓ Branch 2 taken 153 times.
 429 if (joint_id != model.joints.size()) // != model.njoints
333 {
334 276 const JointModel & joint = model.joints[joint_id];
335
2/4
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 276 times.
✗ Branch 5 not taken.
 276 typename Model::ConfigVectorType joint_config(joint.nq());
336
2/4
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 276 times.
✗ Branch 5 not taken.
 276 const std::string joint_val = joint_tag.second.get<std::string>("<xmlattr>.value");
337
1/2
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
 276 std::istringstream config_string(joint_val);
338
1/2
✓ Branch 2 taken 276 times.
✗ Branch 3 not taken.
 552 std::vector<double> config_vec(
339
1/2
✓ Branch 2 taken 276 times.
✗ Branch 3 not taken.
 552 (std::istream_iterator<double>(config_string)), std::istream_iterator<double>());
340
3/6
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 276 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 276 times.
✗ Branch 9 not taken.
 552 joint_config = Eigen::Map<Eigen::VectorXd>(
341 276 config_vec.data(), (Eigen::DenseIndex)config_vec.size());
342
343 typedef LoadReferenceConfigurationStep<Scalar, Options, JointCollectionTpl>
344 LoadReferenceConfigurationStep_t;
345
1/2
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
 276 LoadReferenceConfigurationStep_t::run(
346
1/2
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
 276 joint, typename LoadReferenceConfigurationStep_t::ArgsType(
347 joint_name, joint_config, ref_config));
348
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 276 times.
 276 if (verbose)
349 {
350 ✗ std::cout << "(" << joint_name << " , " << joint_config.transpose() << ")"
351 ✗ << std::endl;
352 }
353 276 }
354 else
355 {
356
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 153 times.
 153 if (verbose)
357 ✗ std::cout << "The Joint " << joint_name << " was not found in model" << std::endl;
358 }
359 429 }
360 }
361
362
3/6
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 11 times.
 11 if (!model.referenceConfigurations.insert(std::make_pair(name, ref_config)).second)
363 {
364 // Element already present...
365 ✗ if (verbose)
366 std::cout << "The reference configuration " << name
367 << " has been defined multiple times. "
368 ✗ << "Only the last instance of " << name << " is being used." << std::endl;
369 }
370 11 }
371 } // BOOST_FOREACH
372 10 }
373 } // namespace srdf
374 } // namespace pinocchio
375
376 #endif // ifndef __pinocchio_parser_srdf_hxx__
377