GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/parsers/srdf.hxx Lines: 99 129 76.7 %
Date: 2023-08-09 08:43:58 Branches: 207 498 41.6 %

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__