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
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052 #include "pr2_mechanism_model/pr2_gripper_transmission.h"
00053 #include <pluginlib/class_list_macros.h>
00054 #include <algorithm>
00055 #include <numeric>
00056 #include <angles/angles.h>
00057 #include <boost/lexical_cast.hpp>
00058
00059 using namespace pr2_hardware_interface;
00060 using namespace pr2_mechanism_model;
00061
00062 PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::PR2GripperTransmission,
00063 pr2_mechanism_model::Transmission)
00064
00065 bool PR2GripperTransmission::initXml(TiXmlElement *config, Robot *robot)
00066 {
00067 const char *name = config->Attribute("name");
00068 name_ = name ? name : "";
00069
00070 TiXmlElement *ael = config->FirstChildElement("actuator");
00071 const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00072 if (!actuator_name || !robot->getActuator(actuator_name))
00073 {
00074 ROS_ERROR("PR2GripperTransmission could not find actuator named \"%s\"", actuator_name);
00075 return false;
00076 }
00077 robot->getActuator(actuator_name)->command_.enable_ = true;
00078 actuator_names_.push_back(actuator_name);
00079
00080 for (TiXmlElement *j = config->FirstChildElement("gap_joint"); j; j = j->NextSiblingElement("gap_joint"))
00081 {
00082 const char *joint_name = j->Attribute("name");
00083 if (!joint_name)
00084 {
00085 ROS_ERROR("PR2GripperTransmission did not specify joint name");
00086 return false;
00087 }
00088
00089 const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
00090 if (!joint)
00091 {
00092 ROS_ERROR("PR2GripperTransmission could not find joint named \"%s\"", joint_name);
00093 return false;
00094 }
00095 gap_joint_ = std::string(joint_name);
00096 joint_names_.push_back(joint_name);
00097
00098
00099 const char *joint_reduction = j->Attribute("mechanical_reduction");
00100 if (!joint_reduction)
00101 {
00102 ROS_ERROR("PR2GripperTransmission's joint \"%s\" has no coefficient: mechanical reduction.", joint_name);
00103 return false;
00104 }
00105 try
00106 {
00107 gap_mechanical_reduction_ = boost::lexical_cast<double>(joint_reduction);
00108 }
00109 catch (boost::bad_lexical_cast &e)
00110 {
00111 ROS_ERROR("joint_reduction (%s) is not a float",joint_reduction);
00112 return false;
00113 }
00114
00115
00116 const char *screw_reduction_str = j->Attribute("screw_reduction");
00117 if (screw_reduction_str == NULL)
00118 {
00119 screw_reduction_ = 2.0/1000.0;
00120 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: screw drive reduction, using default for PR2 alpha2.", joint_name);
00121 }
00122 else
00123 try
00124 {
00125 screw_reduction_ = boost::lexical_cast<double>(screw_reduction_str);
00126 }
00127 catch (boost::bad_lexical_cast &e)
00128 {
00129 ROS_ERROR("screw_reduction (%s) is not a float",screw_reduction_str);
00130 return false;
00131 }
00132
00133
00134
00135 const char *gear_ratio_str = j->Attribute("gear_ratio");
00136 if (gear_ratio_str == NULL)
00137 {
00138 gear_ratio_ = 29.16;
00139 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: gear_ratio, using default for PR2 alpha2.", joint_name);
00140 }
00141 else
00142 try
00143 {
00144 gear_ratio_ = boost::lexical_cast<double>(gear_ratio_str);
00145 }
00146 catch (boost::bad_lexical_cast &e)
00147 {
00148 ROS_ERROR("gear_ratio (%s) is not a float",gear_ratio_str);
00149 return false;
00150 }
00151
00152
00153 const char *theta0_str = j->Attribute("theta0");
00154 if (theta0_str == NULL)
00155 {
00156 theta0_ = 2.97571*M_PI/180.0;
00157 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: theta0, using default for PR2 alpha2.", joint_name);
00158 }
00159 else
00160 try
00161 {
00162 theta0_ = boost::lexical_cast<double>(theta0_str);
00163 }
00164 catch (boost::bad_lexical_cast &e)
00165 {
00166 ROS_ERROR("theta0 (%s) is not a float",theta0_str);
00167 return false;
00168 }
00169
00170 const char *phi0_str = j->Attribute("phi0");
00171 if (phi0_str == NULL)
00172 {
00173 phi0_ = 29.98717*M_PI/180.0;
00174 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: phi0, using default for PR2 alpha2.", joint_name);
00175 }
00176 else
00177 try
00178 {
00179 phi0_ = boost::lexical_cast<double>(phi0_str);
00180 }
00181 catch (boost::bad_lexical_cast &e)
00182 {
00183 ROS_ERROR("phi0 (%s) is not a float",phi0_str);
00184 return false;
00185 }
00186
00187 const char *t0_str = j->Attribute("t0");
00188 if (t0_str == NULL)
00189 {
00190 t0_ = -0.19543/1000.0;
00191 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: t0, using default for PR2 alpha2.", joint_name);
00192 }
00193 else
00194 try
00195 {
00196 t0_ = boost::lexical_cast<double>(t0_str);
00197 }
00198 catch (boost::bad_lexical_cast &e)
00199 {
00200 ROS_ERROR("t0 (%s) is not a float",t0_str);
00201 return false;
00202 }
00203
00204 const char *L0_str = j->Attribute("L0");
00205 if (L0_str == NULL)
00206 {
00207 L0_ = 34.70821/1000.0;
00208 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: L0, using default for PR2 alpha2.", joint_name);
00209 }
00210 else
00211 try
00212 {
00213 L0_ = boost::lexical_cast<double>(L0_str);
00214 }
00215 catch (boost::bad_lexical_cast &e)
00216 {
00217 ROS_ERROR("L0 (%s) is not a float",L0_str);
00218 return false;
00219 }
00220
00221 const char *h_str = j->Attribute("h");
00222 if (h_str == NULL)
00223 {
00224 h_ = 5.200/1000.0;
00225 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: h, using default for PR2 alpha2.", joint_name);
00226 }
00227 else
00228 try
00229 {
00230 h_ = boost::lexical_cast<double>(h_str);
00231 }
00232 catch (boost::bad_lexical_cast &e)
00233 {
00234 ROS_ERROR("h (%s) is not a float",h_str);
00235 return false;
00236 }
00237
00238 const char *a_str = j->Attribute("a");
00239 if (a_str == NULL)
00240 {
00241 a_ = 67.56801/1000.0;
00242 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: a, using default for PR2 alpha2.", joint_name);
00243 }
00244 else
00245 try
00246 {
00247 a_ = boost::lexical_cast<double>(a_str);
00248 }
00249 catch (boost::bad_lexical_cast &e)
00250 {
00251 ROS_ERROR("a (%s) is not a float",a_str);
00252 return false;
00253 }
00254
00255 const char *b_str = j->Attribute("b");
00256 if (b_str == NULL)
00257 {
00258 b_ = 48.97193/1000.0;
00259 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: b, using default for PR2 alpha2.", joint_name);
00260 }
00261 else
00262 try
00263 {
00264 b_ = boost::lexical_cast<double>(b_str);
00265 }
00266 catch (boost::bad_lexical_cast &e)
00267 {
00268 ROS_ERROR("b (%s) is not a float",b_str);
00269 return false;
00270 }
00271
00272 const char *r_str = j->Attribute("r");
00273 if (r_str == NULL)
00274 {
00275 r_ = 91.50000/1000.0;
00276 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: r, using default for PR2 alpha2.", joint_name);
00277 }
00278 else
00279 try
00280 {
00281 r_ = boost::lexical_cast<double>(r_str);
00282 }
00283 catch (boost::bad_lexical_cast &e)
00284 {
00285 ROS_ERROR("r (%s) is not a float",r_str);
00286 return false;
00287 }
00288 }
00289
00290
00291 ROS_DEBUG("Gripper transmission parameters for %s: a=%f, b=%f, r=%f, h=%f, L0=%f, t0=%f, theta0=%f, phi0=%f, gear_ratio=%f, screw_red=%f",
00292 name_.c_str(), a_, b_, r_, h_, L0_, t0_, theta0_, phi0_, gear_ratio_, screw_reduction_);
00293
00294
00295 for (TiXmlElement *j = config->FirstChildElement("passive_joint"); j; j = j->NextSiblingElement("passive_joint"))
00296 {
00297 const char *joint_name = j->Attribute("name");
00298 if (!joint_name)
00299 {
00300 ROS_ERROR("PR2GripperTransmission did not specify joint name");
00301 return false;
00302 }
00303 const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
00304
00305 if (!joint)
00306 {
00307 ROS_ERROR("PR2GripperTransmission could not find joint named \"%s\"", joint_name);
00308 return false;
00309 }
00310
00311
00312 joint_names_.push_back(joint_name);
00313 passive_joints_.push_back(joint_name);
00314 }
00315
00316
00317 for (TiXmlElement *j = config->FirstChildElement("simulated_actuated_joint"); j; j = j->NextSiblingElement("simulated_actuated_joint"))
00318 {
00319 const char *joint_name = j->Attribute("name");
00320 if (!joint_name)
00321 {
00322 ROS_ERROR("PR2GripperTransmission simulated_actuated_joint did snot specify joint name");
00323 use_simulated_actuated_joint_=false;
00324 }
00325 else
00326 {
00327 const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
00328 if (!joint)
00329 {
00330 ROS_ERROR("PR2GripperTransmission could not find joint named \"%s\"", joint_name);
00331 use_simulated_actuated_joint_=false;
00332 }
00333 else
00334 {
00335 use_simulated_actuated_joint_=true;
00336 joint_names_.push_back(joint_name);
00337
00338
00339 const char *simulated_reduction = j->Attribute("simulated_reduction");
00340 if (!simulated_reduction)
00341 {
00342 ROS_ERROR("PR2GripperTransmission's joint \"%s\" has no coefficient: simulated_reduction.", joint_name);
00343 return false;
00344 }
00345 try
00346 {
00347 simulated_reduction_ = boost::lexical_cast<double>(simulated_reduction);
00348 }
00349 catch (boost::bad_lexical_cast &e)
00350 {
00351 ROS_ERROR("simulated_reduction (%s) is not a float",simulated_reduction);
00352 return false;
00353 }
00354
00355
00356
00357
00358
00359
00360
00361 const char *passive_actuated_joint_name = j->Attribute("passive_actuated_joint");
00362 if (passive_actuated_joint_name)
00363 {
00364 const boost::shared_ptr<const urdf::Joint> passive_actuated_joint = robot->robot_model_.getJoint(passive_actuated_joint_name);
00365 if (passive_actuated_joint)
00366 {
00367 has_simulated_passive_actuated_joint_ = true;
00368 joint_names_.push_back(passive_actuated_joint_name);
00369 }
00370 }
00371
00372 }
00373 }
00374 }
00375
00376
00377
00378 return true;
00379 }
00380
00381 bool PR2GripperTransmission::initXml(TiXmlElement *config)
00382 {
00383 const char *name = config->Attribute("name");
00384 name_ = name ? name : "";
00385
00386 TiXmlElement *ael = config->FirstChildElement("actuator");
00387 const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00388 if (!actuator_name)
00389 {
00390 ROS_ERROR("PR2GripperTransmission could not find actuator named \"%s\"", actuator_name);
00391 return false;
00392 }
00393 actuator_names_.push_back(actuator_name);
00394
00395 for (TiXmlElement *j = config->FirstChildElement("gap_joint"); j; j = j->NextSiblingElement("gap_joint"))
00396 {
00397 const char *joint_name = j->Attribute("name");
00398 if (!joint_name)
00399 {
00400 ROS_ERROR("PR2GripperTransmission did not specify joint name");
00401 return false;
00402 }
00403 gap_joint_ = std::string(joint_name);
00404 joint_names_.push_back(joint_name);
00405
00406
00407 const char *joint_reduction = j->Attribute("mechanical_reduction");
00408 if (!joint_reduction)
00409 {
00410 ROS_ERROR("PR2GripperTransmission's joint \"%s\" has no coefficient: mechanical reduction.", joint_name);
00411 return false;
00412 }
00413 try
00414 {
00415 gap_mechanical_reduction_ = boost::lexical_cast<double>(joint_reduction);
00416 }
00417 catch (boost::bad_lexical_cast &e)
00418 {
00419 ROS_ERROR("joint_reduction (%s) is not a float",joint_reduction);
00420 return false;
00421 }
00422
00423
00424 const char *screw_reduction_str = j->Attribute("screw_reduction");
00425 if (screw_reduction_str == NULL)
00426 {
00427 screw_reduction_ = 2.0/1000.0;
00428 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: screw drive reduction, using default for PR2 alpha2.", joint_name);
00429 }
00430 else
00431 try
00432 {
00433 screw_reduction_ = boost::lexical_cast<double>(screw_reduction_str);
00434 }
00435 catch (boost::bad_lexical_cast &e)
00436 {
00437 ROS_ERROR("screw_reduction (%s) is not a float",screw_reduction_str);
00438 return false;
00439 }
00440
00441
00442
00443 const char *gear_ratio_str = j->Attribute("gear_ratio");
00444 if (gear_ratio_str == NULL)
00445 {
00446 gear_ratio_ = 29.16;
00447 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: gear_ratio, using default for PR2 alpha2.", joint_name);
00448 }
00449 else
00450 try
00451 {
00452 gear_ratio_ = boost::lexical_cast<double>(gear_ratio_str);
00453 }
00454 catch (boost::bad_lexical_cast &e)
00455 {
00456 ROS_ERROR("gear_ratio (%s) is not a float",gear_ratio_str);
00457 return false;
00458 }
00459
00460
00461
00462 const char *theta0_str = j->Attribute("theta0");
00463 if (theta0_str == NULL)
00464 {
00465 theta0_ = 2.97571*M_PI/180.0;
00466 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: theta0, using default for PR2 alpha2.", joint_name);
00467 }
00468 else
00469 try
00470 {
00471 theta0_ = boost::lexical_cast<double>(theta0_str);
00472 }
00473 catch (boost::bad_lexical_cast &e)
00474 {
00475 ROS_ERROR("theta0 (%s) is not a float",theta0_str);
00476 return false;
00477 }
00478
00479 const char *phi0_str = j->Attribute("phi0");
00480 if (phi0_str == NULL)
00481 {
00482 phi0_ = 29.98717*M_PI/180.0;
00483 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: phi0, using default for PR2 alpha2.", joint_name);
00484 }
00485 else
00486 try
00487 {
00488 phi0_ = boost::lexical_cast<double>(phi0_str);
00489 }
00490 catch (boost::bad_lexical_cast &e)
00491 {
00492 ROS_ERROR("phi0 (%s) is not a float",phi0_str);
00493 return false;
00494 }
00495
00496 const char *t0_str = j->Attribute("t0");
00497 if (t0_str == NULL)
00498 {
00499 t0_ = -0.19543/1000.0;
00500 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: t0, using default for PR2 alpha2.", joint_name);
00501 }
00502 else
00503 try
00504 {
00505 t0_ = boost::lexical_cast<double>(t0_str);
00506 }
00507 catch (boost::bad_lexical_cast &e)
00508 {
00509 ROS_ERROR("t0 (%s) is not a float",t0_str);
00510 return false;
00511 }
00512
00513 const char *L0_str = j->Attribute("L0");
00514 if (L0_str == NULL)
00515 {
00516 L0_ = 34.70821/1000.0;
00517 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: L0, using default for PR2 alpha2.", joint_name);
00518 }
00519 else
00520 try
00521 {
00522 L0_ = boost::lexical_cast<double>(L0_str);
00523 }
00524 catch (boost::bad_lexical_cast &e)
00525 {
00526 ROS_ERROR("L0 (%s) is not a float",L0_str);
00527 return false;
00528 }
00529
00530 const char *h_str = j->Attribute("h");
00531 if (h_str == NULL)
00532 {
00533 h_ = 5.200/1000.0;
00534 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: h, using default for PR2 alpha2.", joint_name);
00535 }
00536 else
00537 try
00538 {
00539 h_ = boost::lexical_cast<double>(h_str);
00540 }
00541 catch (boost::bad_lexical_cast &e)
00542 {
00543 ROS_ERROR("h (%s) is not a float",h_str);
00544 return false;
00545 }
00546
00547 const char *a_str = j->Attribute("a");
00548 if (a_str == NULL)
00549 {
00550 a_ = 67.56801/1000.0;
00551 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: a, using default for PR2 alpha2.", joint_name);
00552 }
00553 else
00554 try
00555 {
00556 a_ = boost::lexical_cast<double>(a_str);
00557 }
00558 catch (boost::bad_lexical_cast &e)
00559 {
00560 ROS_ERROR("a (%s) is not a float",a_str);
00561 return false;
00562 }
00563
00564 const char *b_str = j->Attribute("b");
00565 if (b_str == NULL)
00566 {
00567 b_ = 48.97193/1000.0;
00568 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: b, using default for PR2 alpha2.", joint_name);
00569 }
00570 else
00571 try
00572 {
00573 b_ = boost::lexical_cast<double>(b_str);
00574 }
00575 catch (boost::bad_lexical_cast &e)
00576 {
00577 ROS_ERROR("b (%s) is not a float",b_str);
00578 return false;
00579 }
00580
00581 const char *r_str = j->Attribute("r");
00582 if (r_str == NULL)
00583 {
00584 r_ = 91.50000/1000.0;
00585 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: r, using default for PR2 alpha2.", joint_name);
00586 }
00587 else
00588 try
00589 {
00590 r_ = boost::lexical_cast<double>(r_str);
00591 }
00592 catch (boost::bad_lexical_cast &e)
00593 {
00594 ROS_ERROR("r (%s) is not a float",r_str);
00595 return false;
00596 }
00597 }
00598
00599
00600 ROS_DEBUG("Gripper transmission parameters for %s: a=%f, b=%f, r=%f, h=%f, L0=%f, t0=%f, theta0=%f, phi0=%f, gear_ratio=%f, screw_red=%f",
00601 name_.c_str(), a_, b_, r_, h_, L0_, t0_, theta0_, phi0_, gear_ratio_, screw_reduction_);
00602
00603
00604 for (TiXmlElement *j = config->FirstChildElement("passive_joint"); j; j = j->NextSiblingElement("passive_joint"))
00605 {
00606 const char *joint_name = j->Attribute("name");
00607 if (!joint_name)
00608 {
00609 ROS_ERROR("PR2GripperTransmission did not specify joint name");
00610 return false;
00611 }
00612
00613
00614 joint_names_.push_back(joint_name);
00615 passive_joints_.push_back(joint_name);
00616 }
00617
00618
00619 for (TiXmlElement *j = config->FirstChildElement("simulated_actuated_joint"); j; j = j->NextSiblingElement("simulated_actuated_joint"))
00620 {
00621 const char *joint_name = j->Attribute("name");
00622 if (!joint_name)
00623 {
00624 ROS_ERROR("PR2GripperTransmission screw joint did not specify joint name");
00625 use_simulated_actuated_joint_=false;
00626 }
00627 else
00628 {
00629 use_simulated_actuated_joint_=true;
00630 joint_names_.push_back(joint_name);
00631
00632
00633 const char *simulated_reduction = j->Attribute("simulated_reduction");
00634 if (!simulated_reduction)
00635 {
00636 ROS_ERROR("PR2GripperTransmission's joint \"%s\" has no coefficient: simulated_reduction.", joint_name);
00637 return false;
00638 }
00639 try
00640 {
00641 simulated_reduction_ = boost::lexical_cast<double>(simulated_reduction);
00642 }
00643 catch (boost::bad_lexical_cast &e)
00644 {
00645 ROS_ERROR("simulated_reduction (%s) is not a float",simulated_reduction);
00646 return false;
00647 }
00648
00649
00650
00651
00652
00653
00654
00655 const char *passive_actuated_joint_name = j->Attribute("passive_actuated_joint");
00656 if (passive_actuated_joint_name)
00657 {
00658 has_simulated_passive_actuated_joint_ = true;
00659 joint_names_.push_back(passive_actuated_joint_name);
00660 }
00661
00662 }
00663 }
00664
00665
00666
00667 return true;
00668 }
00669
00672 void PR2GripperTransmission::computeGapStates(
00673 double MR,double MR_dot,double MT,
00674 double &theta,double &dtheta_dMR,double &dt_dtheta,double &dt_dMR,double &gap_size,double &gap_velocity,double &gap_effort)
00675 {
00676
00677
00678
00679 double u = (a_*a_+b_*b_-h_*h_
00680 -pow(L0_+MR*screw_reduction_/gear_ratio_,2))/(2.0*a_*b_);
00681 u = u < -1.0 ? -1.0 : u > 1.0 ? 1.0 : u;
00682 theta = theta0_ - phi0_ + acos(u);
00683
00684
00685
00686
00687
00688 gap_size = t0_ + r_ * ( sin(theta) - sin(theta0_) );
00689
00690
00691
00692
00693
00694 MR = MR >= 0.0 ? MR : 0.0;
00695
00696 u = (a_*a_+b_*b_-h_*h_
00697 -pow(L0_+MR*screw_reduction_/gear_ratio_,2))/(2.0*a_*b_);
00698 u = u < -1.0 ? -1.0 : u > 1.0 ? 1.0 : u;
00699 double tmp_theta = theta0_ - phi0_ + acos(u);
00700
00701
00702 double arg = 1.0 - pow(u,2);
00703 arg = arg > TOL ? arg : TOL;
00704
00705 double du_dMR = -(L0_ * screw_reduction_)/(gear_ratio_*a_*b_)
00706 -MR/(a_*b_)*pow(screw_reduction_/gear_ratio_,2);
00707
00708 dtheta_dMR = -1.0/sqrt(arg) * du_dMR;
00709
00710 dt_dtheta = r_ * cos( tmp_theta );
00711 dt_dMR = dt_dtheta * dtheta_dMR;
00712 gap_velocity = MR_dot * dt_dMR;
00713
00714
00715
00716
00717
00718
00719
00720
00721 gap_effort = MT / dt_dMR / RAD2MR ;
00722
00723 }
00724
00729 void PR2GripperTransmission::inverseGapStates(
00730 double gap_size,double &MR,double &dMR_dtheta,double &dtheta_dt,double &dMR_dt)
00731 {
00732
00733 double sin_theta = (gap_size - t0_)/r_ + sin(theta0_);
00734 sin_theta = sin_theta > 1.0 ? 1.0 : sin_theta < -1.0 ? -1.0 : sin_theta;
00735 double theta = asin(sin_theta);
00736
00737
00738 inverseGapStates1(theta,MR,dMR_dtheta,dtheta_dt,dMR_dt);
00739 }
00740
00745 void PR2GripperTransmission::inverseGapStates1(
00746 double theta,double &MR,double &dMR_dtheta,double &dtheta_dt,double &dMR_dt)
00747 {
00748
00749
00750
00751
00752
00753
00754 double arg = -2.0*a_*b_*cos(theta-theta0_+phi0_)
00755 -h_*h_+a_*a_+b_*b_;
00756 if (arg > 0.0)
00757 {
00758 MR = gear_ratio_/screw_reduction_ * ( sqrt(arg) - L0_ );
00759 dMR_dtheta = gear_ratio_/(2.0 * screw_reduction_) / sqrt(arg)
00760 * 2.0 * a_ * b_ * sin(theta + phi0_ - theta0_);
00761 }
00762 else
00763 {
00764 MR = gear_ratio_/screw_reduction_ * ( 0.0 - L0_ );
00765 dMR_dtheta = 0.0;
00766 }
00767
00768
00769 double gap_size = t0_ + r_ * ( sin(theta) - sin(theta0_) );
00770
00771
00772
00773
00774
00775 double tmp_dMR_dtheta = fabs(dMR_dtheta);
00776
00777 double u = (gap_size - t0_)/r_ + sin(theta0_);
00778 double arg2 = 1.0 - pow(u,2);
00779 arg2 = arg2 > TOL ? arg2 : TOL;
00780 dtheta_dt = 1.0 / sqrt( arg2 ) / r_;
00781 dMR_dt = tmp_dMR_dtheta * dtheta_dt;
00782 }
00783
00784
00788 void PR2GripperTransmission::propagatePosition(
00789 std::vector<Actuator*>& as, std::vector<JointState*>& js)
00790 {
00791
00792 ROS_ASSERT(as.size() == 1);
00793
00794 if (use_simulated_actuated_joint_ && has_simulated_passive_actuated_joint_)
00795 {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);}
00796 else if (use_simulated_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 1);}
00797 else {ROS_ASSERT(js.size() == 1 + passive_joints_.size());}
00798
00802 double MR = as[0]->state_.position_ / gap_mechanical_reduction_ * RAD2MR ;
00803
00805 double MR_dot = as[0]->state_.velocity_ / gap_mechanical_reduction_ * RAD2MR ;
00806
00816
00817
00818
00820 double MT = as[0]->state_.last_measured_effort_ / gap_mechanical_reduction_;
00821
00823 double theta, dtheta_dMR, dt_dtheta, dt_dMR;
00825 double gap_size,gap_velocity,gap_effort;
00826
00827
00828 computeGapStates(MR,MR_dot,MT,theta,dtheta_dMR, dt_dtheta, dt_dMR,gap_size,gap_velocity,gap_effort);
00829
00830
00831 js[0]->position_ = gap_size*2.0;
00832 js[0]->velocity_ = gap_velocity*2.0;
00833 js[0]->measured_effort_ = gap_effort/2.0;
00834
00835
00836
00837
00838 for (size_t i = 1; i < passive_joints_.size()+1; ++i)
00839 {
00840 js[i]->position_ = theta - theta0_;
00841 js[i]->velocity_ = dtheta_dMR * MR_dot;
00842 js[i]->measured_effort_ = MT / dtheta_dMR / RAD2MR;
00843 js[i]->reference_position_ = MT / dtheta_dMR / RAD2MR;
00844 }
00845
00846 if (use_simulated_actuated_joint_)
00847 {
00848
00849 js[passive_joints_.size()+1]->position_ = 0.0;
00850 js[passive_joints_.size()+1]->velocity_ = 0.0;
00851 js[passive_joints_.size()+1]->measured_effort_ = 0.0;
00852 js[passive_joints_.size()+1]->reference_position_ = 0.0;
00853 js[passive_joints_.size()+1]->calibrated_ = true;
00854 }
00855 if (has_simulated_passive_actuated_joint_)
00856 {
00857
00858 js[passive_joints_.size()+2]->position_ = 0.0;
00859 js[passive_joints_.size()+2]->velocity_ = 0.0;
00860 js[passive_joints_.size()+2]->measured_effort_ = 0.0;
00861 js[passive_joints_.size()+2]->reference_position_ = 0.0;
00862 js[passive_joints_.size()+2]->calibrated_ = true;
00863 }
00864 }
00865
00866
00867 void PR2GripperTransmission::propagatePositionBackwards(
00868 std::vector<JointState*>& js, std::vector<Actuator*>& as)
00869 {
00870 ROS_ASSERT(as.size() == 1);
00871 if (use_simulated_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 1);}
00872 else {ROS_ASSERT(js.size() == 1 + passive_joints_.size());}
00873
00874
00875 double MR,dMR_dtheta,dtheta_dt,dMR_dt;
00876 double joint_rate;
00877 {
00878
00879
00880 double gap_size = js[0]->position_/2.0;
00881
00882 inverseGapStates(gap_size,MR,dMR_dtheta,dtheta_dt,dMR_dt);
00883 double gap_rate = js[0]->velocity_/2.0;
00884 joint_rate = gap_rate * dtheta_dt;
00885 }
00886 double gap_effort = js[0]->commanded_effort_;
00887
00888
00889
00891 as[0]->state_.position_ = MR * gap_mechanical_reduction_ / RAD2MR ;
00892
00895 as[0]->state_.velocity_ = joint_rate * dMR_dtheta * gap_mechanical_reduction_ / RAD2MR ;
00896
00900 as[0]->state_.last_measured_effort_ = 2.0*gap_effort / dMR_dt * RAD2MR * gap_mechanical_reduction_;
00901
00902
00903 if (! simulated_actuator_timestamp_initialized_)
00904 {
00905
00906 as[0]->state_.sample_timestamp_ = ros::Duration(0);
00907
00908
00909 if (ros::isStarted())
00910 {
00911 simulated_actuator_start_time_ = ros::Time::now();
00912 simulated_actuator_timestamp_initialized_ = true;
00913 }
00914 }
00915 else
00916 {
00917
00918 as[0]->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
00919 }
00920
00921 as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.toSec();
00922
00923
00924 this->joint_calibration_simulator_.simulateJointCalibration(js[0],as[0]);
00925 }
00926
00927 void PR2GripperTransmission::propagateEffort(
00928 std::vector<JointState*>& js, std::vector<Actuator*>& as)
00929 {
00930 ROS_ASSERT(as.size() == 1);
00931 if (use_simulated_actuated_joint_ && has_simulated_passive_actuated_joint_)
00932 {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);}
00933 else if (use_simulated_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 1);}
00934 else {ROS_ASSERT(js.size() == 1 + passive_joints_.size());}
00935
00936
00937
00938
00939
00940
00941
00942 double gap_size = js[0]->position_/2.0;
00943
00944
00945 double MR,dMR_dtheta,dtheta_dt,dMR_dt;
00946
00947 inverseGapStates(gap_size,MR,dMR_dtheta,dtheta_dt,dMR_dt);
00948
00949 double gap_effort = js[0]->commanded_effort_;
00950
00951
00952
00954 as[0]->command_.enable_ = true;
00955 as[0]->command_.effort_ = 2.0*gap_effort / dMR_dt * RAD2MR * gap_mechanical_reduction_;
00956 }
00957
00958 void PR2GripperTransmission::propagateEffortBackwards(
00959 std::vector<Actuator*>& as, std::vector<JointState*>& js)
00960 {
00961 ROS_ASSERT(as.size() == 1);
00962 if (use_simulated_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 1);}
00963 else {ROS_ASSERT(js.size() == 1 + passive_joints_.size());}
00964 ROS_ASSERT(simulated_reduction_>0.0);
00965
00966
00967
00968
00970 double MR = as[0]->state_.position_ / gap_mechanical_reduction_ * RAD2MR ;
00971 double MR_dot = as[0]->state_.velocity_ / gap_mechanical_reduction_ * RAD2MR ;
00972 double MT = as[0]->command_.effort_ / gap_mechanical_reduction_;
00973
00975 double theta, dtheta_dMR, dt_dtheta, dt_dMR;
00977 double gap_size,gap_velocity,gap_effort;
00978
00979
00980 computeGapStates(MR,MR_dot,MT,theta,dtheta_dMR, dt_dtheta, dt_dMR,gap_size,gap_velocity,gap_effort);
00981
00982 {
00983
00984
00985 if (use_simulated_actuated_joint_)
00986 {
00987
00988 js[passive_joints_.size()+1]->commanded_effort_ = gap_effort/simulated_reduction_;
00989
00990 }
00991 else
00992 {
00993
00994 double eps=0.01;
00995 js[0]->commanded_effort_ = (1.0-eps)*js[0]->commanded_effort_ + eps*gap_effort/2.0;
00996 }
00997 }
00998 }
00999