parser.hh 7.77 KB
Newer Older
1
2
// Copyright (C) 2012, 2013, 2014 CNRS-LAAS
// Authors: Antonio El Khoury, Florent Lamiraux
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
//
// This file is part of the hpp-model-urdf.
//
// hpp-model-urdf is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// hpp-model-urdf is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with hpp-model-urdf.  If not, see <http://www.gnu.org/licenses/>.


/**
 * \brief Declaration of Parser.
 */

#ifndef HPP_MODEL_URDF_PARSER
# define HPP_MODEL_URDF_PARSER

# include <string>
28
# include <map>
29

30
# include <urdf/model.h>
31

32
33
34
35
# include <fcl/BV/OBB.h>
# include <fcl/BVH/BVH_model.h>

# include <hpp/model/body.hh>
36
# include <hpp/model/humanoid-robot.hh>
37
38
39
40
# include <hpp/model/object-factory.hh>

class aiNode;
class aiScene;
41
42
43
44
45
46
47
48

namespace hpp
{
  namespace model
  {
    namespace urdf
    {
      /// \brief Parse an URDF file and return a
49
      /// hpp::model::HumanoidRobotPtr_t.
50
51
52
      class Parser
      {
      public:
53
54
55
56
57
58
	typedef boost::shared_ptr < ::urdf::Link> UrdfLinkPtrType;
	typedef boost::shared_ptr < ::urdf::Joint> UrdfJointPtrType;
	typedef boost::shared_ptr < ::urdf::JointLimits> UrdfJointLimitsPtrType;
	typedef boost::shared_ptr <const ::urdf::Link> UrdfLinkConstPtrType;
	typedef boost::shared_ptr <const ::urdf::Joint> UrdfJointConstPtrType;

59
	typedef HumanoidRobotPtr_t RobotPtrType;
60
61
62
63
64
65
	typedef Joint JointType;
	typedef Body BodyType;
	typedef fcl::BVHModel< fcl::RSS > PolyhedronType;
	typedef boost::shared_ptr <PolyhedronType> PolyhedronPtrType;

	typedef Transform3f MatrixHomogeneousType;
66
67

	/// \brief Map of abstract robot dynamics compatible joints.
68
	typedef std::map<const std::string, JointPtr_t> MapHppJointType;
69
70
	/// \brief Map of URDF joints.
	typedef std::map<std::string, UrdfJointPtrType> MapJointType;
71

72
	/// \brief Default constructor.
73
74
75
76
	///
	/// \param rootJointType type of root joint among "anchor", "freeflyer",
	/// "planar",
	explicit Parser (const std::string& rooJointType);
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
	/// \brief Destructor.
	virtual ~Parser ();

	/// \brief Parse an URDF file and return a humanoid robot.
	///
	/// The URDF file location must use the resource retriever format.
	/// For instance, the following strings are allowed:
	/// - package://myPackage/robot.urdf
	/// - file:///tmp/robot.urdf
	/// - http://mywebsite.com/robot.urdf
	///
	/// See resource_retriever documentation for more information.
	///
	/// \param resourceName resource name using the
	/// resource_retriever format.
92
	RobotPtrType
93
	parse (const std::string& resourceName);
94

95
96
97
	/// \brief Parse an URDF sent as a stream and return a
	/// humanoid robot.
	RobotPtrType
98
	parseStream (const std::string& robotDescription);
99

Florent Lamiraux's avatar
Florent Lamiraux committed
100
101
102
103
104
	/// \brief Set special joints in robot.
	void setSpecialJoints ();
	/// \brief Fill gaze.
	void fillGaze ();

105
      private:
106
	/// \brief Retrieve joint name attached to a particular link.
107
108
	void findSpecialJoint (const std::string& linkName,
			       std::string& jointName);
109
110
111
112
113
114
115

	/// \brief Find special joints using REP 120.
	///
	/// This is not direct as abstract-robot-dynamics needs
	/// joints where as REP 120 deals with frames/robot links.
	/// We have to use the REP naming standard to identify the
	/// links and then retrieve the attached joints name.
116
	void findSpecialJoints ();
117

118
119
120
121
122
	/// Create root joint of robot
	void createRootJoint (const std::string& name,
			      const MatrixHomogeneousType& mat,
			      DevicePtr_t robot);

123
124
125
	/// \brief Parse URDF model and get joints.
	///
	/// Each joint in the URDF model is used to build the
126
	/// corresponding hpp::model::JointPtr_t object.
127
	void parseJoints ();
128
129

	/// \brief Connect recursively joints to their children.
130
	void connectJoints (const JointPtr_t& rootJoint);
131
132

