1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2023 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_multibody_parsers_urdf_geometry_hxx__ |
6 |
|
|
#define __pinocchio_multibody_parsers_urdf_geometry_hxx__ |
7 |
|
|
|
8 |
|
|
#include "pinocchio/parsers/urdf.hpp" |
9 |
|
|
|
10 |
|
|
#include <iostream> |
11 |
|
|
#include <sstream> |
12 |
|
|
#include <iomanip> |
13 |
|
|
|
14 |
|
|
#ifdef PINOCCHIO_WITH_HPP_FCL |
15 |
|
|
#include <hpp/fcl/mesh_loader/loader.h> |
16 |
|
|
#include <hpp/fcl/mesh_loader/assimp.h> |
17 |
|
|
#endif // PINOCCHIO_WITH_HPP_FCL |
18 |
|
|
|
19 |
|
|
namespace pinocchio |
20 |
|
|
{ |
21 |
|
|
namespace urdf |
22 |
|
|
{ |
23 |
|
|
namespace details |
24 |
|
|
{ |
25 |
|
|
struct UrdfGeomVisitorBase |
26 |
|
|
{ |
27 |
|
|
typedef FrameTpl<urdf_scalar_type, 0> Frame; |
28 |
|
|
|
29 |
|
|
virtual Frame getBodyFrame (const std::string& name, FrameIndex& fid) const = 0; |
30 |
|
|
}; |
31 |
|
|
|
32 |
|
|
template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl> |
33 |
|
|
struct UrdfGeomVisitor : UrdfGeomVisitorBase |
34 |
|
|
{ |
35 |
|
|
typedef ModelTpl<_Scalar,_Options,JointCollectionTpl> Model; |
36 |
|
|
const Model& model; |
37 |
|
|
|
38 |
|
41 |
UrdfGeomVisitor (const Model& model) : model(model) {} |
39 |
|
|
|
40 |
|
760 |
Frame getBodyFrame (const std::string& link_name, FrameIndex& fid) const |
41 |
|
|
{ |
42 |
✓✗✗✓
|
760 |
if (!model.existFrame(link_name, BODY)) |
43 |
|
|
{ |
44 |
|
|
throw std::invalid_argument("No link " + link_name + " in model"); |
45 |
|
|
} |
46 |
✓✗ |
760 |
fid = model.getFrameId(link_name, BODY); |
47 |
✗✓✗✗
|
760 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(model.frames[fid].type == BODY); |
48 |
|
760 |
return model.frames[fid].template cast<urdf_scalar_type>(); |
49 |
|
|
} |
50 |
|
|
}; |
51 |
|
|
|
52 |
|
|
/** |
53 |
|
|
* @brief Recursive procedure for reading the URDF tree, looking for geometries |
54 |
|
|
* This function fill the geometric model whith geometry objects retrieved from the URDF tree |
55 |
|
|
* |
56 |
|
|
* @param[in] tree The URDF kinematic tree |
57 |
|
|
* @param[in] meshLoader The FCL mesh loader to avoid duplications of already loaded geometries |
58 |
|
|
* @param[in] link The current URDF link |
59 |
|
|
* @param model The model to which is the GeometryModel associated |
60 |
|
|
* @param geomModel The GeometryModel where the Collision Objects must be added |
61 |
|
|
* @param[in] package_dirs A vector containing the different directories where to search for packages |
62 |
|
|
* @param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION) |
63 |
|
|
* |
64 |
|
|
*/ |
65 |
|
|
PINOCCHIO_DLLAPI void parseTreeForGeom(UrdfGeomVisitorBase& visitor, |
66 |
|
|
const std::istream& xmlStream, |
67 |
|
|
const GeometryType type, |
68 |
|
|
GeometryModel & geomModel, |
69 |
|
|
const std::vector<std::string> & package_dirs, |
70 |
|
|
::hpp::fcl::MeshLoaderPtr meshLoader); |
71 |
|
|
|
72 |
|
|
} // namespace details |
73 |
|
|
|
74 |
|
|
|
75 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
76 |
|
20 |
GeometryModel& buildGeom(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
77 |
|
|
const std::string & filename, |
78 |
|
|
const GeometryType type, |
79 |
|
|
GeometryModel & geomModel, |
80 |
|
|
const std::vector<std::string> & package_dirs, |
81 |
|
|
::hpp::fcl::MeshLoaderPtr meshLoader) |
82 |
|
|
{ |
83 |
✓✗ |
20 |
std::ifstream xmlStream(filename.c_str()); |
84 |
✓✗✗✓
|
20 |
if (! xmlStream.is_open()) |
85 |
|
|
{ |
86 |
|
|
const std::string exception_message (filename + " does not seem to be a valid file."); |
87 |
|
|
throw std::invalid_argument(exception_message); |
88 |
|
|
} |
89 |
✓✗ |
40 |
return buildGeom (model, xmlStream, type, geomModel, package_dirs, meshLoader); |
90 |
|
|
} |
91 |
|
|
|
92 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
93 |
|
41 |
GeometryModel& buildGeom(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
94 |
|
|
const std::istream& xmlStream, |
95 |
|
|
const GeometryType type, |
96 |
|
|
GeometryModel & geomModel, |
97 |
|
|
const std::vector<std::string> & package_dirs, |
98 |
|
|
::hpp::fcl::MeshLoaderPtr meshLoader) |
99 |
|
|
{ |
100 |
|
41 |
details::UrdfGeomVisitor<Scalar, Options, JointCollectionTpl> visitor (model); |
101 |
✓✗ |
41 |
details::parseTreeForGeom (visitor, xmlStream, type, geomModel, |
102 |
|
|
package_dirs, meshLoader); |
103 |
|
41 |
return geomModel; |
104 |
|
|
} |
105 |
|
|
|
106 |
|
|
} // namespace urdf |
107 |
|
|
} // namespace pinocchio |
108 |
|
|
|
109 |
|
|
#endif // ifndef __pinocchio_multibody_parsers_urdf_geometry_hxx__ |