parser.hh 7.28 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
// Copyright (C) 2012 by Antonio El Khoury.
//
// 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>
27
# include <map>
28

29
# include <urdf/model.h>
30

31
# include <hpp/model/body.hh>
32
33
34
35
36
37
38
39
40
41
42
43
44
# include <hpp/model/humanoid-robot.hh>

namespace hpp
{
  namespace model
  {
    namespace urdf
    {
      /// \brief Parse an URDF file and return a
      /// hpp::model::HumanoidRobotShPtr.
      class Parser
      {
      public:
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
	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;

	typedef hpp::model::HumanoidRobotShPtr RobotPtrType;
	typedef hpp::model::JointShPtr JointPtrType;
	typedef hpp::model::BodyShPtr BodyPtrType;
	typedef CjrlHand* HandPtrType;
	typedef CjrlFoot* FootPtrType;

	/// \brief Map of abstract robot dynamics compatible joints.
	typedef std::map<const std::string, JointPtrType> MapHppJoint;
	/// \brief Map of URDF joints.
	typedef std::map<std::string, UrdfJointPtrType> MapJointType;
61

62
63
64
65
66
	/// \brief Default constructor.
	explicit Parser ();
	/// \brief Destructor.
	virtual ~Parser ();

67
68
69
70
71
	void displayActuatedJoints(std::ostream &os);
	void displayFoot(CjrlFoot* aFoot,std::ostream& os);
	void displayHand(CjrlHand* aHand,std::ostream& os);
	void displayEndEffectors(std::ostream& os);

72
73
74
75
76
77
78
79
80
81
82
83
	/// \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.
84
	RobotPtrType
85
86
87
	parse (const std::string& resourceName,
	       const std::string& rootJointName);

88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
	/// \brief Parse an URDF sent as a stream and return a
	/// humanoid robot.
	RobotPtrType
	parseStream (const std::string& robotDescription,
		     const std::string& rootJointName);

      protected:
	/// \brief Retrieve joint name attached to a particular link.
	void
	findSpecialJoint (const std::string& linkName,
			  std::string& jointName);

	/// \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.
	void
	findSpecialJoints ();

109
110
111
112
	/// \brief Set special joints in robot.
	void
	setSpecialJoints ();

113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
	/// \brief Parse URDF model and get joints.
	///
	/// Each joint in the URDF model is used to build the
	/// corresponding hpp::model::JointShPtr object.
	void
	parseJoints (const std::string rootJointName);

	/// \brief Get actuated joints.
	std::vector<CjrlJoint*>
	actuatedJoints();

	/// \brief Connect recursively joints to their children.
	void
	connectJoints (const JointPtrType& rootJoint);

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

131
	/// \brief compute body absolute position.
132
133
134
135
	///
	/// \param link link for which absolute position is computed
	/// \param pose pose in local from, i.e origin of visual or
	/// collision node
136
	CkitMat4
137
138
	computeBodyAbsolutePosition (const UrdfLinkConstPtrType& link,
				     const ::urdf::Pose& pose);
139

140
141
142
143
144
145
146
147
	/// \brief Add solid component to body.
	///
	/// The visual and collision geometries attached to the link
	/// are used to create the appropriate Kite solid component.
	void
	addSolidComponentToBody (const UrdfLinkConstPtrType& link,
				 const BodyPtrType& body);

148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
	/// \brief Set free-flyer joint bounds for roamdap builders.
	void
	setFreeFlyerBounds ();

	/// \brief Compute hands information.
	void
	computeHandsInformation (MapHppJoint::const_iterator& hand,
				 MapHppJoint::const_iterator& wrist,
				 vector3d& center,
				 vector3d& thumbAxis,
				 vector3d& foreFingerAxis,
				 vector3d& palmNormal) const;

	/// \brief Fill hands and feet.
	void
	fillHandsAndFeet ();

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

	void getChildrenJoint (const std::string& jointName,
			       std::vector<std::string>& result);

	/// \brief Create free-flyer joint and add it to joints map.
	JointPtrType
	createFreeflyerJoint (const std::string& name, const CkitMat4& mat);

	/// \brief Create rotation joint and add it to joints map.
	JointPtrType
	createRotationJoint (const std::string& name,
			     const CkitMat4& mat,
			     const UrdfJointLimitsPtrType& limits);

	/// \brief Create continuous joint and add it to joints map.
	JointPtrType
	createContinuousJoint (const std::string& name,
			       const CkitMat4& mat);

	/// \brief Create translation joint and add it to joints map.
	JointPtrType
	createTranslationJoint (const std::string& name,
				const CkitMat4& mat,
				const UrdfJointLimitsPtrType& limits);

	/// \brief Create anchor joint and add it to joints map.
	JointPtrType
	createAnchorJoint (const std::string& name, const CkitMat4& mat);

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

	/// \brief Compute ankle position in foot frame.
	vector3d
	computeAnklePositionInLocalFrame
	(MapHppJoint::const_iterator& foot,
	 MapHppJoint::const_iterator& ankle) const;

	/// \brief Convert URDF pose to CkitMat4 transformation.
	CkitMat4
	poseToMatrix (::urdf::Pose p);

	/// \brief Get joint position in given reference frame.
	CkitMat4
	getPoseInReferenceFrame(const std::string& referenceJointName,
				const std::string& currentJointName);

219
      private:
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
	::urdf::Model model_;
	RobotPtrType robot_;
	JointPtrType rootJoint_;
	MapHppJoint jointsMap_;
	dynamicsJRLJapan::ObjectFactory factory_;

	/// \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_;
	/// \}
240

241
242
243
244
245
246
247
      }; // class Parser

    } // end of namespace urdf.
  } // end of namespace model.
} // end of namespace hpp.

#endif // HPP_MODEL_URDF_PARSER