00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <urdf/joint.h>
00038 #include <ros/ros.h>
00039 #include <boost/lexical_cast.hpp>
00040
00041 namespace urdf{
00042
00043 bool JointDynamics::initXml(TiXmlElement* config)
00044 {
00045 this->clear();
00046
00047
00048 const char* damping_str = config->Attribute("damping");
00049 if (damping_str == NULL){
00050 ROS_DEBUG("joint dynamics: no damping, defaults to 0");
00051 this->damping = 0;
00052 }
00053 else
00054 {
00055 try
00056 {
00057 this->damping = boost::lexical_cast<double>(damping_str);
00058 }
00059 catch (boost::bad_lexical_cast &e)
00060 {
00061 ROS_ERROR("damping value (%s) is not a float",damping_str);
00062 return false;
00063 }
00064 }
00065
00066
00067 const char* friction_str = config->Attribute("friction");
00068 if (friction_str == NULL){
00069 ROS_DEBUG("joint dynamics: no friction, defaults to 0");
00070 this->friction = 0;
00071 }
00072 else
00073 {
00074 try
00075 {
00076 this->friction = boost::lexical_cast<double>(friction_str);
00077 }
00078 catch (boost::bad_lexical_cast &e)
00079 {
00080 ROS_ERROR("friction value (%s) is not a float",friction_str);
00081 return false;
00082 }
00083 }
00084
00085 if (damping_str == NULL && friction_str == NULL)
00086 {
00087 ROS_ERROR("joint dynamics element specified with no damping and no friction");
00088 return false;
00089 }
00090 else{
00091 ROS_DEBUG("joint dynamics: damping %f and friction %f", damping, friction);
00092 return true;
00093 }
00094 }
00095
00096 bool JointLimits::initXml(TiXmlElement* config)
00097 {
00098 this->clear();
00099
00100
00101 const char* lower_str = config->Attribute("lower");
00102 if (lower_str == NULL){
00103 ROS_DEBUG("joint limit: no lower, defaults to 0");
00104 this->lower = 0;
00105 }
00106 else
00107 {
00108 try
00109 {
00110 this->lower = boost::lexical_cast<double>(lower_str);
00111 }
00112 catch (boost::bad_lexical_cast &e)
00113 {
00114 ROS_ERROR("lower value (%s) is not a float",lower_str);
00115 return false;
00116 }
00117 }
00118
00119
00120 const char* upper_str = config->Attribute("upper");
00121 if (upper_str == NULL){
00122 ROS_DEBUG("joint limit: no upper, , defaults to 0");
00123 this->upper = 0;
00124 }
00125 else
00126 {
00127 try
00128 {
00129 this->upper = boost::lexical_cast<double>(upper_str);
00130 }
00131 catch (boost::bad_lexical_cast &e)
00132 {
00133 ROS_ERROR("upper value (%s) is not a float",upper_str);
00134 return false;
00135 }
00136 }
00137
00138
00139 const char* effort_str = config->Attribute("effort");
00140 if (effort_str == NULL){
00141 ROS_ERROR("joint limit: no effort");
00142 return false;
00143 }
00144 else
00145 {
00146 try
00147 {
00148 this->effort = boost::lexical_cast<double>(effort_str);
00149 }
00150 catch (boost::bad_lexical_cast &e)
00151 {
00152 ROS_ERROR("effort value (%s) is not a float",effort_str);
00153 return false;
00154 }
00155 }
00156
00157
00158 const char* velocity_str = config->Attribute("velocity");
00159 if (velocity_str == NULL){
00160 ROS_ERROR("joint limit: no velocity");
00161 return false;
00162 }
00163 else
00164 {
00165 try
00166 {
00167 this->velocity = boost::lexical_cast<double>(velocity_str);
00168 }
00169 catch (boost::bad_lexical_cast &e)
00170 {
00171 ROS_ERROR("velocity value (%s) is not a float",velocity_str);
00172 return false;
00173 }
00174 }
00175
00176 return true;
00177 }
00178
00179 bool JointSafety::initXml(TiXmlElement* config)
00180 {
00181 this->clear();
00182
00183
00184 const char* soft_lower_limit_str = config->Attribute("soft_lower_limit");
00185 if (soft_lower_limit_str == NULL)
00186 {
00187 ROS_DEBUG("joint safety: no soft_lower_limit, using default value");
00188 this->soft_lower_limit = 0;
00189 }
00190 else
00191 {
00192 try
00193 {
00194 this->soft_lower_limit = boost::lexical_cast<double>(soft_lower_limit_str);
00195 }
00196 catch (boost::bad_lexical_cast &e)
00197 {
00198 ROS_ERROR("soft_lower_limit value (%s) is not a float",soft_lower_limit_str);
00199 return false;
00200 }
00201 }
00202
00203
00204 const char* soft_upper_limit_str = config->Attribute("soft_upper_limit");
00205 if (soft_upper_limit_str == NULL)
00206 {
00207 ROS_DEBUG("joint safety: no soft_upper_limit, using default value");
00208 this->soft_upper_limit = 0;
00209 }
00210 else
00211 {
00212 try
00213 {
00214 this->soft_upper_limit = boost::lexical_cast<double>(soft_upper_limit_str);
00215 }
00216 catch (boost::bad_lexical_cast &e)
00217 {
00218 ROS_ERROR("soft_upper_limit value (%s) is not a float",soft_upper_limit_str);
00219 return false;
00220 }
00221 }
00222
00223
00224 const char* k_position_str = config->Attribute("k_position");
00225 if (k_position_str == NULL)
00226 {
00227 ROS_DEBUG("joint safety: no k_position, using default value");
00228 this->k_position = 0;
00229 }
00230 else
00231 {
00232 try
00233 {
00234 this->k_position = boost::lexical_cast<double>(k_position_str);
00235 }
00236 catch (boost::bad_lexical_cast &e)
00237 {
00238 ROS_ERROR("k_position value (%s) is not a float",k_position_str);
00239 return false;
00240 }
00241 }
00242
00243 const char* k_velocity_str = config->Attribute("k_velocity");
00244 if (k_velocity_str == NULL)
00245 {
00246 ROS_ERROR("joint safety: no k_velocity");
00247 return false;
00248 }
00249 else
00250 {
00251 try
00252 {
00253 this->k_velocity = boost::lexical_cast<double>(k_velocity_str);
00254 }
00255 catch (boost::bad_lexical_cast &e)
00256 {
00257 ROS_ERROR("k_velocity value (%s) is not a float",k_velocity_str);
00258 return false;
00259 }
00260 }
00261
00262 return true;
00263 }
00264
00265 bool JointCalibration::initXml(TiXmlElement* config)
00266 {
00267 this->clear();
00268
00269
00270 const char* rising_position_str = config->Attribute("rising");
00271 if (rising_position_str == NULL)
00272 {
00273 ROS_DEBUG("joint calibration: no rising, using default value");
00274 this->rising.reset();
00275 }
00276 else
00277 {
00278 try
00279 {
00280 this->rising.reset(new double(boost::lexical_cast<double>(rising_position_str)));
00281 }
00282 catch (boost::bad_lexical_cast &e)
00283 {
00284 ROS_ERROR("risingvalue (%s) is not a float",rising_position_str);
00285 return false;
00286 }
00287 }
00288
00289
00290 const char* falling_position_str = config->Attribute("falling");
00291 if (falling_position_str == NULL)
00292 {
00293 ROS_DEBUG("joint calibration: no falling, using default value");
00294 this->falling.reset();
00295 }
00296 else
00297 {
00298 try
00299 {
00300 this->falling.reset(new double(boost::lexical_cast<double>(falling_position_str)));
00301 }
00302 catch (boost::bad_lexical_cast &e)
00303 {
00304 ROS_ERROR("fallingvalue (%s) is not a float",falling_position_str);
00305 return false;
00306 }
00307 }
00308
00309 return true;
00310 }
00311
00312 bool JointMimic::initXml(TiXmlElement* config)
00313 {
00314 this->clear();
00315
00316
00317 const char* joint_name_str = config->Attribute("joint");
00318 if (joint_name_str == NULL)
00319 {
00320 ROS_ERROR("joint mimic: no mimic joint specified");
00321
00322 }
00323 else
00324 this->joint_name = joint_name_str;
00325
00326
00327 const char* multiplier_str = config->Attribute("multiplier");
00328 if (multiplier_str == NULL)
00329 {
00330 ROS_DEBUG("joint mimic: no multiplier, using default value of 1");
00331 this->multiplier = 1;
00332 }
00333 else
00334 {
00335 try
00336 {
00337 this->multiplier = boost::lexical_cast<double>(multiplier_str);
00338 }
00339 catch (boost::bad_lexical_cast &e)
00340 {
00341 ROS_ERROR("multiplier value (%s) is not a float",multiplier_str);
00342 return false;
00343 }
00344 }
00345
00346
00347 const char* offset_str = config->Attribute("offset");
00348 if (offset_str == NULL)
00349 {
00350 ROS_DEBUG("joint mimic: no offset, using default value of 0");
00351 this->offset = 0;
00352 }
00353 else
00354 {
00355 try
00356 {
00357 this->offset = boost::lexical_cast<double>(offset_str);
00358 }
00359 catch (boost::bad_lexical_cast &e)
00360 {
00361 ROS_ERROR("offset value (%s) is not a float",offset_str);
00362 return false;
00363 }
00364 }
00365
00366 return true;
00367 }
00368
00369 bool Joint::initXml(TiXmlElement* config)
00370 {
00371 this->clear();
00372
00373
00374 const char *name = config->Attribute("name");
00375 if (!name)
00376 {
00377 ROS_ERROR("unnamed joint found");
00378 return false;
00379 }
00380 this->name = name;
00381
00382
00383 TiXmlElement *origin_xml = config->FirstChildElement("origin");
00384 if (!origin_xml)
00385 {
00386 ROS_DEBUG("Joint '%s' missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", this->name.c_str());
00387 this->parent_to_joint_origin_transform.clear();
00388 }
00389 else
00390 {
00391 if (!this->parent_to_joint_origin_transform.initXml(origin_xml))
00392 {
00393 ROS_ERROR("Malformed parent origin element for joint '%s'", this->name.c_str());
00394 this->parent_to_joint_origin_transform.clear();
00395 return false;
00396 }
00397 }
00398
00399
00400 TiXmlElement *parent_xml = config->FirstChildElement("parent");
00401 if (parent_xml)
00402 {
00403 const char *pname = parent_xml->Attribute("link");
00404 if (!pname)
00405 ROS_INFO("no parent link name specified for Joint link '%s'. this might be the root?", this->name.c_str());
00406 else
00407 {
00408 this->parent_link_name = std::string(pname);
00409
00410 }
00411 }
00412
00413
00414 TiXmlElement *child_xml = config->FirstChildElement("child");
00415 if (child_xml)
00416 {
00417 const char *pname = child_xml->Attribute("link");
00418 if (!pname)
00419 ROS_INFO("no child link name specified for Joint link '%s'.", this->name.c_str());
00420 else
00421 {
00422 this->child_link_name = std::string(pname);
00423
00424 }
00425 }
00426
00427
00428 const char* type_char = config->Attribute("type");
00429 if (!type_char)
00430 {
00431 ROS_ERROR("joint '%s' has no type, check to see if it's a reference.", this->name.c_str());
00432 return false;
00433 }
00434 std::string type_str = type_char;
00435 if (type_str == "planar")
00436 type = PLANAR;
00437 else if (type_str == "floating")
00438 type = FLOATING;
00439 else if (type_str == "revolute")
00440 type = REVOLUTE;
00441 else if (type_str == "continuous")
00442 type = CONTINUOUS;
00443 else if (type_str == "prismatic")
00444 type = PRISMATIC;
00445 else if (type_str == "fixed")
00446 type = FIXED;
00447 else
00448 {
00449 ROS_ERROR("Joint '%s' has no known type '%s'", this->name.c_str(), type_str.c_str());
00450 return false;
00451 }
00452
00453
00454 if (this->type != FLOATING && this->type != FIXED)
00455 {
00456
00457 TiXmlElement *axis_xml = config->FirstChildElement("axis");
00458 if (!axis_xml){
00459 ROS_DEBUG("no axis elemement for Joint link '%s', defaulting to (1,0,0) axis", this->name.c_str());
00460 this->axis = Vector3(1.0, 0.0, 0.0);
00461 }
00462 else{
00463 if (!axis_xml->Attribute("xyz")){
00464 ROS_ERROR("no xyz attribute for axis element for Joint link '%s'", this->name.c_str());
00465 }
00466 else {
00467 if (!this->axis.init(axis_xml->Attribute("xyz")))
00468 {
00469 ROS_ERROR("Malformed axis element for joint '%s'", this->name.c_str());
00470 this->axis.clear();
00471 return false;
00472 }
00473 }
00474 }
00475 }
00476
00477
00478 TiXmlElement *limit_xml = config->FirstChildElement("limit");
00479 if (limit_xml)
00480 {
00481 limits.reset(new JointLimits);
00482 if (!limits->initXml(limit_xml))
00483 {
00484 ROS_ERROR("Could not parse limit element for joint '%s'", this->name.c_str());
00485 limits.reset();
00486 return false;
00487 }
00488 }
00489 else if (this->type == REVOLUTE)
00490 {
00491 ROS_ERROR("Joint '%s' is of type REVOLUTE but it does not specify limits", this->name.c_str());
00492 return false;
00493 }
00494 else if (this->type == PRISMATIC)
00495 {
00496 ROS_INFO("Joint '%s' is of type PRISMATIC without limits", this->name.c_str());
00497 limits.reset();
00498 }
00499
00500
00501 TiXmlElement *safety_xml = config->FirstChildElement("safety_controller");
00502 if (safety_xml)
00503 {
00504 safety.reset(new JointSafety);
00505 if (!safety->initXml(safety_xml))
00506 {
00507 ROS_ERROR("Could not parse safety element for joint '%s'", this->name.c_str());
00508 safety.reset();
00509 return false;
00510 }
00511 }
00512
00513
00514 TiXmlElement *calibration_xml = config->FirstChildElement("calibration");
00515 if (calibration_xml)
00516 {
00517 calibration.reset(new JointCalibration);
00518 if (!calibration->initXml(calibration_xml))
00519 {
00520 ROS_ERROR("Could not parse calibration element for joint '%s'", this->name.c_str());
00521 calibration.reset();
00522 return false;
00523 }
00524 }
00525
00526
00527 TiXmlElement *mimic_xml = config->FirstChildElement("mimic");
00528 if (mimic_xml)
00529 {
00530 mimic.reset(new JointMimic);
00531 if (!mimic->initXml(mimic_xml))
00532 {
00533 ROS_ERROR("Could not parse mimic element for joint '%s'", this->name.c_str());
00534 mimic.reset();
00535 return false;
00536 }
00537 }
00538
00539
00540 TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
00541 if (prop_xml)
00542 {
00543 dynamics.reset(new JointDynamics);
00544 if (!dynamics->initXml(prop_xml))
00545 {
00546 ROS_ERROR("Could not parse joint_dynamics element for joint '%s'", this->name.c_str());
00547 dynamics.reset();
00548 return false;
00549 }
00550 }
00551
00552 return true;
00553 }
00554
00555
00556
00557 }