GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/parsers/urdf.hpp Lines: 3 3 100.0 %
Date: 2023-08-09 08:43:58 Branches: 2 4 50.0 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2021 CNRS INRIA
3
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4
//
5
6
#ifndef __pinocchio_parsers_urdf_hpp__
7
#define __pinocchio_parsers_urdf_hpp__
8
9
#include "pinocchio/multibody/model.hpp"
10
#include "pinocchio/multibody/geometry.hpp"
11
12
#ifdef PINOCCHIO_WITH_CXX11_SUPPORT
13
#include <memory>
14
#endif
15
16
/// \cond
17
// forward declaration of the unique type from urdfdom which is expose.
18
namespace urdf {
19
  class ModelInterface;
20
}
21
22
namespace hpp
23
{
24
  namespace fcl
25
  {
26
    class MeshLoader;
27
#ifdef PINOCCHIO_HPPFCL_USE_STD_SHARED_PTR
28
    typedef std::shared_ptr<MeshLoader> MeshLoaderPtr;
29
#else
30
    typedef boost::shared_ptr<MeshLoader> MeshLoaderPtr;
31
#endif
32
  }
33
}
34
/// \endcond
35
36
namespace pinocchio
37
{
38
  namespace urdf
39
  {
40
41
    ///
42
    /// \brief Build the model from a URDF file with a particular joint as root of the model tree inside
43
    /// the model given as reference argument.
44
    ///
45
    /// \param[in] filename The URDF complete file path.
46
    /// \param[in] rootJoint The joint at the root of the model tree.
47
    /// \param[in] verbose Print parsing info.
48
    /// \param[out] model Reference model where to put the parsed information.
49
    /// \return Return the reference on argument model for convenience.
50
    ///
51
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
52
    ModelTpl<Scalar,Options,JointCollectionTpl> &
53
    buildModel(const std::string & filename,
54
               const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & rootJoint,
55
               ModelTpl<Scalar,Options,JointCollectionTpl> & model,
56
               const bool verbose = false);
57
58
59
    ///
60
    /// \brief Build the model from a URDF file with a fixed joint as root of the model tree.
61
    ///
62
    /// \param[in] filename The URDF complete file path.
63
    /// \param[in] verbose Print parsing info.
64
    /// \param[out] model Reference model where to put the parsed information.
65
    /// \return Return the reference on argument model for convenience.
66
    ///
67
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
68
    ModelTpl<Scalar,Options,JointCollectionTpl> &
69
    buildModel(const std::string & filename,
70
               ModelTpl<Scalar,Options,JointCollectionTpl> & model,
71
               const bool verbose = false);
72
73
    ///
74
    /// \brief Build the model from a URDF model with a particular joint as root of the model tree inside
75
    /// the model given as reference argument.
76
    ///
77
    /// \param[in] urdfTree the tree build from the URDF
78
    /// \param[in] rootJoint The joint at the root of the model tree.
79
    /// \param[in] verbose Print parsing info.
80
    /// \param[out] model Reference model where to put the parsed information.
81
    /// \return Return the reference on argument model for convenience.
82
    ///
83
    /// \note urdfTree can be build from ::urdf::parseURDF
84
    ///       or ::urdf::parseURDFFile
85
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
86
    ModelTpl<Scalar,Options,JointCollectionTpl> &
87
    buildModel(const boost::shared_ptr< ::urdf::ModelInterface> urdfTree,
88
               const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & rootJoint,
89
               ModelTpl<Scalar,Options,JointCollectionTpl> & model,
90
               const bool verbose = false);
91
92
    ///
93
    /// \brief Build the model from a URDF model
94
    ///
95
    /// \param[in] urdfTree the tree build from the URDF
96
    /// \param[in] verbose Print parsing info.
97
    /// \param[out] model Reference model where to put the parsed information.
98
    /// \return Return the reference on argument model for convenience.
99
    ///
100
    /// \note urdfTree can be build from ::urdf::parseURDF
101
    ///       or ::urdf::parseURDFFile
102
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
103
    ModelTpl<Scalar,Options,JointCollectionTpl> &
104
    buildModel(const boost::shared_ptr< ::urdf::ModelInterface> urdfTree,
105
               ModelTpl<Scalar,Options,JointCollectionTpl> & model,
106
               const bool verbose = false);
107
108
#ifdef PINOCCHIO_WITH_CXX11_SUPPORT
109
    /// copydoc buildModel<Scalar,Options,JointCollectionTpl>(const boost::shared_ptr< ::urdf::ModelInterface>, const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel &, ModelTpl<Scalar,Options,JointCollectionTpl> &, const bool verbose)
110
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
111
    ModelTpl<Scalar,Options,JointCollectionTpl> &
112
    buildModel(const std::shared_ptr< ::urdf::ModelInterface> urdfTree,
113
               const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & rootJoint,
114
               ModelTpl<Scalar,Options,JointCollectionTpl> & model,
115
               const bool verbose = false);
116
117
    /// copydoc buildModel<Scalar,Options,JointCollectionTpl>(const boost::shared_ptr< ::urdf::ModelInterface>, ModelTpl<Scalar,Options,JointCollectionTpl> &, const bool verbose)
118
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
119
    ModelTpl<Scalar,Options,JointCollectionTpl> &
120
    buildModel(const std::shared_ptr< ::urdf::ModelInterface> urdfTree,
121
               ModelTpl<Scalar,Options,JointCollectionTpl> & model,
122
               const bool verbose = false);
123
#endif
124
125
    ///
126
    /// \brief Build the model from an XML stream with a particular joint as root of the model tree inside
127
    /// the model given as reference argument.
128
    ///
129
    /// \param[in] xml_stream stream containing the URDF model.
130
    /// \param[in] rootJoint The joint at the root of the model tree.
131
    /// \param[in] verbose Print parsing info.
132
    /// \param[out] model Reference model where to put the parsed information.
133
    /// \return Return the reference on argument model for convenience.
134
    ///
135
    /// \note urdfTree can be build from ::urdf::parseURDF
136
    ///       or ::urdf::parseURDFFile
137
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
138
    ModelTpl<Scalar,Options,JointCollectionTpl> &
139
    buildModelFromXML(const std::string & xml_stream,
140
                      const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & rootJoint,
141
                      ModelTpl<Scalar,Options,JointCollectionTpl> & model,
142
                      const bool verbose = false);
143
144
    ///
145
    /// \brief Build the model from an XML stream
146
    ///
147
    /// \param[in] xml_stream stream containing the URDF model.
148
    /// \param[in] verbose Print parsing info.
149
    /// \param[out] model Reference model where to put the parsed information.
150
    /// \return Return the reference on argument model for convenience.
151
    ///
152
    /// \note urdfTree can be build from ::urdf::parseURDF
153
    ///       or ::urdf::parseURDFFile
154
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
155
    ModelTpl<Scalar,Options,JointCollectionTpl> &
156
    buildModelFromXML(const std::string & xml_stream,
157
                      ModelTpl<Scalar,Options,JointCollectionTpl> & model,
158
                      const bool verbose = false);
159
160
161
    /**
162
     * @brief      Build The GeometryModel from a URDF file. Search for meshes
163
     *             in the directories specified by the user first and then in
164
     *             the environment variable ROS_PACKAGE_PATH
165
     *
166
     * @param[in]  model         The model of the robot, built with
167
     *                           urdf::buildModel
168
     * @param[in]  filename      The URDF complete (absolute) file path
169
     * @param[in]  package_paths   A vector containing the different directories
170
     *                           where to search for models and meshes, typically
171
     *                           obtained from calling pinocchio::rosPaths()
172
     *
173
     * @param[in]   type         The type of objects that must be loaded (must be VISUAL or COLLISION)
174
     * @param[in]   mesh_loader   object used to load meshes: hpp::fcl::MeshLoader [default] or hpp::fcl::CachedMeshLoader.
175
     * @param[out]  geom_model    Reference where to put the parsed information.
176
     *
177
     * @return      Returns the reference on geom model for convenience.
178
     *
179
     * \warning     If hpp-fcl has not been found during compilation, COLLISION objects can not be loaded
180
     *
181
     */
182
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
183
    GeometryModel & buildGeom(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
184
                              const std::string & filename,
185
                              const GeometryType type,
186
                              GeometryModel & geom_model,
187
                              const std::vector<std::string> & package_paths = std::vector<std::string> (),
188
                              ::hpp::fcl::MeshLoaderPtr mesh_loader = ::hpp::fcl::MeshLoaderPtr());
189
190
    /**
191
     * @brief      Build The GeometryModel from a URDF file. Search for meshes
192
     *             in the directories specified by the user first and then in
193
     *             the environment variable ROS_PACKAGE_PATH
194
     *
195
     * @param[in]  model         The model of the robot, built with
196
     *                           urdf::buildModel
197
     * @param[in]  filename      The URDF complete (absolute) file path
198
     * @param[in]  package_path    A string containing the path to the directories of the meshes,
199
     *                           typically obtained from calling pinocchio::rosPaths().
200
     *
201
     * @param[in]   type         The type of objects that must be loaded (must be VISUAL or COLLISION)
202
     * @param[in]   mesh_loader   object used to load meshes: hpp::fcl::MeshLoader [default] or hpp::fcl::CachedMeshLoader.
203
     * @param[out]  geom_model    Reference where to put the parsed information.
204
     *
205
     * @return      Returns the reference on geom model for convenience.
206
     *
207
     * \warning     If hpp-fcl has not been found during compilation, COLLISION objects can not be loaded
208
     *
209
     */
210
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
211
9
    GeometryModel & buildGeom(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
212
                              const std::string & filename,
213
                              const GeometryType type,
214
                              GeometryModel & geom_model,
215
                              const std::string & package_path,
216
                              hpp::fcl::MeshLoaderPtr mesh_loader = hpp::fcl::MeshLoaderPtr())
217
218
    {
219
9
      const std::vector<std::string> dirs(1,package_path);
220
18
      return buildGeom(model,filename,type,geom_model,dirs,mesh_loader);
221
    }
222
223
    /**
224
     * @brief      Build The GeometryModel from a URDF model. Search for meshes
225
     *             in the directories specified by the user first and then in
226
     *             the environment variable ROS_PACKAGE_PATH
227
     *
228
     * @param[in]  model         The model of the robot, built with
229
     *                           urdf::buildModel
230
     * @param[in]  xml_stream     Stream containing the URDF model
231
     * @param[in]  package_paths   A vector containing the different directories
232
     *                           where to search for models and meshes, typically
233
     *                           obtained from calling pinocchio::rosPaths()
234
     *
235
     * @param[in]   type         The type of objects that must be loaded (must be VISUAL or COLLISION)
236
     * @param[in]   mesh_loader   object used to load meshes: hpp::fcl::MeshLoader [default] or hpp::fcl::CachedMeshLoader.
237
     * @param[out]  geom_model    Reference where to put the parsed information.
238
     *
239
     * @return      Returns the reference on geom model for convenience.
240
     *
241
     * \warning     If hpp-fcl has not been found during compilation, COLLISION objects cannot be loaded
242
     *
243
     */
244
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
245
    GeometryModel & buildGeom(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
246
                              const std::istream & xml_stream,
247
                              const GeometryType type,
248
                              GeometryModel & geom_model,
249
                              const std::vector<std::string> & package_paths = std::vector<std::string> (),
250
                              hpp::fcl::MeshLoaderPtr mesh_loader = hpp::fcl::MeshLoaderPtr());
251
252
    /**
253
     * @brief      Build The GeometryModel from a URDF model. Search for meshes
254
     *             in the directories specified by the user first and then in
255
     *             the environment variable ROS_PACKAGE_PATH
256
     *
257
     * @param[in]  model         The model of the robot, built with
258
     *                           urdf::buildModel
259
     * @param[in]  xml_stream     Stream containing the URDF model
260
     * @param[in]  package_path    A string containing the path to the directories of the meshes,
261
     *                           typically obtained from calling pinocchio::rosPaths().
262
     *
263
     * @param[in]   type         The type of objects that must be loaded (must be VISUAL or COLLISION)
264
     * @param[in]   mesh_loader   object used to load meshes: hpp::fcl::MeshLoader [default] or hpp::fcl::CachedMeshLoader.
265
     * @param[out]  geom_model    Reference where to put the parsed information.
266
     *
267
     * @return      Returns the reference on geom model for convenience.
268
     *
269
     * \warning     If hpp-fcl has not been found during compilation, COLLISION objects cannot be loaded
270
     *
271
     */
272
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
273
    GeometryModel & buildGeom(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
274
                              const std::istream & xml_stream,
275
                              const GeometryType type,
276
                              GeometryModel & geom_model,
277
                              const std::string & package_path,
278
                              hpp::fcl::MeshLoaderPtr mesh_loader = hpp::fcl::MeshLoaderPtr())
279
280
    {
281
      const std::vector<std::string> dirs(1,package_path);
282
      return buildGeom(model,xml_stream,type,geom_model,dirs,mesh_loader);
283
    }
284
285
286
  } // namespace urdf
287
} // namespace pinocchio
288
289
#include "pinocchio/parsers/urdf/model.hxx"
290
#include "pinocchio/parsers/urdf/geometry.hxx"
291
292
#endif // ifndef __pinocchio_parsers_urdf_hpp__