Branch data Line data Source code
1 : : // Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
2 : : // Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
3 : : // RDL - Robot Dynamics Library
4 : : // Licensed under the zlib license. See LICENSE for more details.
5 : :
6 : : #include <fstream>
7 : : #include <stack>
8 : :
9 : : #include <urdf_model/model.h>
10 : : #include <urdf_parser/urdf_parser.h>
11 : :
12 : : #include <rdl_dynamics/Model.hpp>
13 : : #include <rdl_urdfreader/urdfreader.hpp>
14 : : #include <tinyxml.h>
15 : :
16 : : using namespace std;
17 : :
18 : : namespace RobotDynamics
19 : : {
20 : : namespace Urdf
21 : : {
22 : : using namespace Math;
23 : :
24 : : /**
25 : : * @brief Helper method for getting xml elements like <origin> 1 2 3 </origin> into an stl container for
26 : : * further operations
27 : : * @param element
28 : : * @return vector of doubles
29 : : */
30 : 63 : inline std::vector<double> xmlElementToVector(TiXmlElement* element)
31 : : {
32 : 63 : assert(element);
33 : 126 : std::istringstream stringStream(element->GetText());
34 : :
35 : 63 : std::string numberAsString;
36 : :
37 : 63 : std::vector<double> output;
38 : :
39 : 1118 : while (stringStream >> numberAsString)
40 : : {
41 : 1055 : output.push_back(std::stod(numberAsString));
42 : : }
43 : :
44 : 126 : return output;
45 : 63 : }
46 : :
47 : : typedef vector<urdf::LinkSharedPtr> URDFLinkVector;
48 : : typedef vector<urdf::JointSharedPtr> URDFJointVector;
49 : : typedef map<string, urdf::LinkSharedPtr> URDFLinkMap;
50 : : typedef map<string, urdf::JointSharedPtr> URDFJointMap;
51 : :
52 : 21 : bool construct_model(ModelPtr rdl_model, urdf::ModelInterfaceSharedPtr urdf_model, bool floating_base, const std::map<std::string, hydro::BodyHydroData>& hydroData,
53 : : JointType floating_base_rotation_joint_type, bool verbose)
54 : : {
55 : 21 : urdf::LinkSharedPtr urdf_root_link;
56 : :
57 : 21 : URDFLinkMap link_map;
58 : 21 : link_map = urdf_model->links_;
59 : :
60 : 21 : URDFJointMap joint_map;
61 : 21 : joint_map = urdf_model->joints_;
62 : :
63 : 21 : vector<string> joint_names;
64 : :
65 : 21 : stack<urdf::LinkSharedPtr> link_stack;
66 : 21 : stack<int> joint_index_stack;
67 : :
68 : : // add the bodies in a depth-first order of the model tree
69 : 21 : link_stack.push(link_map[(urdf_model->getRoot()->name)]);
70 : :
71 : : // add the root body
72 : 21 : urdf::LinkConstSharedPtr root = urdf_model->getRoot();
73 : :
74 : 21 : if (root->inertial)
75 : : {
76 : 12 : Vector3d root_inertial_position(root->inertial->origin.position.x, root->inertial->origin.position.y, root->inertial->origin.position.z);
77 : :
78 : 12 : RobotDynamics::Math::Matrix3d root_inertia_in_com_frame(root->inertial->ixx, root->inertial->ixy, root->inertial->ixz, root->inertial->ixy, root->inertial->iyy,
79 : 24 : root->inertial->iyz, root->inertial->ixz, root->inertial->iyz, root->inertial->izz);
80 : :
81 : 12 : RigidBodyInertia I(root->inertial->mass, root_inertial_position, root_inertia_in_com_frame);
82 : : // this transform here handles the case where the com frame has a non-trivial rotation
83 : 12 : I.transform(RobotDynamics::Math::XrotQuat(root->inertial->origin.rotation.x, root->inertial->origin.rotation.y, root->inertial->origin.rotation.z,
84 : 12 : root->inertial->origin.rotation.w));
85 : 12 : Body root_link;
86 : 12 : hydro::BodyHydroData bodyHydroData = hydroData.at(root->name);
87 : 12 : if (bodyHydroData.hasHydro)
88 : : {
89 : 5 : DragData dragData(bodyHydroData.linearDrag, bodyHydroData.quadraticDrag);
90 : 10 : root_link = Body(root->inertial->mass, root_inertial_position, Matrix3d(I.Ixx, I.Iyx, I.Izx, I.Iyx, I.Iyy, I.Izy, I.Izx, I.Izy, I.Izz),
91 : 5 : bodyHydroData.centerOfBuoyancy, bodyHydroData.volume, bodyHydroData.addedMassMatrix, dragData);
92 : : }
93 : : else
94 : : {
95 : 7 : root_link = Body(root->inertial->mass, root_inertial_position, Matrix3d(I.Ixx, I.Iyx, I.Izx, I.Iyx, I.Iyy, I.Izy, I.Izx, I.Izy, I.Izz));
96 : : }
97 : :
98 : 12 : Joint root_joint(JointTypeFixed);
99 : 12 : if (floating_base && floating_base_rotation_joint_type == JointTypeSpherical)
100 : : {
101 : 9 : root_joint = Joint(JointTypeFloatingBase);
102 : : }
103 : 3 : else if (floating_base && floating_base_rotation_joint_type != JointTypeSpherical)
104 : : {
105 : 2 : Body null_body(0., Vector3d(0., 0., 0.), Vector3d(0., 0., 0.));
106 : 2 : null_body.mIsVirtual = true;
107 : :
108 : 2 : rdl_model->appendBody(SpatialTransform(), Joint(JointTypeTranslationXYZ), null_body);
109 : :
110 : 2 : if (floating_base_rotation_joint_type == JointTypeEulerZYX)
111 : : {
112 : 1 : root_joint = Joint(JointTypeEulerZYX);
113 : : }
114 : 1 : else if (floating_base_rotation_joint_type == JointTypeEulerXYZ)
115 : : {
116 : 1 : root_joint = Joint(JointTypeEulerXYZ);
117 : : }
118 : : else
119 : : {
120 : 0 : std::cerr << "floating base rotation joint type " << floating_base_rotation_joint_type << " not currently supported by urdfreader" << std::endl;
121 : 0 : return false;
122 : : }
123 : 2 : }
124 : :
125 : 12 : SpatialTransform root_joint_frame = SpatialTransform();
126 : :
127 : 12 : if (verbose)
128 : : {
129 : 0 : cout << "+ Adding Root Body " << endl;
130 : 0 : cout << " joint frame: " << root_joint_frame << endl;
131 : 0 : if (floating_base)
132 : : {
133 : 0 : cout << " joint type : floating" << endl;
134 : : }
135 : : else
136 : : {
137 : 0 : cout << " joint type : fixed" << endl;
138 : : }
139 : 0 : cout << " body inertia: " << endl << root_link.mInertia << endl;
140 : 0 : cout << " body mass : " << root_link.mMass << endl;
141 : 0 : cout << " body name : " << root->name << endl;
142 : : }
143 : :
144 : 12 : rdl_model->appendBody(root_joint_frame, root_joint, root_link, root->name);
145 : 12 : }
146 : :
147 : 21 : if (link_stack.top()->child_joints.size() > 0)
148 : : {
149 : 19 : joint_index_stack.push(0);
150 : : }
151 : :
152 : 826 : while (!link_stack.empty())
153 : : {
154 : 805 : urdf::LinkSharedPtr cur_link = link_stack.top();
155 : : unsigned int joint_idx;
156 : 805 : if (joint_index_stack.size() > 0)
157 : : {
158 : 803 : joint_idx = joint_index_stack.top();
159 : : }
160 : : else
161 : : {
162 : 2 : joint_idx = 0;
163 : : }
164 : :
165 : 805 : if (joint_idx < cur_link->child_joints.size())
166 : : {
167 : 392 : urdf::JointSharedPtr cur_joint = cur_link->child_joints[joint_idx];
168 : :
169 : : // increment joint index
170 : 392 : joint_index_stack.pop();
171 : 392 : joint_index_stack.push(joint_idx + 1);
172 : :
173 : 392 : link_stack.push(link_map[cur_joint->child_link_name]);
174 : 392 : joint_index_stack.push(0);
175 : :
176 : 392 : if (verbose)
177 : : {
178 : 0 : for (unsigned int i = 1; i < joint_index_stack.size() - 1; i++)
179 : : {
180 : 0 : cout << " ";
181 : : }
182 : 0 : cout << "joint '" << cur_joint->name << "' child link '" << link_stack.top()->name << "' type = " << cur_joint->type << endl;
183 : : }
184 : :
185 : 392 : joint_names.push_back(cur_joint->name);
186 : 392 : }
187 : : else
188 : : {
189 : 413 : link_stack.pop();
190 : 413 : joint_index_stack.pop();
191 : : }
192 : 805 : }
193 : :
194 : 413 : for (size_t j = 0; j < joint_names.size(); j++)
195 : : {
196 : 392 : urdf::JointSharedPtr urdf_joint = joint_map[joint_names[j]];
197 : 392 : urdf::LinkSharedPtr urdf_parent = link_map[urdf_joint->parent_link_name];
198 : 392 : urdf::LinkSharedPtr urdf_child = link_map[urdf_joint->child_link_name];
199 : :
200 : : // determine where to add the current joint and child body
201 : 392 : unsigned int rdl_parent_id = 0;
202 : 392 : if (rdl_model->mBodies.size() != 1 || rdl_model->mFixedBodies.size() != 0)
203 : : {
204 : 383 : rdl_parent_id = rdl_model->GetBodyId(urdf_parent->name.c_str());
205 : : }
206 : :
207 : 392 : if (rdl_parent_id == std::numeric_limits<unsigned int>::max())
208 : : {
209 : 0 : cerr << "Error while processing joint '" << urdf_joint->name << "': parent link '" << urdf_parent->name << "' could not be found." << endl;
210 : : }
211 : :
212 : : // create the joint
213 : 392 : Joint rdl_joint;
214 : 392 : if (urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::CONTINUOUS)
215 : : {
216 : 123 : rdl_joint = Joint(SpatialVector(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z, 0., 0., 0.));
217 : : }
218 : 269 : else if (urdf_joint->type == urdf::Joint::PRISMATIC)
219 : : {
220 : 16 : rdl_joint = Joint(SpatialVector(0., 0., 0., urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
221 : : }
222 : 253 : else if (urdf_joint->type == urdf::Joint::FIXED)
223 : : {
224 : 251 : rdl_joint = Joint(JointTypeFixed);
225 : : }
226 : 2 : else if (urdf_joint->type == urdf::Joint::FLOATING)
227 : : {
228 : : // todo: what order of DoF should be used?
229 : 4 : rdl_joint = Joint(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.),
230 : 6 : SpatialVector(1., 0., 0., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(0., 0., 1., 0., 0., 0.));
231 : : }
232 : 0 : else if (urdf_joint->type == urdf::Joint::PLANAR)
233 : : {
234 : : // todo: which two directions should be used that are perpendicular
235 : : // to the specified axis?
236 : 0 : cerr << "Error while processing joint '" << urdf_joint->name << "': planar joints not yet supported!" << endl;
237 : 0 : return false;
238 : : }
239 : :
240 : : // compute the joint transformation
241 : 392 : Vector3d joint_rpy;
242 : 392 : Vector3d joint_translation;
243 : 392 : urdf_joint->parent_to_joint_origin_transform.rotation.getRPY(joint_rpy[0], joint_rpy[1], joint_rpy[2]);
244 : 392 : joint_translation.set(urdf_joint->parent_to_joint_origin_transform.position.x, urdf_joint->parent_to_joint_origin_transform.position.y,
245 : 392 : urdf_joint->parent_to_joint_origin_transform.position.z);
246 : 392 : SpatialTransform rdl_joint_frame = Xrot(joint_rpy[0], Vector3d(1., 0., 0.)) * Xrot(joint_rpy[1], Vector3d(0., 1., 0.)) *
247 : 784 : Xrot(joint_rpy[2], Vector3d(0., 0., 1.)) * Xtrans(Vector3d(joint_translation));
248 : :
249 : : // assemble the body
250 : 392 : Vector3d link_inertial_position;
251 : 392 : Vector3d link_inertial_rpy;
252 : 392 : Matrix3d link_inertial_inertia = Matrix3d::Zero();
253 : 392 : double link_inertial_mass = 0.;
254 : :
255 : : // but only if we actually have inertial data
256 : 392 : if (urdf_child->inertial)
257 : : {
258 : 306 : link_inertial_mass = urdf_child->inertial->mass;
259 : :
260 : 306 : link_inertial_position.set(urdf_child->inertial->origin.position.x, urdf_child->inertial->origin.position.y, urdf_child->inertial->origin.position.z);
261 : 306 : urdf_child->inertial->origin.rotation.getRPY(link_inertial_rpy[0], link_inertial_rpy[1], link_inertial_rpy[2]);
262 : :
263 : 306 : RobotDynamics::Math::Matrix3d link_inertia_in_com_frame = RobotDynamics::Math::Matrix3dZero;
264 : 306 : link_inertia_in_com_frame(0, 0) = urdf_child->inertial->ixx;
265 : 306 : link_inertia_in_com_frame(0, 1) = urdf_child->inertial->ixy;
266 : 306 : link_inertia_in_com_frame(0, 2) = urdf_child->inertial->ixz;
267 : 306 : link_inertia_in_com_frame(1, 0) = urdf_child->inertial->ixy;
268 : 306 : link_inertia_in_com_frame(1, 1) = urdf_child->inertial->iyy;
269 : 306 : link_inertia_in_com_frame(1, 2) = urdf_child->inertial->iyz;
270 : 306 : link_inertia_in_com_frame(2, 0) = urdf_child->inertial->ixz;
271 : 306 : link_inertia_in_com_frame(2, 1) = urdf_child->inertial->iyz;
272 : 306 : link_inertia_in_com_frame(2, 2) = urdf_child->inertial->izz;
273 : :
274 : 306 : RobotDynamics::Math::SpatialTransform X_body_to_inertial = RobotDynamics::Math::XeulerXYZ(link_inertial_rpy);
275 : 311 : link_inertial_inertia = link_inertial_rpy == Vector3d(0., 0., 0.) ? link_inertia_in_com_frame :
276 : 311 : X_body_to_inertial.E.transpose() * link_inertia_in_com_frame * X_body_to_inertial.E;
277 : : }
278 : :
279 : 392 : Body rdl_body;
280 : 392 : hydro::BodyHydroData bodyHydroData = hydroData.at(urdf_child->name);
281 : 392 : if (bodyHydroData.hasHydro)
282 : : {
283 : 72 : DragData dragData(bodyHydroData.linearDrag, bodyHydroData.quadraticDrag);
284 : 144 : rdl_body = Body(link_inertial_mass, link_inertial_position, link_inertial_inertia, bodyHydroData.centerOfBuoyancy, bodyHydroData.volume,
285 : 72 : bodyHydroData.addedMassMatrix, dragData);
286 : : }
287 : : else
288 : : {
289 : 320 : rdl_body = Body(link_inertial_mass, link_inertial_position, link_inertial_inertia);
290 : : }
291 : :
292 : 392 : if (verbose)
293 : : {
294 : 0 : cout << "+ Adding Body " << endl;
295 : 0 : cout << " parent_id : " << rdl_parent_id << endl;
296 : 0 : cout << " joint frame: " << rdl_joint_frame << endl;
297 : 0 : cout << " joint dofs : " << rdl_joint.mDoFCount << endl;
298 : 0 : for (unsigned int j = 0; j < rdl_joint.mDoFCount; j++)
299 : : {
300 : 0 : cout << " " << j << ": " << rdl_joint.mJointAxes[j].transpose() << endl;
301 : : }
302 : 0 : cout << " body inertia: " << endl << rdl_body.mInertia << endl;
303 : 0 : cout << " body mass : " << rdl_body.mMass << endl;
304 : 0 : cout << " body name : " << urdf_child->name << endl;
305 : 0 : cout << " body has_hydro :" << bodyHydroData.hasHydro << std::endl;
306 : 0 : cout << " body volume : " << bodyHydroData.volume << endl;
307 : 0 : cout << " body center_of_buoyancy : " << bodyHydroData.centerOfBuoyancy.transpose() << endl;
308 : : }
309 : :
310 : 392 : if (urdf_joint->type == urdf::Joint::FLOATING)
311 : : {
312 : 2 : Matrix3d zero_matrix = Matrix3d::Zero();
313 : 2 : Body null_body(0., Vector3d::Zero(3), zero_matrix);
314 : 2 : Joint joint_txtytz(JointTypeTranslationXYZ);
315 : 2 : string trans_body_name = urdf_child->name + "_Translate";
316 : 2 : rdl_model->addBody(rdl_parent_id, rdl_joint_frame, joint_txtytz, null_body, trans_body_name);
317 : :
318 : 2 : Joint joint_euler_zyx(JointTypeEulerXYZ);
319 : 2 : rdl_model->appendBody(SpatialTransform(), joint_euler_zyx, rdl_body, urdf_child->name);
320 : 2 : }
321 : : else
322 : : {
323 : 390 : rdl_model->addBody(rdl_parent_id, rdl_joint_frame, rdl_joint, rdl_body, urdf_child->name);
324 : : }
325 : 392 : }
326 : :
327 : 21 : return true;
328 : 21 : }
329 : :
330 : 1 : bool parseJointBodyNameMapFromFile(const std::string& filename, std::map<std::string, std::string>& jointBodyMap)
331 : : {
332 : 1 : ifstream model_file(filename);
333 : 1 : if (!model_file)
334 : : {
335 : 0 : cerr << "Error opening file '" << filename << "'." << endl;
336 : 0 : return false;
337 : : }
338 : :
339 : : // reserve memory for the contents of the file
340 : 1 : std::string model_xml_string;
341 : 1 : model_file.seekg(0, std::ios::end);
342 : 1 : model_xml_string.reserve(model_file.tellg());
343 : 1 : model_file.seekg(0, std::ios::beg);
344 : 1 : model_xml_string.assign((std::istreambuf_iterator<char>(model_file)), std::istreambuf_iterator<char>());
345 : :
346 : 1 : model_file.close();
347 : :
348 : 1 : return parseJointBodyNameMapFromString(model_xml_string.c_str(), jointBodyMap);
349 : 1 : }
350 : :
351 : 6 : bool parseJointAndBodyNamesFromString(const std::string& model_xml_string, std::vector<std::string>& joint_names, std::vector<std::string>& body_names)
352 : : {
353 : 6 : TiXmlDocument doc;
354 : :
355 : : // Check if contents are valid, if not, abort
356 : 6 : if (!doc.Parse(model_xml_string.c_str()) && doc.Error())
357 : : {
358 : 0 : std::cerr << "Can't parse urdf. Xml is invalid" << std::endl;
359 : 0 : return false;
360 : : }
361 : :
362 : : // Find joints in transmission tags
363 : 6 : TiXmlElement* root = doc.RootElement();
364 : :
365 : 56 : for (TiXmlElement* joint = root->FirstChildElement("joint"); joint; joint = joint->NextSiblingElement("joint"))
366 : : {
367 : 50 : if (!std::strcmp(joint->Attribute("type"), "fixed") || !std::strcmp(joint->Attribute("type"), "floating"))
368 : : {
369 : 19 : continue;
370 : : }
371 : :
372 : 31 : joint_names.push_back(joint->Attribute("name"));
373 : 31 : body_names.push_back(joint->FirstChildElement("child")->Attribute("link"));
374 : : }
375 : :
376 : 6 : return true;
377 : 6 : }
378 : :
379 : 1 : bool parseJointBodyNameMapFromString(const std::string& model_xml_string, std::map<std::string, std::string>& jointBodyMap)
380 : : {
381 : 1 : std::vector<std::string> joint_names, body_names;
382 : 1 : if (!parseJointAndBodyNamesFromString(model_xml_string, joint_names, body_names))
383 : : {
384 : 0 : return false;
385 : : }
386 : :
387 : 1 : jointBodyMap.clear();
388 : 13 : for (unsigned int i = 0; i < joint_names.size(); i++)
389 : : {
390 : 12 : jointBodyMap.insert(std::pair<std::string, std::string>(joint_names[i], body_names[i]));
391 : : }
392 : :
393 : 1 : return true;
394 : 1 : }
395 : :
396 : 4 : bool parseJointAndQIndex(const RobotDynamics::Model& model, const std::vector<std::string>& body_names, std::vector<unsigned int>& q_indices)
397 : : {
398 : 4 : q_indices.clear();
399 : 20 : for (unsigned int i = 0; i < body_names.size(); i++)
400 : : {
401 : 16 : q_indices.push_back(model.mJoints[model.GetBodyId(body_names[i].c_str())].q_index);
402 : : }
403 : :
404 : 4 : return true;
405 : : }
406 : :
407 : 4 : bool parseJointAndQIndex(const std::string& model_xml_string, std::vector<unsigned int>& q_indices)
408 : : {
409 : 4 : RobotDynamics::ModelPtr model(new RobotDynamics::Model());
410 : 4 : if (!urdfReadFromString(model_xml_string, model))
411 : : {
412 : 0 : return false;
413 : : }
414 : :
415 : 4 : std::vector<std::string> joint_names, body_names;
416 : 4 : if (!parseJointAndBodyNamesFromString(model_xml_string, joint_names, body_names))
417 : : {
418 : 0 : return false;
419 : : }
420 : :
421 : 4 : return parseJointAndQIndex(*model, body_names, q_indices);
422 : 4 : }
423 : :
424 : 17 : bool urdfReadFromFile(const std::string& filename, ModelPtr model, JointType floating_base_rotation_joint_type, bool verbose)
425 : : {
426 : 17 : ifstream model_file(filename.c_str());
427 : 17 : if (!model_file)
428 : : {
429 : 0 : cerr << "Error opening file '" << filename << "'." << endl;
430 : 0 : return false;
431 : : }
432 : :
433 : : // reserve memory for the contents of the file
434 : 17 : std::string model_xml_string;
435 : 17 : model_file.seekg(0, std::ios::end);
436 : 17 : model_xml_string.reserve(model_file.tellg());
437 : 17 : model_file.seekg(0, std::ios::beg);
438 : 17 : model_xml_string.assign((std::istreambuf_iterator<char>(model_file)), std::istreambuf_iterator<char>());
439 : :
440 : 17 : model_file.close();
441 : :
442 : 17 : return urdfReadFromString(model_xml_string, model, floating_base_rotation_joint_type, verbose);
443 : 17 : }
444 : :
445 : 21 : bool urdfReadFromString(const std::string& model_xml_string, ModelPtr model, JointType floating_base_rotation_joint_type, bool verbose)
446 : : {
447 : 21 : if (!model)
448 : : {
449 : 0 : cerr << "Rdl model is nullptr" << std::endl;
450 : 0 : return false;
451 : : }
452 : :
453 : 42 : urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(model_xml_string.c_str());
454 : :
455 : 21 : bool floating_base = std::strcmp(urdf_model->getRoot()->name.c_str(), "world") ? true : false;
456 : :
457 : 21 : std::map<std::string, hydro::BodyHydroData> bodyHydroData;
458 : 21 : if (!hydro::parseHydrodynamicsParameters(model_xml_string.c_str(), bodyHydroData, false))
459 : : {
460 : 0 : std::cerr << "Failed to parse hydrodynamics terms from urdf" << std::endl;
461 : 0 : return false;
462 : : }
463 : :
464 : 21 : if (!construct_model(model, urdf_model, floating_base, bodyHydroData, floating_base_rotation_joint_type, verbose))
465 : : {
466 : 0 : cerr << "Error constructing model from urdf file." << endl;
467 : 0 : return false;
468 : : }
469 : :
470 : 21 : model->gravity.SpatialVector::set(0., 0., 0., 0., 0., -9.81);
471 : :
472 : 21 : return true;
473 : 21 : }
474 : :
475 : 25 : bool hydro::parseHydrodynamicsParameters(const char* model_xml_string, std::map<std::string, hydro::BodyHydroData>& data, bool verbose)
476 : : {
477 : 25 : TiXmlDocument doc;
478 : 25 : doc.Parse(model_xml_string);
479 : 25 : if (doc.Error())
480 : : {
481 : 1 : std::cerr << "Error encountered parsing xml urdf. Error desc: " << doc.ErrorDesc() << std::endl;
482 : 1 : return false;
483 : : }
484 : :
485 : 24 : TiXmlElement* root = doc.RootElement();
486 : 24 : TiXmlElement* link_it = nullptr;
487 : :
488 : 501 : for (link_it = root->FirstChildElement("link"); link_it; link_it = link_it->NextSiblingElement("link"))
489 : : {
490 : : TiXmlElement* hydro;
491 : :
492 : 479 : std::string link_name = link_it->Attribute("name");
493 : 479 : hydro::BodyHydroData hydro_data;
494 : 479 : hydro = link_it->FirstChildElement("hydro");
495 : :
496 : 479 : if (verbose)
497 : : {
498 : 0 : std::cout << "---- Parsing hydro parameters for body " << link_name << "----" << std::endl;
499 : : }
500 : :
501 : 479 : if (!hydro)
502 : : {
503 : : // Doesn't have a hydro tag
504 : 381 : hydro_data.hasHydro = false;
505 : 381 : data[link_name] = hydro_data;
506 : :
507 : 381 : if (verbose)
508 : : {
509 : 0 : std::cout << "No <hydro> block found, skipping" << std::endl;
510 : : }
511 : :
512 : 381 : continue; // This link has no hydro parameters, so just move on to the next one
513 : : }
514 : : else
515 : : {
516 : 98 : if (verbose)
517 : : {
518 : 0 : std::cout << "Found <hydro> block, proceeding" << std::endl;
519 : : }
520 : 98 : hydro_data.hasHydro = true;
521 : : }
522 : :
523 : : TiXmlElement* buoyancy;
524 : 98 : buoyancy = hydro->FirstChildElement("buoyancy");
525 : :
526 : 98 : if (!buoyancy)
527 : : {
528 : 4 : if (verbose)
529 : : {
530 : 0 : std::cout << "No <buoyancy> block found" << std::endl;
531 : : }
532 : :
533 : 4 : hydro_data.volume = 0.0;
534 : 4 : hydro_data.centerOfBuoyancy = Math::Vector3d(0., 0., 0.);
535 : : }
536 : : else
537 : : {
538 : 94 : hydro_data.volume = strtod(buoyancy->FirstChildElement("volume")->GetText(), nullptr);
539 : 94 : std::string origin_str = buoyancy->FirstChildElement("origin")->Attribute("xyz");
540 : 94 : std::stringstream ss(origin_str);
541 : 94 : std::vector<double> xyz(3);
542 : 376 : for (unsigned int i = 0; i < xyz.size(); ++i)
543 : : {
544 : 282 : ss >> xyz[i];
545 : : }
546 : :
547 : 94 : hydro_data.centerOfBuoyancy.x() = xyz[0];
548 : 94 : hydro_data.centerOfBuoyancy.y() = xyz[1];
549 : 94 : hydro_data.centerOfBuoyancy.z() = xyz[2];
550 : 94 : }
551 : :
552 : 98 : if (verbose)
553 : : {
554 : 0 : std::cout << " Buoyancy params: " << std::endl;
555 : 0 : std::cout << " volume : " << hydro_data.volume << endl;
556 : 0 : std::cout << " center of buoyancy: " << hydro_data.centerOfBuoyancy.x() << ", " << hydro_data.centerOfBuoyancy.y() << ", "
557 : 0 : << hydro_data.centerOfBuoyancy.z() << endl;
558 : : }
559 : :
560 : 98 : TiXmlElement* addedMass = hydro->FirstChildElement("added_mass");
561 : 98 : if (addedMass)
562 : : {
563 : 23 : std::vector<double> added_mass_vec;
564 : 23 : added_mass_vec = xmlElementToVector(addedMass);
565 : 23 : if (added_mass_vec.size() != 36)
566 : : {
567 : 1 : std::cerr << "Added mass matrix for " << link_name << " only has " << added_mass_vec.size()
568 : 1 : << " elements. It MUST be a 6x6 matrix so it needs 36 elements" << std::endl;
569 : 1 : return false;
570 : : }
571 : :
572 : 22 : RobotDynamics::Math::SpatialMatrix added_mass;
573 : 154 : for (unsigned int i = 0; i < added_mass.rows(); i++)
574 : : {
575 : 924 : for (unsigned int j = 0; j < added_mass.rows(); j++)
576 : : {
577 : 792 : added_mass(i, j) = added_mass_vec[6 * i + j];
578 : : }
579 : : }
580 : :
581 : : /**
582 : : * Flip the order bc in RDL everything is angular part first
583 : : */
584 : 22 : hydro_data.addedMassMatrix.block<3, 3>(0, 0) = added_mass.block<3, 3>(3, 3);
585 : 22 : hydro_data.addedMassMatrix.block<3, 3>(3, 0) = added_mass.block<3, 3>(0, 3);
586 : 22 : hydro_data.addedMassMatrix.block<3, 3>(0, 3) = added_mass.block<3, 3>(3, 0);
587 : 22 : hydro_data.addedMassMatrix.block<3, 3>(3, 3) = added_mass.block<3, 3>(0, 0);
588 : 23 : }
589 : :
590 : 97 : if (verbose)
591 : : {
592 : 0 : std::cout << " Added mass matrix: \n" << std::endl;
593 : 0 : std::cout << hydro_data.addedMassMatrix << std::endl;
594 : : }
595 : :
596 : 97 : TiXmlElement* drag = hydro->FirstChildElement("drag");
597 : 97 : if (!drag)
598 : : {
599 : 539 : for (unsigned int i = 0; i < 6; i++)
600 : : {
601 : 462 : hydro_data.linearDrag[i] = 0.;
602 : 462 : hydro_data.quadraticDrag[i] = 0.;
603 : : }
604 : : }
605 : : else
606 : : {
607 : 20 : TiXmlElement* linear_damping = drag->FirstChildElement("linear_damping");
608 : 20 : TiXmlElement* quadratic_damping = drag->FirstChildElement("quadratic_damping");
609 : :
610 : 20 : if (linear_damping)
611 : : {
612 : 20 : std::vector<double> linearDrag = xmlElementToVector(linear_damping);
613 : :
614 : 20 : if (linearDrag.size() != 6)
615 : : {
616 : 0 : std::cerr << "Linear drag vector for " << link_name << " only has " << std::to_string(hydro_data.linearDrag.size()) << "elements, it must have 6!"
617 : 0 : << std::endl;
618 : 0 : return false;
619 : : }
620 : :
621 : : /**
622 : : * RDL does everything with angular first, so these have to be flipped assuming they're
623 : : * put in the urdf in the order x y z r p y
624 : : */
625 : 20 : hydro_data.linearDrag[0] = linearDrag[3];
626 : 20 : hydro_data.linearDrag[1] = linearDrag[4];
627 : 20 : hydro_data.linearDrag[2] = linearDrag[5];
628 : 20 : hydro_data.linearDrag[3] = linearDrag[0];
629 : 20 : hydro_data.linearDrag[4] = linearDrag[1];
630 : 20 : hydro_data.linearDrag[5] = linearDrag[2];
631 : 20 : }
632 : :
633 : 20 : if (quadratic_damping)
634 : : {
635 : 20 : std::vector<double> quadraticDrag = xmlElementToVector(quadratic_damping);
636 : :
637 : 20 : if (quadraticDrag.size() != 6)
638 : : {
639 : 2 : std::cerr << "Quadratic drag vector for " << link_name << " only has " << std::to_string(hydro_data.quadraticDrag.size())
640 : 2 : << "elements, it must have 6!" << std::endl;
641 : 1 : return false;
642 : : }
643 : :
644 : : /**
645 : : * RDL does everything with angular first, so these have to be flipped assuming they're
646 : : * put in the urdf in the order x y z r p y
647 : : */
648 : 19 : hydro_data.quadraticDrag[0] = quadraticDrag[3];
649 : 19 : hydro_data.quadraticDrag[1] = quadraticDrag[4];
650 : 19 : hydro_data.quadraticDrag[2] = quadraticDrag[5];
651 : 19 : hydro_data.quadraticDrag[3] = quadraticDrag[0];
652 : 19 : hydro_data.quadraticDrag[4] = quadraticDrag[1];
653 : 19 : hydro_data.quadraticDrag[5] = quadraticDrag[2];
654 : 20 : }
655 : : }
656 : :
657 : 96 : if (verbose)
658 : : {
659 : 0 : std::cout << " Drag params: " << std::endl;
660 : 0 : std::cout << " Linear drag : " << hydro_data.linearDrag[0] << " " << hydro_data.linearDrag[1] << " " << hydro_data.linearDrag[2] << " "
661 : 0 : << hydro_data.linearDrag[3] << " " << hydro_data.linearDrag[4] << " " << hydro_data.linearDrag[5] << endl;
662 : 0 : std::cout << " Quadratic drag : " << hydro_data.quadraticDrag[0] << " " << hydro_data.quadraticDrag[1] << " " << hydro_data.quadraticDrag[2] << " "
663 : 0 : << hydro_data.quadraticDrag[3] << " " << hydro_data.quadraticDrag[4] << " " << hydro_data.quadraticDrag[5] << endl;
664 : : }
665 : :
666 : 96 : data[link_name] = hydro_data;
667 : 862 : }
668 : :
669 : 22 : return true;
670 : 25 : }
671 : : } // namespace Urdf
672 : : } // namespace RobotDynamics
|