	/// \brief Parse bodies and add them to joints.
133
	void addBodiesToJoints();
134

135
	/// \brief compute body absolute position.
136
137
138
139
	///
	/// \param link link for which absolute position is computed
	/// \param pose pose in local from, i.e origin of visual or
	/// collision node
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
	MatrixHomogeneousType computeBodyAbsolutePosition
	(const UrdfLinkConstPtrType& link, const ::urdf::Pose& pose);

	/// \brief Load polyhedron from resource.
	void loadPolyhedronFromResource
	(const std::string& filename, const ::urdf::Vector3& scale,
	 const Parser::PolyhedronPtrType& polyhedron);

	void meshFromAssimpScene (const std::string& name,
				  const ::urdf::Vector3& scale,
				  const aiScene* scene,
				  const PolyhedronPtrType& mesh);

	void buildMesh (const ::urdf::Vector3& scale,
			const aiScene* scene,
			const aiNode* node,
			std::vector<unsigned>& subMeshIndexes,
			const PolyhedronPtrType& mesh);
158

159
160
161
	/// \brief Add solid component to body.
	///
	/// The visual and collision geometries attached to the link
162
163
164
	/// are used to create the appropriate FCL geometry.
	void addSolidComponentToJoint (const UrdfLinkConstPtrType& link,
				       const JointPtr_t& joint);
165
166
167
168

	// returns, in a vector, the children of a joint, or a
	// subchildren if no children si present in the actuated
	// joints map.
169
170
	std::vector<std::string> getChildrenJoint
	(const std::string& jointName);
171

Antonio El Khoury's avatar
Antonio El Khoury committed
172
	bool getChildrenJoint (const std::string& jointName,
173
174
			       std::vector<std::string>& result);

175
176
177
178
179
	/// Create free-flyer joints and add them to joints map.
	/// If robot is provided, set root joint.
	void createFreeflyerJoint (const std::string& name,
				   const MatrixHomogeneousType& mat,
				   DevicePtr_t robot = DevicePtr_t ());
180

181
182
183
184
185
	/// Create two translations and one rotation in horizontal plane.
	void createPlanarJoint (const std::string& name,
				const MatrixHomogeneousType& mat,
				DevicePtr_t robot);

186
	/// \brief Create rotation joint and add it to joints map.
187
188
189
	JointPtr_t createRotationJoint (const std::string& name,
					  const MatrixHomogeneousType& mat,
					  const UrdfJointLimitsPtrType& limits);
190
191

	/// \brief Create continuous joint and add it to joints map.
192
193
	JointPtr_t createContinuousJoint (const std::string& name,
					    const MatrixHomogeneousType& mat);
194
195

	/// \brief Create translation joint and add it to joints map.
196
197
198
	JointPtr_t createTranslationJoint
	(const std::string& name, const MatrixHomogeneousType& mat,
	 const UrdfJointLimitsPtrType& limits);
199
200

	/// \brief Create anchor joint and add it to joints map.
201
202
	JointPtr_t createAnchorJoint (const std::string& name,
					const MatrixHomogeneousType& mat);
203
204
205

	/// \brief Get joint by looking for string in joints map
	/// attribute.
206
	JointPtr_t findJoint (const std::string& jointName);
207

208
209
	/// \brief Convert URDF pose to MatrixHomogeneousType transformation.
	MatrixHomogeneousType poseToMatrix (::urdf::Pose p);
210
211

	/// \brief Get joint position in given reference frame.
212
213
214
	MatrixHomogeneousType getPoseInReferenceFrame
	(const std::string& referenceJointName,
	 const std::string& currentJointName);
215
216
217

	::urdf::Model model_;
	RobotPtrType robot_;
218
	JointPtr_t rootJoint_;
Antonio El Khoury's avatar
Antonio El Khoury committed
219
	MapHppJointType jointsMap_;
220
	std::string rootJointType_;
221
222
223
224
225
226
227
228
229
230
231
232
233
234
	/// \brief Special joints names.
	/// \{
	std::string waistJointName_;
	std::string chestJointName_;
	std::string leftWristJointName_;
	std::string rightWristJointName_;
	std::string leftHandJointName_;
	std::string rightHandJointName_;
	std::string leftAnkleJointName_;
	std::string rightAnkleJointName_;
	std::string leftFootJointName_;
	std::string rightFootJointName_;
	std::string gazeJointName_;
	/// \}
235
236
237
	std::vector <fcl::Vec3f> vertices_;
	std::vector <fcl::Triangle> triangles_;
	ObjectFactory objectFactory_;
238
239
240
241
242
243
      }; // class Parser
    } // end of namespace urdf.
  } // end of namespace model.
} // end of namespace hpp.

#endif // HPP_MODEL_URDF_PARSER