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
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078 #include "pr2_mechanism_model/pr2_gripper_transmission.h"
00079 #include <pluginlib/class_list_macros.h>
00080 #include <algorithm>
00081 #include <numeric>
00082 #include <angles/angles.h>
00083 #include <boost/lexical_cast.hpp>
00084
00085 #define PASSIVE_JOINTS 1
00086
00087 using namespace pr2_hardware_interface;
00088 using namespace pr2_mechanism_model;
00089
00090 PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::PR2GripperTransmission,
00091 pr2_mechanism_model::Transmission)
00092
00093 bool PR2GripperTransmission::initXml(TiXmlElement *config, Robot *robot)
00094 {
00095 if (this->initXml(config))
00096 {
00097
00098 for (std::vector<std::string>::iterator actuator_name = actuator_names_.begin(); actuator_name != actuator_names_.end(); ++actuator_name)
00099 {
00100 if (robot->getActuator(*actuator_name))
00101 {
00102 robot->getActuator(*actuator_name)->command_.enable_ = true;
00103 }
00104 else
00105 {
00106 ROS_ERROR("PR2GripperTransmission actuator named \"%s\" not loaded in Robot", actuator_name->c_str());
00107 return false;
00108 }
00109 }
00110
00111
00112 for (std::vector<std::string>::iterator joint_name = joint_names_.begin(); joint_name != joint_names_.end(); ++joint_name)
00113 {
00114 if (!robot->robot_model_.getJoint(*joint_name))
00115 {
00116 ROS_ERROR("PR2GripperTransmission joint named \"%s\" not loaded in Robot", joint_name->c_str());
00117 return false;
00118 }
00119 }
00120
00121
00122 return true;
00123 }
00124 else
00125 return false;
00126 }
00127
00128 bool PR2GripperTransmission::initXml(TiXmlElement *config)
00129 {
00130 const char *name = config->Attribute("name");
00131 name_ = name ? name : "";
00132
00133 TiXmlElement *ael = config->FirstChildElement("actuator");
00134 const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00135 if (!actuator_name)
00136 {
00137 ROS_ERROR("PR2GripperTransmission could not find actuator named \"%s\"", actuator_name);
00138 return false;
00139 }
00140 actuator_names_.push_back(actuator_name);
00141
00142 for (TiXmlElement *j = config->FirstChildElement("gap_joint"); j; j = j->NextSiblingElement("gap_joint"))
00143 {
00144 const char *gap_joint_name = j->Attribute("name");
00145 if (!gap_joint_name)
00146 {
00147 ROS_ERROR("PR2GripperTransmission did not specify joint name");
00148 return false;
00149 }
00150 gap_joint_ = std::string(gap_joint_name);
00151 joint_names_.push_back(gap_joint_name);
00152
00153
00154 const char *joint_reduction = j->Attribute("mechanical_reduction");
00155 if (!joint_reduction)
00156 {
00157 ROS_ERROR("PR2GripperTransmission's joint \"%s\" has no coefficient: mechanical reduction.", gap_joint_name);
00158 return false;
00159 }
00160 try
00161 {
00162 gap_mechanical_reduction_ = boost::lexical_cast<double>(joint_reduction);
00163 }
00164 catch (boost::bad_lexical_cast &e)
00165 {
00166 ROS_ERROR("joint_reduction (%s) is not a float",joint_reduction);
00167 return false;
00168 }
00169
00170
00171 const char *screw_reduction_str = j->Attribute("screw_reduction");
00172 if (screw_reduction_str == NULL)
00173 {
00174 screw_reduction_ = 2.0/1000.0;
00175 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: screw drive reduction, using default for PR2 alpha2.", gap_joint_name);
00176 }
00177 else
00178 try
00179 {
00180 screw_reduction_ = boost::lexical_cast<double>(screw_reduction_str);
00181 }
00182 catch (boost::bad_lexical_cast &e)
00183 {
00184 ROS_ERROR("screw_reduction (%s) is not a float",screw_reduction_str);
00185 return false;
00186 }
00187
00188
00189 const char *gear_ratio_str = j->Attribute("gear_ratio");
00190 if (gear_ratio_str == NULL)
00191 {
00192 gear_ratio_ = 29.16;
00193 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: gear_ratio, using default for PR2 alpha2.", gap_joint_name);
00194 }
00195 else
00196 try
00197 {
00198 gear_ratio_ = boost::lexical_cast<double>(gear_ratio_str);
00199 }
00200 catch (boost::bad_lexical_cast &e)
00201 {
00202 ROS_ERROR("gear_ratio (%s) is not a float",gear_ratio_str);
00203 return false;
00204 }
00205
00206
00207 const char *theta0_str = j->Attribute("theta0");
00208 if (theta0_str == NULL)
00209 {
00210 theta0_ = 2.97571*M_PI/180.0;
00211 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: theta0, using default for PR2 alpha2.", gap_joint_name);
00212 }
00213 else
00214 try
00215 {
00216 theta0_ = boost::lexical_cast<double>(theta0_str);
00217 }
00218 catch (boost::bad_lexical_cast &e)
00219 {
00220 ROS_ERROR("theta0 (%s) is not a float",theta0_str);
00221 return false;
00222 }
00223
00224 const char *phi0_str = j->Attribute("phi0");
00225 if (phi0_str == NULL)
00226 {
00227 phi0_ = 29.98717*M_PI/180.0;
00228 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: phi0, using default for PR2 alpha2.", gap_joint_name);
00229 }
00230 else
00231 try
00232 {
00233 phi0_ = boost::lexical_cast<double>(phi0_str);
00234 }
00235 catch (boost::bad_lexical_cast &e)
00236 {
00237 ROS_ERROR("phi0 (%s) is not a float",phi0_str);
00238 return false;
00239 }
00240
00241 const char *t0_str = j->Attribute("t0");
00242 if (t0_str == NULL)
00243 {
00244 t0_ = -0.19543/1000.0;
00245 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: t0, using default for PR2 alpha2.", gap_joint_name);
00246 }
00247 else
00248 try
00249 {
00250 t0_ = boost::lexical_cast<double>(t0_str);
00251 }
00252 catch (boost::bad_lexical_cast &e)
00253 {
00254 ROS_ERROR("t0 (%s) is not a float",t0_str);
00255 return false;
00256 }
00257
00258 const char *L0_str = j->Attribute("L0");
00259 if (L0_str == NULL)
00260 {
00261 L0_ = 34.70821/1000.0;
00262 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: L0, using default for PR2 alpha2.", gap_joint_name);
00263 }
00264 else
00265 try
00266 {
00267 L0_ = boost::lexical_cast<double>(L0_str);
00268 }
00269 catch (boost::bad_lexical_cast &e)
00270 {
00271 ROS_ERROR("L0 (%s) is not a float",L0_str);
00272 return false;
00273 }
00274
00275 const char *h_str = j->Attribute("h");
00276 if (h_str == NULL)
00277 {
00278 h_ = 5.200/1000.0;
00279 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: h, using default for PR2 alpha2.", gap_joint_name);
00280 }
00281 else
00282 try
00283 {
00284 h_ = boost::lexical_cast<double>(h_str);
00285 }
00286 catch (boost::bad_lexical_cast &e)
00287 {
00288 ROS_ERROR("h (%s) is not a float",h_str);
00289 return false;
00290 }
00291
00292 const char *a_str = j->Attribute("a");
00293 if (a_str == NULL)
00294 {
00295 a_ = 67.56801/1000.0;
00296 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: a, using default for PR2 alpha2.", gap_joint_name);
00297 }
00298 else
00299 try
00300 {
00301 a_ = boost::lexical_cast<double>(a_str);
00302 }
00303 catch (boost::bad_lexical_cast &e)
00304 {
00305 ROS_ERROR("a (%s) is not a float",a_str);
00306 return false;
00307 }
00308
00309 const char *b_str = j->Attribute("b");
00310 if (b_str == NULL)
00311 {
00312 b_ = 48.97193/1000.0;
00313 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: b, using default for PR2 alpha2.", gap_joint_name);
00314 }
00315 else
00316 try
00317 {
00318 b_ = boost::lexical_cast<double>(b_str);
00319 }
00320 catch (boost::bad_lexical_cast &e)
00321 {
00322 ROS_ERROR("b (%s) is not a float",b_str);
00323 return false;
00324 }
00325
00326 const char *r_str = j->Attribute("r");
00327 if (r_str == NULL)
00328 {
00329 r_ = 91.50000/1000.0;
00330 ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: r, using default for PR2 alpha2.", gap_joint_name);
00331 }
00332 else
00333 try
00334 {
00335 r_ = boost::lexical_cast<double>(r_str);
00336 }
00337 catch (boost::bad_lexical_cast &e)
00338 {
00339 ROS_ERROR("r (%s) is not a float",r_str);
00340 return false;
00341 }
00342 }
00343
00344
00345 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",
00346 name_.c_str(), a_, b_, r_, h_, L0_, t0_, theta0_, phi0_, gear_ratio_, screw_reduction_);
00347
00348
00349 for (TiXmlElement *j = config->FirstChildElement("passive_joint"); j; j = j->NextSiblingElement("passive_joint"))
00350 {
00351 const char *passive_joint_name = j->Attribute("name");
00352 if (!passive_joint_name)
00353 {
00354 ROS_ERROR("PR2GripperTransmission did not specify joint name");
00355 return false;
00356 }
00357
00358
00359 #if PASSIVE_JOINTS
00360 joint_names_.push_back(passive_joint_name);
00361 #endif
00362 passive_joints_.push_back(passive_joint_name);
00363 }
00364
00365
00366 for (TiXmlElement *j = config->FirstChildElement("simulated_actuated_joint"); j; j = j->NextSiblingElement("simulated_actuated_joint"))
00367 {
00368 const char *simulated_actuated_joint_name = j->Attribute("name");
00369 if (simulated_actuated_joint_name)
00370 {
00371 #if PASSIVE_JOINTS
00372 joint_names_.push_back(simulated_actuated_joint_name);
00373 #endif
00374 }
00375 else
00376 {
00377 ROS_ERROR("PR2GripperTransmission simulated_actuated_joint did snot specify joint name");
00378 return false;
00379 }
00380
00381
00382 const char *simulated_reduction = j->Attribute("simulated_reduction");
00383 if (!simulated_reduction)
00384 {
00385 ROS_ERROR("PR2GripperTransmission's simulated_actuated_joint \"%s\" has no coefficient: simulated_reduction.", simulated_actuated_joint_name);
00386 return false;
00387 }
00388 try
00389 {
00390 simulated_reduction_ = boost::lexical_cast<double>(simulated_reduction);
00391 }
00392 catch (boost::bad_lexical_cast &e)
00393 {
00394 ROS_ERROR("simulated_reduction (%s) is not a float",simulated_reduction);
00395 return false;
00396 }
00397
00398
00399
00400
00401
00402
00403
00404 const char *passive_actuated_joint_name = j->Attribute("passive_actuated_joint");
00405 if (passive_actuated_joint_name)
00406 {
00407 has_simulated_passive_actuated_joint_ = true;
00408 #if PASSIVE_JOINTS
00409 joint_names_.push_back(passive_actuated_joint_name);
00410 #endif
00411 }
00412 }
00413
00414
00415
00416 return true;
00417 }
00418
00421 void PR2GripperTransmission::computeGapStates(
00422 double MR,double MR_dot,double MT,
00423 double &theta,double &dtheta_dMR,double &dt_dtheta,double &dt_dMR,double &gap_size,double &gap_velocity,double &gap_effort)
00424 {
00425
00426
00427
00428 double u = (a_*a_+b_*b_-h_*h_
00429 -pow(L0_+MR*screw_reduction_/gear_ratio_,2))/(2.0*a_*b_);
00430 u = u < -1.0 ? -1.0 : u > 1.0 ? 1.0 : u;
00431 theta = theta0_ - phi0_ + acos(u);
00432
00433
00434
00435
00436
00437 gap_size = t0_ + r_ * ( sin(theta) - sin(theta0_) );
00438
00439
00440
00441
00442
00443 MR = MR >= 0.0 ? MR : 0.0;
00444
00445 u = (a_*a_+b_*b_-h_*h_
00446 -pow(L0_+MR*screw_reduction_/gear_ratio_,2))/(2.0*a_*b_);
00447 u = u < -1.0 ? -1.0 : u > 1.0 ? 1.0 : u;
00448 double tmp_theta = theta0_ - phi0_ + acos(u);
00449
00450
00451 double arg = 1.0 - pow(u,2);
00452 arg = arg > TOL ? arg : TOL;
00453
00454 double du_dMR = -(L0_ * screw_reduction_)/(gear_ratio_*a_*b_)
00455 -MR/(a_*b_)*pow(screw_reduction_/gear_ratio_,2);
00456
00457 dtheta_dMR = -1.0/sqrt(arg) * du_dMR;
00458
00459 dt_dtheta = r_ * cos( tmp_theta );
00460 dt_dMR = dt_dtheta * dtheta_dMR;
00461 gap_velocity = MR_dot * dt_dMR;
00462
00463
00464
00465
00466
00467
00468
00469
00470 gap_effort = MT / dt_dMR / RAD2MR ;
00471
00472 }
00473
00478 void PR2GripperTransmission::inverseGapStates(
00479 double gap_size,double &MR,double &dMR_dtheta,double &dtheta_dt,double &dMR_dt)
00480 {
00481
00482 double sin_theta = (gap_size - t0_)/r_ + sin(theta0_);
00483 sin_theta = sin_theta > 1.0 ? 1.0 : sin_theta < -1.0 ? -1.0 : sin_theta;
00484 double theta = asin(sin_theta);
00485
00486
00487 inverseGapStates1(theta,MR,dMR_dtheta,dtheta_dt,dMR_dt);
00488 }
00489
00494 void PR2GripperTransmission::inverseGapStates1(
00495 double theta,double &MR,double &dMR_dtheta,double &dtheta_dt,double &dMR_dt)
00496 {
00497
00498
00499
00500
00501
00502
00503 double arg = -2.0*a_*b_*cos(theta-theta0_+phi0_)
00504 -h_*h_+a_*a_+b_*b_;
00505 if (arg > 0.0)
00506 {
00507 MR = gear_ratio_/screw_reduction_ * ( sqrt(arg) - L0_ );
00508 dMR_dtheta = gear_ratio_/(2.0 * screw_reduction_) / sqrt(arg)
00509 * 2.0 * a_ * b_ * sin(theta + phi0_ - theta0_);
00510 }
00511 else
00512 {
00513 MR = gear_ratio_/screw_reduction_ * ( 0.0 - L0_ );
00514 dMR_dtheta = 0.0;
00515 }
00516
00517
00518 double gap_size = t0_ + r_ * ( sin(theta) - sin(theta0_) );
00519
00520
00521
00522
00523
00524 double tmp_dMR_dtheta = fabs(dMR_dtheta);
00525
00526 double u = (gap_size - t0_)/r_ + sin(theta0_);
00527 double arg2 = 1.0 - pow(u,2);
00528 arg2 = arg2 > TOL ? arg2 : TOL;
00529 dtheta_dt = 1.0 / sqrt( arg2 ) / r_;
00530 dMR_dt = tmp_dMR_dtheta * dtheta_dt;
00531 }
00532
00533
00537 void PR2GripperTransmission::propagatePosition(
00538 std::vector<Actuator*>& as, std::vector<JointState*>& js)
00539 {
00540
00541 ROS_ASSERT(as.size() == 1);
00542
00543 #if PASSIVE_JOINTS
00544 if (has_simulated_passive_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);}
00545 else ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 1);
00546 #endif
00547
00551 double MR = as[0]->state_.position_ / gap_mechanical_reduction_ * RAD2MR ;
00552
00554 double MR_dot = as[0]->state_.velocity_ / gap_mechanical_reduction_ * RAD2MR ;
00555
00565
00566
00567
00569 double MT = as[0]->state_.last_measured_effort_ / gap_mechanical_reduction_;
00570
00572 double theta, dtheta_dMR, dt_dtheta, dt_dMR;
00574 double gap_size,gap_velocity,gap_effort;
00575
00576
00577 computeGapStates(MR,MR_dot,MT,theta,dtheta_dMR, dt_dtheta, dt_dMR,gap_size,gap_velocity,gap_effort);
00578
00579
00580 js[0]->position_ = gap_size*2.0;
00581 js[0]->velocity_ = gap_velocity*2.0;
00582 js[0]->measured_effort_ = gap_effort/2.0;
00583
00584
00585 #if PASSIVE_JOINTS
00586
00587
00588 for (size_t i = 1; i < passive_joints_.size()+1; ++i)
00589 {
00590 js[i]->position_ = theta - theta0_;
00591 js[i]->velocity_ = dtheta_dMR * MR_dot;
00592 js[i]->measured_effort_ = MT / dtheta_dMR / RAD2MR;
00593 js[i]->reference_position_ = MT / dtheta_dMR / RAD2MR;
00594 }
00595
00596
00597 js[passive_joints_.size()+1]->position_ = 0.0;
00598 js[passive_joints_.size()+1]->velocity_ = 0.0;
00599 js[passive_joints_.size()+1]->measured_effort_ = 0.0;
00600 js[passive_joints_.size()+1]->reference_position_ = 0.0;
00601 js[passive_joints_.size()+1]->calibrated_ = true;
00602
00603 if (has_simulated_passive_actuated_joint_)
00604 {
00605
00606 js[passive_joints_.size()+2]->position_ = 0.0;
00607 js[passive_joints_.size()+2]->velocity_ = 0.0;
00608 js[passive_joints_.size()+2]->measured_effort_ = 0.0;
00609 js[passive_joints_.size()+2]->reference_position_ = 0.0;
00610 js[passive_joints_.size()+2]->calibrated_ = true;
00611 }
00612 #endif
00613 }
00614
00615
00616 void PR2GripperTransmission::propagatePositionBackwards(
00617 std::vector<JointState*>& js, std::vector<Actuator*>& as)
00618 {
00619 ROS_ASSERT(as.size() == 1);
00620 #if PASSIVE_JOINTS
00621 if (has_simulated_passive_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);}
00622 else {ROS_ASSERT(js.size() == 1);}
00623 #endif
00624 ROS_DEBUG("js [%zd], pjs [%zd]", js.size(), passive_joints_.size());
00625
00626
00627 double MR,dMR_dtheta,dtheta_dt,dMR_dt;
00628 double joint_rate;
00629 {
00630
00631
00632 double gap_size = js[0]->position_/2.0;
00633
00634 inverseGapStates(gap_size,MR,dMR_dtheta,dtheta_dt,dMR_dt);
00635 double gap_rate = js[0]->velocity_/2.0;
00636 joint_rate = gap_rate * dtheta_dt;
00637 }
00638 double gap_effort = js[0]->commanded_effort_;
00639
00640
00641
00643 as[0]->state_.position_ = MR * gap_mechanical_reduction_ / RAD2MR ;
00644
00647 as[0]->state_.velocity_ = joint_rate * dMR_dtheta * gap_mechanical_reduction_ / RAD2MR ;
00648
00652 as[0]->state_.last_measured_effort_ = 2.0*gap_effort / dMR_dt * RAD2MR * gap_mechanical_reduction_;
00653
00654
00655 if (!simulated_actuator_timestamp_initialized_)
00656 {
00657
00658 as[0]->state_.sample_timestamp_ = ros::Duration(0);
00659
00660
00661 if (ros::isStarted())
00662 {
00663 simulated_actuator_start_time_ = ros::Time::now();
00664 simulated_actuator_timestamp_initialized_ = true;
00665 }
00666 }
00667 else
00668 {
00669
00670 as[0]->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
00671 }
00672
00673 as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.toSec();
00674
00675
00676 this->joint_calibration_simulator_.simulateJointCalibration(js[0],as[0]);
00677 }
00678
00679 void PR2GripperTransmission::propagateEffort(
00680 std::vector<JointState*>& js, std::vector<Actuator*>& as)
00681 {
00682 ROS_ASSERT(as.size() == 1);
00683 #if PASSIVE_JOINTS
00684 if (has_simulated_passive_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);}
00685 else {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 1);}
00686 #endif
00687
00688
00689
00690
00691
00692
00693
00694 double gap_size = js[0]->position_/2.0;
00695
00696
00697 double MR,dMR_dtheta,dtheta_dt,dMR_dt;
00698
00699 inverseGapStates(gap_size,MR,dMR_dtheta,dtheta_dt,dMR_dt);
00700
00701 double gap_effort = js[0]->commanded_effort_;
00702
00703
00704
00706 as[0]->command_.enable_ = true;
00707 as[0]->command_.effort_ = 2.0*gap_effort / dMR_dt * RAD2MR * gap_mechanical_reduction_;
00708 }
00709
00710 void PR2GripperTransmission::propagateEffortBackwards(
00711 std::vector<Actuator*>& as, std::vector<JointState*>& js)
00712 {
00713 ROS_ASSERT(as.size() == 1);
00714 #if PASSIVE_JOINTS
00715 ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);
00716 #endif
00717 ROS_ASSERT(simulated_reduction_>0.0);
00718
00719
00720
00721
00723 double MR = as[0]->state_.position_ / gap_mechanical_reduction_ * RAD2MR ;
00724 double MR_dot = as[0]->state_.velocity_ / gap_mechanical_reduction_ * RAD2MR ;
00725 double MT = as[0]->command_.effort_ / gap_mechanical_reduction_;
00726
00728 double theta, dtheta_dMR, dt_dtheta, dt_dMR;
00730 double gap_size,gap_velocity,gap_effort;
00731
00732
00733 computeGapStates(MR,MR_dot,MT,theta,dtheta_dMR, dt_dtheta, dt_dMR,gap_size,gap_velocity,gap_effort);
00734
00735 #if PASSIVE_JOINTS
00736 {
00737
00738
00739
00740
00741 js[passive_joints_.size()+1]->commanded_effort_ = gap_effort/simulated_reduction_;
00742 }
00743 #endif
00744 }
00745