GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2017-2020 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 |
5 |
void removeCollisionPairs(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
30 |
GeometryModel & geom_model, |
||
31 |
std::istream & stream, |
||
32 |
const bool verbose = false) |
||
33 |
{ |
||
34 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
35 |
|||
36 |
// Read xml stream |
||
37 |
using boost::property_tree::ptree; |
||
38 |
✓✗ | 10 |
ptree pt; |
39 |
✓✗ | 5 |
read_xml(stream, pt); |
40 |
|||
41 |
// Iterate over collision pairs |
||
42 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✓ ✓✗✓✗ ✓✓✓✗ ✓✗ |
5885 |
BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot")) |
43 |
{ |
||
44 |
✓✓ | 2940 |
if (v.first == "disable_collisions") |
45 |
{ |
||
46 |
✓✗✓✗ |
2831 |
const std::string link1 = v.second.get<std::string>("<xmlattr>.link1"); |
47 |
✓✗✓✗ |
2831 |
const std::string link2 = v.second.get<std::string>("<xmlattr>.link2"); |
48 |
|||
49 |
// Check first if the two bodies exist in model |
||
50 |
✓✗✓✓ ✓✗✓✓ ✓✓ |
2831 |
if (!model.existBodyName(link1) || !model.existBodyName(link2)) |
51 |
{ |
||
52 |
✗✓ | 1892 |
if (verbose) |
53 |
std::cout << "It seems that " << link1 << " or " << link2 << " do not exist in model. Skip." << std::endl; |
||
54 |
1892 |
continue; |
|
55 |
} |
||
56 |
|||
57 |
✓✗ | 939 |
const typename Model::FrameIndex frame_id1 = model.getBodyId(link1); |
58 |
✓✗ | 939 |
const typename Model::FrameIndex frame_id2 = model.getBodyId(link2); |
59 |
|||
60 |
// Malformed SRDF |
||
61 |
✗✓ | 939 |
if (frame_id1 == frame_id2) |
62 |
{ |
||
63 |
if (verbose) |
||
64 |
std::cout << "Cannot disable collision between " << link1 << " and " << link2 << std::endl; |
||
65 |
continue; |
||
66 |
} |
||
67 |
|||
68 |
typedef GeometryModel::CollisionPairVector CollisionPairVector; |
||
69 |
939 |
bool didRemove = false; |
|
70 |
544139 |
for(CollisionPairVector::iterator _colPair = geom_model.collisionPairs.begin(); |
|
71 |
✓✓ | 544139 |
_colPair != geom_model.collisionPairs.end(); ) { |
72 |
543200 |
const CollisionPair& colPair (*_colPair); |
|
73 |
543200 |
bool remove = |
|
74 |
( |
||
75 |
543200 |
(geom_model.geometryObjects[colPair.first ].parentFrame == frame_id1) |
|
76 |
✓✓ | 11095 |
&& (geom_model.geometryObjects[colPair.second].parentFrame == frame_id2) |
77 |
✓✓ | 1086400 |
) || ( |
78 |
✓✓ | 542684 |
(geom_model.geometryObjects[colPair.second].parentFrame == frame_id1) |
79 |
✓✓ | 10910 |
&& (geom_model.geometryObjects[colPair.first ].parentFrame == frame_id2) |
80 |
); |
||
81 |
|||
82 |
✓✓ | 543200 |
if (remove) { |
83 |
✓✗ | 828 |
_colPair = geom_model.collisionPairs.erase(_colPair); |
84 |
828 |
didRemove = true; |
|
85 |
} else { |
||
86 |
542372 |
++_colPair; |
|
87 |
} |
||
88 |
} |
||
89 |
✓✓✗✓ |
939 |
if(didRemove && verbose) |
90 |
std::cout << "Remove collision pair (" << link1 << "," << link2 << ")" << std::endl; |
||
91 |
|||
92 |
} |
||
93 |
} // BOOST_FOREACH |
||
94 |
5 |
} |
|
95 |
} // namespace details |
||
96 |
|||
97 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
98 |
5 |
void removeCollisionPairs(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
99 |
GeometryModel & geom_model, |
||
100 |
const std::string & filename, |
||
101 |
const bool verbose) |
||
102 |
{ |
||
103 |
// Check extension |
||
104 |
✓✗ | 10 |
const std::string extension = filename.substr(filename.find_last_of('.')+1); |
105 |
✓✓✗✓ |
5 |
if (extension != "srdf") |
106 |
{ |
||
107 |
const std::string exception_message (filename + " does not have the right extension."); |
||
108 |
throw std::invalid_argument(exception_message); |
||
109 |
} |
||
110 |
|||
111 |
// Open file |
||
112 |
✓✗ | 10 |
std::ifstream srdf_stream(filename.c_str()); |
113 |
✓✗✗✓ |
5 |
if (! srdf_stream.is_open()) |
114 |
{ |
||
115 |
const std::string exception_message (filename + " does not seem to be a valid file."); |
||
116 |
throw std::invalid_argument(exception_message); |
||
117 |
} |
||
118 |
|||
119 |
✓✗ | 5 |
details::removeCollisionPairs(model, geom_model, srdf_stream, verbose); |
120 |
5 |
} |
|
121 |
|||
122 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
123 |
void removeCollisionPairsFromXML(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
||
124 |
GeometryModel & geom_model, |
||
125 |
const std::string & xmlString, |
||
126 |
const bool verbose) |
||
127 |
{ |
||
128 |
std::istringstream srdf_stream(xmlString); |
||
129 |
details::removeCollisionPairs(model, geom_model, srdf_stream, verbose); |
||
130 |
} |
||
131 |
|||
132 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
133 |
1 |
bool loadRotorParameters(ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
134 |
const std::string & filename, |
||
135 |
const bool verbose) |
||
136 |
{ |
||
137 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
138 |
typedef typename Model::JointModel JointModel; |
||
139 |
|||
140 |
// Check extension |
||
141 |
✓✗ | 2 |
const std::string extension = filename.substr(filename.find_last_of('.')+1); |
142 |
✓✗✗✓ |
1 |
if (extension != "srdf") |
143 |
{ |
||
144 |
const std::string exception_message (filename + " does not have the right extension."); |
||
145 |
throw std::invalid_argument(exception_message); |
||
146 |
} |
||
147 |
|||
148 |
// Open file |
||
149 |
✓✗ | 2 |
std::ifstream srdf_stream(filename.c_str()); |
150 |
✓✗✗✓ |
1 |
if (! srdf_stream.is_open()) |
151 |
{ |
||
152 |
const std::string exception_message (filename + " does not seem to be a valid file."); |
||
153 |
throw std::invalid_argument(exception_message); |
||
154 |
} |
||
155 |
|||
156 |
// Read xml stream |
||
157 |
using boost::property_tree::ptree; |
||
158 |
✓✗ | 1 |
ptree pt; |
159 |
✓✗ | 1 |
read_xml(srdf_stream, pt); |
160 |
|||
161 |
// Iterate over all tags directly children of robot |
||
162 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✗ ✓✗ |
7 |
BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot")) |
163 |
{ |
||
164 |
// if we encounter a tag rotor_params |
||
165 |
✓✓ | 4 |
if (v.first == "rotor_params") |
166 |
{ |
||
167 |
// Iterate over all the joint tags |
||
168 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✓ ✓✗✓✗ ✓✓✓✗ ✓✗ |
59 |
BOOST_FOREACH(const ptree::value_type & joint, v.second) |
169 |
{ |
||
170 |
✓✗ | 29 |
if (joint.first == "joint") |
171 |
{ |
||
172 |
✓✗✓✗ |
58 |
std::string joint_name = joint.second.get<std::string>("<xmlattr>.name"); |
173 |
✓✗✓✗ |
29 |
const Scalar rotor_mass = (Scalar)joint.second.get<double>("<xmlattr>.mass"); |
174 |
✓✗✓✗ |
29 |
const Scalar rotor_gr = (Scalar)joint.second.get<double>("<xmlattr>.gear_ratio"); |
175 |
✗✓ | 29 |
if (verbose) |
176 |
{ |
||
177 |
std::cout << "(" << joint_name << " , " << |
||
178 |
rotor_mass << " , " << rotor_gr << ")" << std::endl; |
||
179 |
} |
||
180 |
// Search in model the joint and its config id |
||
181 |
✓✗ | 29 |
typename Model::JointIndex joint_id = model.getJointId(joint_name); |
182 |
|||
183 |
✓✗ | 29 |
if (joint_id != model.joints.size()) // != model.njoints |
184 |
{ |
||
185 |
29 |
const JointModel & joint = model.joints[joint_id]; |
|
186 |
✓✗✗✓ ✗✗ |
29 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(joint.nv()==1); |
187 |
✓✗✓✗ |
29 |
model.rotorInertia(joint.idx_v()) = rotor_mass; |
188 |
✓✗✓✗ |
29 |
model.rotorGearRatio(joint.idx_v()) = rotor_gr; // joint with 1 dof |
189 |
} |
||
190 |
else |
||
191 |
{ |
||
192 |
if (verbose) std::cout << "The Joint " << joint_name << " was not found in model" << std::endl; |
||
193 |
} |
||
194 |
} |
||
195 |
} |
||
196 |
1 |
return true; |
|
197 |
} |
||
198 |
} |
||
199 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "no rotor params found in the SRDF file"); |
||
200 |
return false; // warning : uninitialized vector is returned |
||
201 |
} |
||
202 |
|||
203 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
204 |
struct LoadReferenceConfigurationStep |
||
205 |
: fusion::JointUnaryVisitorBase< LoadReferenceConfigurationStep<Scalar,Options,JointCollectionTpl> > |
||
206 |
{ |
||
207 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
208 |
typedef typename Model::ConfigVectorType ConfigVectorType; |
||
209 |
|||
210 |
typedef boost::fusion::vector<const std::string&, |
||
211 |
const ConfigVectorType&, |
||
212 |
ConfigVectorType&> ArgsType; |
||
213 |
|||
214 |
template<typename JointModel> |
||
215 |
366 |
static void algo(const JointModelBase<JointModel> & joint, |
|
216 |
const std::string& joint_name, |
||
217 |
const ConfigVectorType& fromXML, |
||
218 |
ConfigVectorType& config) |
||
219 |
{ |
||
220 |
366 |
_algo (joint.derived(), joint_name, fromXML, config); |
|
221 |
} |
||
222 |
|||
223 |
private: |
||
224 |
template<int axis> |
||
225 |
2 |
static void _algo (const JointModelRevoluteUnboundedTpl<Scalar,Options,axis> & joint, |
|
226 |
const std::string& joint_name, |
||
227 |
const ConfigVectorType& fromXML, |
||
228 |
ConfigVectorType& config) |
||
229 |
{ |
||
230 |
typedef JointModelRevoluteUnboundedTpl<Scalar,Options,axis> JointModelRUB; |
||
231 |
PINOCCHIO_STATIC_ASSERT(JointModelRUB::NQ == 2, JOINT_MODEL_REVOLUTE_SHOULD_HAVE_2_PARAMETERS); |
||
232 |
✗✓ | 2 |
if (fromXML.size() != 1) |
233 |
std::cerr << "Could not read joint config (" << joint_name << " , " << fromXML.transpose() << ")" << std::endl; |
||
234 |
else { |
||
235 |
2 |
SINCOS(fromXML[0], |
|
236 |
2 |
&config[joint.idx_q()+1], |
|
237 |
2 |
&config[joint.idx_q()+0]); |
|
238 |
} |
||
239 |
} |
||
240 |
|||
241 |
template<typename JointModel> |
||
242 |
364 |
static void _algo (const JointModel & joint, |
|
243 |
const std::string& joint_name, |
||
244 |
const ConfigVectorType& fromXML, |
||
245 |
ConfigVectorType& config) |
||
246 |
{ |
||
247 |
✗✓ | 364 |
if (joint.nq() != fromXML.size()) |
248 |
std::cerr << "Could not read joint config (" << joint_name << " , " << fromXML.transpose() << ")" << std::endl; |
||
249 |
else |
||
250 |
✓✗ | 364 |
config.segment(joint.idx_q(),joint.nq()) = fromXML; |
251 |
} |
||
252 |
}; |
||
253 |
|||
254 |
|||
255 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
256 |
void |
||
257 |
5 |
loadReferenceConfigurations(ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
258 |
const std::string & filename, |
||
259 |
const bool verbose) |
||
260 |
{ |
||
261 |
// Check extension |
||
262 |
✓✗ | 10 |
const std::string extension = filename.substr(filename.find_last_of('.')+1); |
263 |
✓✓✗✓ |
5 |
if (extension != "srdf") |
264 |
{ |
||
265 |
const std::string exception_message (filename + " does not have the right extension."); |
||
266 |
throw std::invalid_argument(exception_message); |
||
267 |
} |
||
268 |
|||
269 |
// Open file |
||
270 |
✓✗ | 10 |
std::ifstream srdf_stream(filename.c_str()); |
271 |
✓✗✗✓ |
5 |
if (! srdf_stream.is_open()) |
272 |
{ |
||
273 |
const std::string exception_message (filename + " does not seem to be a valid file."); |
||
274 |
throw std::invalid_argument(exception_message); |
||
275 |
} |
||
276 |
|||
277 |
✓✗ | 5 |
loadReferenceConfigurationsFromXML (model, srdf_stream, verbose); |
278 |
5 |
} |
|
279 |
|||
280 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
281 |
void |
||
282 |
7 |
loadReferenceConfigurationsFromXML(ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
283 |
std::istream & xmlStream, |
||
284 |
const bool verbose) |
||
285 |
{ |
||
286 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
287 |
typedef typename Model::JointModel JointModel; |
||
288 |
|||
289 |
// Read xml stream |
||
290 |
using boost::property_tree::ptree; |
||
291 |
✓✗ | 14 |
ptree pt; |
292 |
✓✗ | 7 |
read_xml(xmlStream, pt); |
293 |
|||
294 |
// Iterate over all tags directly children of robot |
||
295 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✓ ✓✗✓✗ ✓✓✓✗ ✓✗ |
4679 |
BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot")) |
296 |
{ |
||
297 |
// if we encounter a tag group_state |
||
298 |
✓✓ | 2336 |
if (v.first == "group_state") |
299 |
{ |
||
300 |
✓✗✓✗ |
16 |
const std::string name = v.second.get<std::string>("<xmlattr>.name"); |
301 |
✓✗ | 16 |
typename Model::ConfigVectorType ref_config (model.nq); |
302 |
✓✗ | 8 |
neutral (model, ref_config); |
303 |
|||
304 |
// Iterate over all the joint tags |
||
305 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✓ ✓✗✓✗ ✓✓✓✗ ✓✗ |
576 |
BOOST_FOREACH(const ptree::value_type & joint_tag, v.second) |
306 |
{ |
||
307 |
✓✓ | 284 |
if (joint_tag.first == "joint") |
308 |
{ |
||
309 |
✓✗✓✗ |
552 |
std::string joint_name = joint_tag.second.get<std::string>("<xmlattr>.name"); |
310 |
✓✗ | 276 |
typename Model::JointIndex joint_id = model.getJointId(joint_name); |
311 |
|||
312 |
// Search in model the joint and its config id |
||
313 |
✓✓ | 276 |
if (joint_id != model.joints.size()) // != model.njoints |
314 |
{ |
||
315 |
183 |
const JointModel & joint = model.joints[joint_id]; |
|
316 |
✓✗✓✗ |
366 |
typename Model::ConfigVectorType joint_config(joint.nq()); |
317 |
✓✗✓✗ |
366 |
const std::string joint_val = joint_tag.second.get<std::string>("<xmlattr>.value"); |
318 |
✓✗ | 366 |
std::istringstream config_string(joint_val); |
319 |
✓✗✓✗ |
366 |
std::vector<double> config_vec((std::istream_iterator<double>(config_string)), std::istream_iterator<double>()); |
320 |
✓✗✓✗ ✓✗ |
183 |
joint_config = Eigen::Map<Eigen::VectorXd>(config_vec.data(), (Eigen::DenseIndex)config_vec.size()); |
321 |
|||
322 |
typedef LoadReferenceConfigurationStep<Scalar, Options, JointCollectionTpl> LoadReferenceConfigurationStep_t; |
||
323 |
✓✗✓✗ |
183 |
LoadReferenceConfigurationStep_t::run (joint, |
324 |
typename LoadReferenceConfigurationStep_t::ArgsType (joint_name, joint_config, ref_config)); |
||
325 |
✗✓ | 183 |
if (verbose) |
326 |
{ |
||
327 |
std::cout << "(" << joint_name << " , " << joint_config.transpose() << ")" << std::endl; |
||
328 |
} |
||
329 |
} |
||
330 |
else |
||
331 |
{ |
||
332 |
✗✓✗✗ ✗✗✗✗ ✗✗ |
93 |
if (verbose) std::cout << "The Joint " << joint_name << " was not found in model" << std::endl; |
333 |
} |
||
334 |
|||
335 |
} |
||
336 |
} |
||
337 |
|||
338 |
✓✗✓✗ ✗✓ |
8 |
if ( !model.referenceConfigurations.insert(std::make_pair(name, ref_config)).second) |
339 |
{ |
||
340 |
// Element already present... |
||
341 |
if (verbose) std::cout << "The reference configuration " |
||
342 |
<< name << " has been defined multiple times. " |
||
343 |
<<"Only the last instance of "<<name<<" is being used." |
||
344 |
<<std::endl; |
||
345 |
} |
||
346 |
} |
||
347 |
} // BOOST_FOREACH |
||
348 |
7 |
} |
|
349 |
} |
||
350 |
} // namespace pinocchio |
||
351 |
|||
352 |
#endif // ifndef __pinocchio_parser_srdf_hxx__ |
Generated by: GCOVR (Version 4.2) |