Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/parsers/srdf.hxx	Lines:	99	129	76.7 %
Date:	2024-04-26 13:14:21	Branches:	207	498	41.6 %


//
// Copyright (c) 2017-2020 CNRS INRIA
//

#ifndef __pinocchio_parser_srdf_hxx__
#define __pinocchio_parser_srdf_hxx__

#include "pinocchio/parsers/srdf.hpp"

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/geometry.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include <iostream>

// Read XML file with boost
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <fstream>
#include <sstream>
#include <boost/foreach.hpp>

namespace pinocchio
{
  namespace srdf
  {
    namespace details
    {
      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
      void removeCollisionPairs(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                GeometryModel & geom_model,
                                std::istream & stream,
                                const bool verbose = false)
      {
        typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;

        // Read xml stream
        using boost::property_tree::ptree;
        ptree pt;
        read_xml(stream, pt);

        // Iterate over collision pairs
        BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot"))
        {
          if (v.first == "disable_collisions")
          {
            const std::string link1 = v.second.get<std::string>("<xmlattr>.link1");
            const std::string link2 = v.second.get<std::string>("<xmlattr>.link2");

            // Check first if the two bodies exist in model
            if (!model.existBodyName(link1) || !model.existBodyName(link2))
            {
              if (verbose)
                std::cout << "It seems that " << link1 << " or " << link2 << " do not exist in model. Skip." << std::endl;
              continue;
            }

            const typename Model::FrameIndex frame_id1 = model.getBodyId(link1);
            const typename Model::FrameIndex frame_id2 = model.getBodyId(link2);

            // Malformed SRDF
            if (frame_id1 == frame_id2)
            {
              if (verbose)
                std::cout << "Cannot disable collision between " << link1 << " and " << link2 << std::endl;
              continue;
            }

            typedef GeometryModel::CollisionPairVector CollisionPairVector;
            bool didRemove = false;
            for(CollisionPairVector::iterator _colPair = geom_model.collisionPairs.begin();
                _colPair != geom_model.collisionPairs.end(); ) {
              const CollisionPair& colPair (*_colPair);
              bool remove =
              (
               (geom_model.geometryObjects[colPair.first ].parentFrame == frame_id1)
               && (geom_model.geometryObjects[colPair.second].parentFrame == frame_id2)
               ) || (
                     (geom_model.geometryObjects[colPair.second].parentFrame == frame_id1)
                     && (geom_model.geometryObjects[colPair.first ].parentFrame == frame_id2)
                     );

              if (remove) {
                _colPair = geom_model.collisionPairs.erase(_colPair);
                didRemove = true;
              } else {
                ++_colPair;
              }
            }
            if(didRemove && verbose)
              std::cout << "Remove collision pair (" << link1 << "," << link2 << ")" << std::endl;

          }
        } // BOOST_FOREACH
      }
    } // namespace details

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    void removeCollisionPairs(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                              GeometryModel & geom_model,
                              const std::string & filename,
                              const bool verbose)
    {
      // Check extension
      const std::string extension = filename.substr(filename.find_last_of('.')+1);
      if (extension != "srdf")
      {
        const std::string exception_message (filename + " does not have the right extension.");
        throw std::invalid_argument(exception_message);
      }

      // Open file
      std::ifstream srdf_stream(filename.c_str());
      if (! srdf_stream.is_open())
      {
        const std::string exception_message (filename + " does not seem to be a valid file.");
        throw std::invalid_argument(exception_message);
      }

      details::removeCollisionPairs(model, geom_model, srdf_stream, verbose);
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    void removeCollisionPairsFromXML(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                     GeometryModel & geom_model,
                                     const std::string & xmlString,
                                     const bool verbose)
    {
      std::istringstream srdf_stream(xmlString);
      details::removeCollisionPairs(model, geom_model, srdf_stream, verbose);
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    bool loadRotorParameters(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                             const std::string & filename,
                             const bool verbose)
    {
      typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
      typedef typename Model::JointModel JointModel;

      // Check extension
      const std::string extension = filename.substr(filename.find_last_of('.')+1);
      if (extension != "srdf")
      {
        const std::string exception_message (filename + " does not have the right extension.");
        throw std::invalid_argument(exception_message);
      }

      // Open file
      std::ifstream srdf_stream(filename.c_str());
      if (! srdf_stream.is_open())
      {
        const std::string exception_message (filename + " does not seem to be a valid file.");
        throw std::invalid_argument(exception_message);
      }

      // Read xml stream
      using boost::property_tree::ptree;
      ptree pt;
      read_xml(srdf_stream, pt);

      // Iterate over all tags directly children of robot
      BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot"))
      {
        // if we encounter a tag rotor_params
        if (v.first == "rotor_params")
        {
          // Iterate over all the joint tags
          BOOST_FOREACH(const ptree::value_type & joint, v.second)
          {
            if (joint.first == "joint")
            {
              std::string joint_name = joint.second.get<std::string>("<xmlattr>.name");
              const Scalar rotor_mass = (Scalar)joint.second.get<double>("<xmlattr>.mass");
              const Scalar rotor_gr = (Scalar)joint.second.get<double>("<xmlattr>.gear_ratio");
              if (verbose)
              {
                std::cout << "(" << joint_name << " , " <<
                rotor_mass << " , " << rotor_gr << ")" << std::endl;
              }
              // Search in model the joint and its config id
              typename Model::JointIndex joint_id = model.getJointId(joint_name);

              if (joint_id != model.joints.size()) // != model.njoints
              {
                const JointModel & joint = model.joints[joint_id];
                PINOCCHIO_CHECK_INPUT_ARGUMENT(joint.nv()==1);
                model.rotorInertia(joint.idx_v()) = rotor_mass;
                model.rotorGearRatio(joint.idx_v()) = rotor_gr;  // joint with 1 dof
              }
              else
              {
                if (verbose) std::cout << "The Joint " << joint_name << " was not found in model" << std::endl;
              }
            }
          }
          return true;
        }
      }
      PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "no rotor params found in the SRDF file");
      return false; // warning : uninitialized vector is returned
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    struct LoadReferenceConfigurationStep
    : fusion::JointUnaryVisitorBase< LoadReferenceConfigurationStep<Scalar,Options,JointCollectionTpl> >
    {
      typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
      typedef typename Model::ConfigVectorType ConfigVectorType;

      typedef boost::fusion::vector<const std::string&,
                                    const ConfigVectorType&,
                                    ConfigVectorType&> ArgsType;

      template<typename JointModel>
      static void algo(const JointModelBase<JointModel> & joint,
                       const std::string& joint_name,
                       const ConfigVectorType& fromXML,
                       ConfigVectorType& config)
      {
        _algo (joint.derived(), joint_name, fromXML, config);
      }

      private:
      template<int axis>
      static void _algo (const JointModelRevoluteUnboundedTpl<Scalar,Options,axis> & joint,
                         const std::string& joint_name,
                         const ConfigVectorType& fromXML,
                         ConfigVectorType& config)
      {
        typedef JointModelRevoluteUnboundedTpl<Scalar,Options,axis> JointModelRUB;
        PINOCCHIO_STATIC_ASSERT(JointModelRUB::NQ == 2, JOINT_MODEL_REVOLUTE_SHOULD_HAVE_2_PARAMETERS);
        if (fromXML.size() != 1)
          std::cerr << "Could not read joint config (" << joint_name << " , " << fromXML.transpose() << ")" << std::endl;
        else {
          SINCOS(fromXML[0],
                 &config[joint.idx_q()+1],
                 &config[joint.idx_q()+0]);
        }
      }

      template<typename JointModel>
      static void _algo (const JointModel & joint,
                         const std::string& joint_name,
                         const ConfigVectorType& fromXML,
                         ConfigVectorType& config)
      {
        if (joint.nq() != fromXML.size())
          std::cerr << "Could not read joint config (" << joint_name << " , " << fromXML.transpose() << ")" << std::endl;
        else
          config.segment(joint.idx_q(),joint.nq()) = fromXML;
      }
    };


    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    void
    loadReferenceConfigurations(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                const std::string & filename,
                                const bool verbose)
    {
      // Check extension
      const std::string extension = filename.substr(filename.find_last_of('.')+1);
      if (extension != "srdf")
      {
        const std::string exception_message (filename + " does not have the right extension.");
        throw std::invalid_argument(exception_message);
      }

      // Open file
      std::ifstream srdf_stream(filename.c_str());
      if (! srdf_stream.is_open())
      {
        const std::string exception_message (filename + " does not seem to be a valid file.");
        throw std::invalid_argument(exception_message);
      }

      loadReferenceConfigurationsFromXML (model, srdf_stream, verbose);
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    void
    loadReferenceConfigurationsFromXML(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                       std::istream & xmlStream,
                                       const bool verbose)
    {
      typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
      typedef typename Model::JointModel JointModel;

      // Read xml stream
      using boost::property_tree::ptree;
      ptree pt;
      read_xml(xmlStream, pt);

      // Iterate over all tags directly children of robot
      BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot"))
      {
        // if we encounter a tag group_state
        if (v.first == "group_state")
        {
          const std::string name = v.second.get<std::string>("<xmlattr>.name");
          typename Model::ConfigVectorType ref_config (model.nq);
          neutral (model, ref_config);

          // Iterate over all the joint tags
          BOOST_FOREACH(const ptree::value_type & joint_tag, v.second)
          {
            if (joint_tag.first == "joint")
            {
              std::string joint_name = joint_tag.second.get<std::string>("<xmlattr>.name");
              typename Model::JointIndex joint_id = model.getJointId(joint_name);

              // Search in model the joint and its config id
              if (joint_id != model.joints.size()) // != model.njoints
              {
                const JointModel & joint = model.joints[joint_id];
                typename Model::ConfigVectorType joint_config(joint.nq());
                const std::string joint_val = joint_tag.second.get<std::string>("<xmlattr>.value");
                std::istringstream config_string(joint_val);
                std::vector<double> config_vec((std::istream_iterator<double>(config_string)), std::istream_iterator<double>());
                joint_config = Eigen::Map<Eigen::VectorXd>(config_vec.data(), (Eigen::DenseIndex)config_vec.size());

                typedef LoadReferenceConfigurationStep<Scalar, Options, JointCollectionTpl> LoadReferenceConfigurationStep_t;
                LoadReferenceConfigurationStep_t::run (joint,
                    typename LoadReferenceConfigurationStep_t::ArgsType (joint_name, joint_config, ref_config));
                if (verbose)
                {
                  std::cout << "(" << joint_name << " , " << joint_config.transpose() << ")" << std::endl;
                }
              }
              else
              {
                if (verbose) std::cout << "The Joint " << joint_name << " was not found in model" << std::endl;
              }

            }
          }

          if ( !model.referenceConfigurations.insert(std::make_pair(name, ref_config)).second)
          {
            //  Element already present...
            if (verbose) std::cout << "The reference configuration "
                                   << name << " has been defined multiple times. "
                                   <<"Only the last instance of "<<name<<" is being used."
                                   <<std::endl;
          }
        }
      } // BOOST_FOREACH
    }
  }
} // namespace pinocchio

#endif // ifndef __pinocchio_parser_srdf_hxx__


Generated by: GCOVR (Version 4.2)

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__