1 /*********************************************************************
2 * Software Ligcense Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35 /* Author: John Hsu */
36
37 #include <locale>
38 #include <sstream>
39 #include <stdexcept>
40 #include <string>
41 #include <urdf_model/joint.h>
42 #include <console_bridge/console.h>
43 #include <tinyxml.h>
44 #include <urdf_parser/urdf_parser.h>
45
46 namespace urdf{
47
48 bool parsePose(Pose &pose, TiXmlElement* xml);
49
parseJointDynamics(JointDynamics & jd,TiXmlElement * config)50 bool parseJointDynamics(JointDynamics &jd, TiXmlElement* config)
51 {
52 jd.clear();
53
54 // Get joint damping
55 const char* damping_str = config->Attribute("damping");
56 if (damping_str == NULL){
57 CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: no damping, defaults to 0");
58 jd.damping = 0;
59 }
60 else
61 {
62 try {
63 jd.damping = strToDouble(damping_str);
64 } catch(std::runtime_error &) {
65 CONSOLE_BRIDGE_logError("damping value (%s) is not a valid float", damping_str);
66 return false;
67 }
68 }
69
70 // Get joint friction
71 const char* friction_str = config->Attribute("friction");
72 if (friction_str == NULL){
73 CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: no friction, defaults to 0");
74 jd.friction = 0;
75 }
76 else
77 {
78 try {
79 jd.friction = strToDouble(friction_str);
80 } catch (std::runtime_error &) {
81 CONSOLE_BRIDGE_logError("friction value (%s) is not a valid float", friction_str);
82 return false;
83 }
84 }
85
86 if (damping_str == NULL && friction_str == NULL)
87 {
88 CONSOLE_BRIDGE_logError("joint dynamics element specified with no damping and no friction");
89 return false;
90 }
91 else{
92 CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: damping %f and friction %f", jd.damping, jd.friction);
93 return true;
94 }
95 }
96
parseJointLimits(JointLimits & jl,TiXmlElement * config)97 bool parseJointLimits(JointLimits &jl, TiXmlElement* config)
98 {
99 jl.clear();
100
101 // Get lower joint limit
102 const char* lower_str = config->Attribute("lower");
103 if (lower_str == NULL){
104 CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no lower, defaults to 0");
105 jl.lower = 0;
106 }
107 else
108 {
109 try {
110 jl.lower = strToDouble(lower_str);
111 } catch (std::runtime_error &) {
112 CONSOLE_BRIDGE_logError("lower value (%s) is not a valid float", lower_str);
113 return false;
114 }
115 }
116
117 // Get upper joint limit
118 const char* upper_str = config->Attribute("upper");
119 if (upper_str == NULL){
120 CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no upper, , defaults to 0");
121 jl.upper = 0;
122 }
123 else
124 {
125 try {
126 jl.upper = strToDouble(upper_str);
127 } catch(std::runtime_error &) {
128 CONSOLE_BRIDGE_logError("upper value (%s) is not a valid float", upper_str);
129 return false;
130 }
131 }
132
133 // Get joint effort limit
134 const char* effort_str = config->Attribute("effort");
135 if (effort_str == NULL){
136 CONSOLE_BRIDGE_logError("joint limit: no effort");
137 return false;
138 }
139 else
140 {
141 try {
142 jl.effort = strToDouble(effort_str);
143 } catch(std::runtime_error &) {
144 CONSOLE_BRIDGE_logError("effort value (%s) is not a valid float", effort_str);
145 return false;
146 }
147 }
148
149 // Get joint velocity limit
150 const char* velocity_str = config->Attribute("velocity");
151 if (velocity_str == NULL){
152 CONSOLE_BRIDGE_logError("joint limit: no velocity");
153 return false;
154 }
155 else
156 {
157 try {
158 jl.velocity = strToDouble(velocity_str);
159 } catch(std::runtime_error &) {
160 CONSOLE_BRIDGE_logError("velocity value (%s) is not a valid float", velocity_str);
161 return false;
162 }
163 }
164
165 return true;
166 }
167
parseJointSafety(JointSafety & js,TiXmlElement * config)168 bool parseJointSafety(JointSafety &js, TiXmlElement* config)
169 {
170 js.clear();
171
172 // Get soft_lower_limit joint limit
173 const char* soft_lower_limit_str = config->Attribute("soft_lower_limit");
174 if (soft_lower_limit_str == NULL)
175 {
176 CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no soft_lower_limit, using default value");
177 js.soft_lower_limit = 0;
178 }
179 else
180 {
181 try {
182 js.soft_lower_limit = strToDouble(soft_lower_limit_str);
183 } catch(std::runtime_error &) {
184 CONSOLE_BRIDGE_logError("soft_lower_limit value (%s) is not a valid float", soft_lower_limit_str);
185 return false;
186 }
187 }
188
189 // Get soft_upper_limit joint limit
190 const char* soft_upper_limit_str = config->Attribute("soft_upper_limit");
191 if (soft_upper_limit_str == NULL)
192 {
193 CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no soft_upper_limit, using default value");
194 js.soft_upper_limit = 0;
195 }
196 else
197 {
198 try {
199 js.soft_upper_limit = strToDouble(soft_upper_limit_str);
200 } catch(std::runtime_error &) {
201 CONSOLE_BRIDGE_logError("soft_upper_limit value (%s) is not a valid float", soft_upper_limit_str);
202 return false;
203 }
204 }
205
206 // Get k_position_ safety "position" gain - not exactly position gain
207 const char* k_position_str = config->Attribute("k_position");
208 if (k_position_str == NULL)
209 {
210 CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no k_position, using default value");
211 js.k_position = 0;
212 }
213 else
214 {
215 try {
216 js.k_position = strToDouble(k_position_str);
217 } catch(std::runtime_error &) {
218 CONSOLE_BRIDGE_logError("k_position value (%s) is not a valid float", k_position_str);
219 return false;
220 }
221 }
222 // Get k_velocity_ safety velocity gain
223 const char* k_velocity_str = config->Attribute("k_velocity");
224 if (k_velocity_str == NULL)
225 {
226 CONSOLE_BRIDGE_logError("joint safety: no k_velocity");
227 return false;
228 }
229 else
230 {
231 try {
232 js.k_velocity = strToDouble(k_velocity_str);
233 } catch(std::runtime_error &) {
234 CONSOLE_BRIDGE_logError("k_velocity value (%s) is not a valid float", k_velocity_str);
235 return false;
236 }
237 }
238
239 return true;
240 }
241
parseJointCalibration(JointCalibration & jc,TiXmlElement * config)242 bool parseJointCalibration(JointCalibration &jc, TiXmlElement* config)
243 {
244 jc.clear();
245
246 // Get rising edge position
247 const char* rising_position_str = config->Attribute("rising");
248 if (rising_position_str == NULL)
249 {
250 CONSOLE_BRIDGE_logDebug("urdfdom.joint_calibration: no rising, using default value");
251 jc.rising.reset();
252 }
253 else
254 {
255 try {
256 jc.rising.reset(new double(strToDouble(rising_position_str)));
257 } catch(std::runtime_error &) {
258 CONSOLE_BRIDGE_logError("rising value (%s) is not a valid float", rising_position_str);
259 return false;
260 }
261 }
262
263 // Get falling edge position
264 const char* falling_position_str = config->Attribute("falling");
265 if (falling_position_str == NULL)
266 {
267 CONSOLE_BRIDGE_logDebug("urdfdom.joint_calibration: no falling, using default value");
268 jc.falling.reset();
269 }
270 else
271 {
272 try {
273 jc.falling.reset(new double(strToDouble(falling_position_str)));
274 } catch(std::runtime_error &) {
275 CONSOLE_BRIDGE_logError("falling value (%s) is not a valid float", falling_position_str);
276 return false;
277 }
278 }
279
280 return true;
281 }
282
parseJointMimic(JointMimic & jm,TiXmlElement * config)283 bool parseJointMimic(JointMimic &jm, TiXmlElement* config)
284 {
285 jm.clear();
286
287 // Get name of joint to mimic
288 const char* joint_name_str = config->Attribute("joint");
289
290 if (joint_name_str == NULL)
291 {
292 CONSOLE_BRIDGE_logError("joint mimic: no mimic joint specified");
293 return false;
294 }
295 else
296 jm.joint_name = joint_name_str;
297
298 // Get mimic multiplier
299 const char* multiplier_str = config->Attribute("multiplier");
300
301 if (multiplier_str == NULL)
302 {
303 CONSOLE_BRIDGE_logDebug("urdfdom.joint_mimic: no multiplier, using default value of 1");
304 jm.multiplier = 1;
305 }
306 else
307 {
308 try {
309 jm.multiplier = strToDouble(multiplier_str);
310 } catch(std::runtime_error &) {
311 CONSOLE_BRIDGE_logError("multiplier value (%s) is not a valid float", multiplier_str);
312 return false;
313 }
314 }
315
316
317 // Get mimic offset
318 const char* offset_str = config->Attribute("offset");
319 if (offset_str == NULL)
320 {
321 CONSOLE_BRIDGE_logDebug("urdfdom.joint_mimic: no offset, using default value of 0");
322 jm.offset = 0;
323 }
324 else
325 {
326 try {
327 jm.offset = strToDouble(offset_str);
328 } catch(std::runtime_error &) {
329 CONSOLE_BRIDGE_logError("offset value (%s) is not a valid float", offset_str);
330 return false;
331 }
332 }
333
334 return true;
335 }
336
parseJoint(Joint & joint,TiXmlElement * config)337 bool parseJoint(Joint &joint, TiXmlElement* config)
338 {
339 joint.clear();
340
341 // Get Joint Name
342 const char *name = config->Attribute("name");
343 if (!name)
344 {
345 CONSOLE_BRIDGE_logError("unnamed joint found");
346 return false;
347 }
348 joint.name = name;
349
350 // Get transform from Parent Link to Joint Frame
351 TiXmlElement *origin_xml = config->FirstChildElement("origin");
352 if (!origin_xml)
353 {
354 CONSOLE_BRIDGE_logDebug("urdfdom: Joint [%s] missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", joint.name.c_str());
355 joint.parent_to_joint_origin_transform.clear();
356 }
357 else
358 {
359 if (!parsePose(joint.parent_to_joint_origin_transform, origin_xml))
360 {
361 joint.parent_to_joint_origin_transform.clear();
362 CONSOLE_BRIDGE_logError("Malformed parent origin element for joint [%s]", joint.name.c_str());
363 return false;
364 }
365 }
366
367 // Get Parent Link
368 TiXmlElement *parent_xml = config->FirstChildElement("parent");
369 if (parent_xml)
370 {
371 const char *pname = parent_xml->Attribute("link");
372 if (!pname)
373 {
374 CONSOLE_BRIDGE_logInform("no parent link name specified for Joint link [%s]. this might be the root?", joint.name.c_str());
375 }
376 else
377 {
378 joint.parent_link_name = std::string(pname);
379 }
380 }
381
382 // Get Child Link
383 TiXmlElement *child_xml = config->FirstChildElement("child");
384 if (child_xml)
385 {
386 const char *pname = child_xml->Attribute("link");
387 if (!pname)
388 {
389 CONSOLE_BRIDGE_logInform("no child link name specified for Joint link [%s].", joint.name.c_str());
390 }
391 else
392 {
393 joint.child_link_name = std::string(pname);
394 }
395 }
396
397 // Get Joint type
398 const char* type_char = config->Attribute("type");
399 if (!type_char)
400 {
401 CONSOLE_BRIDGE_logError("joint [%s] has no type, check to see if it's a reference.", joint.name.c_str());
402 return false;
403 }
404
405 std::string type_str = type_char;
406 if (type_str == "planar")
407 joint.type = Joint::PLANAR;
408 else if (type_str == "floating")
409 joint.type = Joint::FLOATING;
410 else if (type_str == "revolute")
411 joint.type = Joint::REVOLUTE;
412 else if (type_str == "continuous")
413 joint.type = Joint::CONTINUOUS;
414 else if (type_str == "prismatic")
415 joint.type = Joint::PRISMATIC;
416 else if (type_str == "fixed")
417 joint.type = Joint::FIXED;
418 else
419 {
420 CONSOLE_BRIDGE_logError("Joint [%s] has no known type [%s]", joint.name.c_str(), type_str.c_str());
421 return false;
422 }
423
424 // Get Joint Axis
425 if (joint.type != Joint::FLOATING && joint.type != Joint::FIXED)
426 {
427 // axis
428 TiXmlElement *axis_xml = config->FirstChildElement("axis");
429 if (!axis_xml){
430 CONSOLE_BRIDGE_logDebug("urdfdom: no axis elemement for Joint link [%s], defaulting to (1,0,0) axis", joint.name.c_str());
431 joint.axis = Vector3(1.0, 0.0, 0.0);
432 }
433 else{
434 if (axis_xml->Attribute("xyz")){
435 try {
436 joint.axis.init(axis_xml->Attribute("xyz"));
437 }
438 catch (ParseError &e) {
439 joint.axis.clear();
440 CONSOLE_BRIDGE_logError("Malformed axis element for joint [%s]: %s", joint.name.c_str(), e.what());
441 return false;
442 }
443 }
444 }
445 }
446
447 // Get limit
448 TiXmlElement *limit_xml = config->FirstChildElement("limit");
449 if (limit_xml)
450 {
451 joint.limits.reset(new JointLimits());
452 if (!parseJointLimits(*joint.limits, limit_xml))
453 {
454 CONSOLE_BRIDGE_logError("Could not parse limit element for joint [%s]", joint.name.c_str());
455 joint.limits.reset();
456 return false;
457 }
458 }
459 else if (joint.type == Joint::REVOLUTE)
460 {
461 CONSOLE_BRIDGE_logError("Joint [%s] is of type REVOLUTE but it does not specify limits", joint.name.c_str());
462 return false;
463 }
464 else if (joint.type == Joint::PRISMATIC)
465 {
466 CONSOLE_BRIDGE_logError("Joint [%s] is of type PRISMATIC without limits", joint.name.c_str());
467 return false;
468 }
469
470 // Get safety
471 TiXmlElement *safety_xml = config->FirstChildElement("safety_controller");
472 if (safety_xml)
473 {
474 joint.safety.reset(new JointSafety());
475 if (!parseJointSafety(*joint.safety, safety_xml))
476 {
477 CONSOLE_BRIDGE_logError("Could not parse safety element for joint [%s]", joint.name.c_str());
478 joint.safety.reset();
479 return false;
480 }
481 }
482
483 // Get calibration
484 TiXmlElement *calibration_xml = config->FirstChildElement("calibration");
485 if (calibration_xml)
486 {
487 joint.calibration.reset(new JointCalibration());
488 if (!parseJointCalibration(*joint.calibration, calibration_xml))
489 {
490 CONSOLE_BRIDGE_logError("Could not parse calibration element for joint [%s]", joint.name.c_str());
491 joint.calibration.reset();
492 return false;
493 }
494 }
495
496 // Get Joint Mimic
497 TiXmlElement *mimic_xml = config->FirstChildElement("mimic");
498 if (mimic_xml)
499 {
500 joint.mimic.reset(new JointMimic());
501 if (!parseJointMimic(*joint.mimic, mimic_xml))
502 {
503 CONSOLE_BRIDGE_logError("Could not parse mimic element for joint [%s]", joint.name.c_str());
504 joint.mimic.reset();
505 return false;
506 }
507 }
508
509 // Get Dynamics
510 TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
511 if (prop_xml)
512 {
513 joint.dynamics.reset(new JointDynamics());
514 if (!parseJointDynamics(*joint.dynamics, prop_xml))
515 {
516 CONSOLE_BRIDGE_logError("Could not parse joint_dynamics element for joint [%s]", joint.name.c_str());
517 joint.dynamics.reset();
518 return false;
519 }
520 }
521
522 return true;
523 }
524
525
526 /* exports */
527 bool exportPose(Pose &pose, TiXmlElement* xml);
528
exportJointDynamics(JointDynamics & jd,TiXmlElement * xml)529 bool exportJointDynamics(JointDynamics &jd, TiXmlElement* xml)
530 {
531 TiXmlElement *dynamics_xml = new TiXmlElement("dynamics");
532 dynamics_xml->SetAttribute("damping", urdf_export_helpers::values2str(jd.damping) );
533 dynamics_xml->SetAttribute("friction", urdf_export_helpers::values2str(jd.friction) );
534 xml->LinkEndChild(dynamics_xml);
535 return true;
536 }
537
exportJointLimits(JointLimits & jl,TiXmlElement * xml)538 bool exportJointLimits(JointLimits &jl, TiXmlElement* xml)
539 {
540 TiXmlElement *limit_xml = new TiXmlElement("limit");
541 limit_xml->SetAttribute("effort", urdf_export_helpers::values2str(jl.effort) );
542 limit_xml->SetAttribute("velocity", urdf_export_helpers::values2str(jl.velocity) );
543 limit_xml->SetAttribute("lower", urdf_export_helpers::values2str(jl.lower) );
544 limit_xml->SetAttribute("upper", urdf_export_helpers::values2str(jl.upper) );
545 xml->LinkEndChild(limit_xml);
546 return true;
547 }
548
exportJointSafety(JointSafety & js,TiXmlElement * xml)549 bool exportJointSafety(JointSafety &js, TiXmlElement* xml)
550 {
551 TiXmlElement *safety_xml = new TiXmlElement("safety_controller");
552 safety_xml->SetAttribute("k_position", urdf_export_helpers::values2str(js.k_position) );
553 safety_xml->SetAttribute("k_velocity", urdf_export_helpers::values2str(js.k_velocity) );
554 safety_xml->SetAttribute("soft_lower_limit", urdf_export_helpers::values2str(js.soft_lower_limit) );
555 safety_xml->SetAttribute("soft_upper_limit", urdf_export_helpers::values2str(js.soft_upper_limit) );
556 xml->LinkEndChild(safety_xml);
557 return true;
558 }
559
exportJointCalibration(JointCalibration & jc,TiXmlElement * xml)560 bool exportJointCalibration(JointCalibration &jc, TiXmlElement* xml)
561 {
562 if (jc.falling || jc.rising)
563 {
564 TiXmlElement *calibration_xml = new TiXmlElement("calibration");
565 if (jc.falling)
566 calibration_xml->SetAttribute("falling", urdf_export_helpers::values2str(*jc.falling) );
567 if (jc.rising)
568 calibration_xml->SetAttribute("rising", urdf_export_helpers::values2str(*jc.rising) );
569 //calibration_xml->SetAttribute("reference_position", urdf_export_helpers::values2str(jc.reference_position) );
570 xml->LinkEndChild(calibration_xml);
571 }
572 return true;
573 }
574
exportJointMimic(JointMimic & jm,TiXmlElement * xml)575 bool exportJointMimic(JointMimic &jm, TiXmlElement* xml)
576 {
577 if (!jm.joint_name.empty())
578 {
579 TiXmlElement *mimic_xml = new TiXmlElement("mimic");
580 mimic_xml->SetAttribute("offset", urdf_export_helpers::values2str(jm.offset) );
581 mimic_xml->SetAttribute("multiplier", urdf_export_helpers::values2str(jm.multiplier) );
582 mimic_xml->SetAttribute("joint", jm.joint_name );
583 xml->LinkEndChild(mimic_xml);
584 }
585 return true;
586 }
587
exportJoint(Joint & joint,TiXmlElement * xml)588 bool exportJoint(Joint &joint, TiXmlElement* xml)
589 {
590 TiXmlElement * joint_xml = new TiXmlElement("joint");
591 joint_xml->SetAttribute("name", joint.name);
592 if (joint.type == urdf::Joint::PLANAR)
593 joint_xml->SetAttribute("type", "planar");
594 else if (joint.type == urdf::Joint::FLOATING)
595 joint_xml->SetAttribute("type", "floating");
596 else if (joint.type == urdf::Joint::REVOLUTE)
597 joint_xml->SetAttribute("type", "revolute");
598 else if (joint.type == urdf::Joint::CONTINUOUS)
599 joint_xml->SetAttribute("type", "continuous");
600 else if (joint.type == urdf::Joint::PRISMATIC)
601 joint_xml->SetAttribute("type", "prismatic");
602 else if (joint.type == urdf::Joint::FIXED)
603 joint_xml->SetAttribute("type", "fixed");
604 else
605 CONSOLE_BRIDGE_logError("ERROR: Joint [%s] type [%d] is not a defined type.\n",joint.name.c_str(), joint.type);
606
607 // origin
608 exportPose(joint.parent_to_joint_origin_transform, joint_xml);
609
610 // axis
611 TiXmlElement * axis_xml = new TiXmlElement("axis");
612 axis_xml->SetAttribute("xyz", urdf_export_helpers::values2str(joint.axis));
613 joint_xml->LinkEndChild(axis_xml);
614
615 // parent
616 TiXmlElement * parent_xml = new TiXmlElement("parent");
617 parent_xml->SetAttribute("link", joint.parent_link_name);
618 joint_xml->LinkEndChild(parent_xml);
619
620 // child
621 TiXmlElement * child_xml = new TiXmlElement("child");
622 child_xml->SetAttribute("link", joint.child_link_name);
623 joint_xml->LinkEndChild(child_xml);
624
625 if (joint.dynamics)
626 exportJointDynamics(*(joint.dynamics), joint_xml);
627 if (joint.limits)
628 exportJointLimits(*(joint.limits), joint_xml);
629 if (joint.safety)
630 exportJointSafety(*(joint.safety), joint_xml);
631 if (joint.calibration)
632 exportJointCalibration(*(joint.calibration), joint_xml);
633 if (joint.mimic)
634 exportJointMimic(*(joint.mimic), joint_xml);
635
636 xml->LinkEndChild(joint_xml);
637 return true;
638 }
639
640
641
642 }
643