Go to the documentation of this file.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 #include <pr2_mechanism_model/pr2_belt_transmission.h>
00036 #include <math.h>
00037 #include <pluginlib/class_list_macros.h>
00038
00039 PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::PR2BeltCompensatorTransmission,
00040 pr2_mechanism_model::Transmission)
00041
00042 namespace pr2_mechanism_model {
00043
00044 bool PR2BeltCompensatorTransmission::initXml(TiXmlElement *elt, Robot *robot)
00045 {
00046 const char *name = elt->Attribute("name");
00047 name_ = name ? name : "";
00048
00049 TiXmlElement *jel = elt->FirstChildElement("joint");
00050 const char *joint_name = jel ? jel->Attribute("name") : NULL;
00051 if (!joint_name)
00052 {
00053 ROS_ERROR("PR2BeltCompensatorTransmission did not specify joint name");
00054 return false;
00055 }
00056
00057 const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
00058 if (!joint)
00059 {
00060 ROS_ERROR("PR2BeltCompensatorTransmission could not find joint named \"%s\"", joint_name);
00061 return false;
00062 }
00063 joint_names_.push_back(joint_name);
00064
00065 TiXmlElement *ael = elt->FirstChildElement("actuator");
00066 const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00067 pr2_hardware_interface::Actuator *a;
00068 if (!actuator_name || (a = robot->getActuator(actuator_name)) == NULL )
00069 {
00070 ROS_ERROR("PR2BeltCompensatorTransmission could not find actuator named \"%s\"", actuator_name);
00071 return false;
00072 }
00073 a->command_.enable_ = true;
00074 actuator_names_.push_back(actuator_name);
00075
00076 mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
00077
00078 TiXmlElement *c = elt->FirstChildElement("compensator");
00079 if (!c)
00080 {
00081 ROS_ERROR("No compensator element given for transmission %s", name_.c_str());
00082 return false;
00083 }
00084
00085 double k_belt;
00086 const char *k_belt_str = c->Attribute("k_belt");
00087 if (!k_belt_str) {
00088 ROS_ERROR("No k_belt given for transmission %s", name_.c_str());
00089 return false;
00090 }
00091 k_belt = atof(k_belt_str);
00092
00093 double mass_motor;
00094 const char *mass_motor_str = c->Attribute("mass_motor");
00095 if (!mass_motor_str) {
00096 ROS_ERROR("No mass_motor given for transmission %s", name_.c_str());
00097 return false;
00098 }
00099 mass_motor = atof(mass_motor_str);
00100
00101 trans_compl_ = (k_belt > 0.0 ? 1.0 / k_belt : 0.0);
00102 trans_tau_ = sqrt(mass_motor * trans_compl_);
00103
00104 const char *kd_motor_str = c->Attribute("kd_motor");
00105 if (!kd_motor_str) {
00106 ROS_ERROR("No kd_motor given for transmission %s", name_.c_str());
00107 return false;
00108 }
00109 Kd_motor_ = atof(kd_motor_str);
00110
00111 const char *lambda_motor_str = c->Attribute("lambda_motor");
00112 if (!lambda_motor_str) {
00113 ROS_ERROR("No lambda_motor given for transmission %s", name_.c_str());
00114 return false;
00115 }
00116 lambda_motor_ = atof(lambda_motor_str);
00117
00118 const char *lambda_joint_str = c->Attribute("lambda_joint");
00119 if (!lambda_joint_str) {
00120 ROS_ERROR("No lambda_joint given for transmission %s", name_.c_str());
00121 return false;
00122 }
00123 lambda_joint_ = atof(lambda_joint_str);
00124
00125 const char *lambda_combined_str = c->Attribute("lambda_combined");
00126 if (!lambda_combined_str) {
00127 ROS_ERROR("No lambda_combined given for transmission %s", name_.c_str());
00128 return false;
00129 }
00130 lambda_combo_ = atof(lambda_combined_str);
00131
00132
00133 last_motor_pos_ = last_motor_vel_ = 0;
00134 last_jnt1_pos_ = last_jnt1_vel_ = last_jnt1_acc_ = 0;
00135 last_defl_pos_ = last_defl_vel_ = last_defl_acc_ = 0;
00136 last_joint_pos_ = last_joint_vel_ = 0;
00137 delta_motor_vel_ = 0;
00138 last_motor_damping_force_ = 0;
00139
00140 last_timestamp_ = ros::Duration(0);
00141
00142
00143 last_timestamp_backwards_ = last_timestamp_;
00144 halfdt_backwards_ = 0.0;
00145 motor_force_backwards_ = 0.0;
00146 last_motor_pos_backwards_ = 0.0;
00147 last_motor_vel_backwards_ = 0.0;
00148 last_motor_acc_backwards_ = 0.0;
00149 last_joint_pos_backwards_ = 0.0;
00150 last_joint_vel_backwards_ = 0.0;
00151
00152 return true;
00153 }
00154
00155 bool PR2BeltCompensatorTransmission::initXml(TiXmlElement *elt)
00156 {
00157 const char *name = elt->Attribute("name");
00158 name_ = name ? name : "";
00159
00160 TiXmlElement *jel = elt->FirstChildElement("joint");
00161 const char *joint_name = jel ? jel->Attribute("name") : NULL;
00162 if (!joint_name)
00163 {
00164 ROS_ERROR("PR2BeltCompensatorTransmission did not specify joint name");
00165 return false;
00166 }
00167
00168 joint_names_.push_back(joint_name);
00169
00170 TiXmlElement *ael = elt->FirstChildElement("actuator");
00171 const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00172 actuator_names_.push_back(actuator_name);
00173
00174 mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
00175
00176 TiXmlElement *c = elt->FirstChildElement("compensator");
00177 if (!c)
00178 {
00179 ROS_ERROR("No compensator element given for transmission %s", name_.c_str());
00180 return false;
00181 }
00182
00183 double k_belt;
00184 const char *k_belt_str = c->Attribute("k_belt");
00185 if (!k_belt_str) {
00186 ROS_ERROR("No k_belt given for transmission %s", name_.c_str());
00187 return false;
00188 }
00189 k_belt = atof(k_belt_str);
00190
00191 double mass_motor;
00192 const char *mass_motor_str = c->Attribute("mass_motor");
00193 if (!mass_motor_str) {
00194 ROS_ERROR("No mass_motor given for transmission %s", name_.c_str());
00195 return false;
00196 }
00197 mass_motor = atof(mass_motor_str);
00198
00199 trans_compl_ = (k_belt > 0.0 ? 1.0 / k_belt : 0.0);
00200 trans_tau_ = sqrt(mass_motor * trans_compl_);
00201
00202 const char *kd_motor_str = c->Attribute("kd_motor");
00203 if (!kd_motor_str) {
00204 ROS_ERROR("No kd_motor given for transmission %s", name_.c_str());
00205 return false;
00206 }
00207 Kd_motor_ = atof(kd_motor_str);
00208
00209 const char *lambda_motor_str = c->Attribute("lambda_motor");
00210 if (!lambda_motor_str) {
00211 ROS_ERROR("No lambda_motor given for transmission %s", name_.c_str());
00212 return false;
00213 }
00214 lambda_motor_ = atof(lambda_motor_str);
00215
00216 const char *lambda_joint_str = c->Attribute("lambda_joint");
00217 if (!lambda_joint_str) {
00218 ROS_ERROR("No lambda_joint given for transmission %s", name_.c_str());
00219 return false;
00220 }
00221 lambda_joint_ = atof(lambda_joint_str);
00222
00223 const char *lambda_combined_str = c->Attribute("lambda_combined");
00224 if (!lambda_combined_str) {
00225 ROS_ERROR("No lambda_combined given for transmission %s", name_.c_str());
00226 return false;
00227 }
00228 lambda_combo_ = atof(lambda_combined_str);
00229
00230
00231 last_motor_pos_ = last_motor_vel_ = 0;
00232 last_jnt1_pos_ = last_jnt1_vel_ = last_jnt1_acc_ = 0;
00233 last_defl_pos_ = last_defl_vel_ = last_defl_acc_ = 0;
00234 last_joint_pos_ = last_joint_vel_ = 0;
00235 delta_motor_vel_ = 0;
00236 last_motor_damping_force_ = 0;
00237
00238 last_timestamp_ = ros::Duration(0);
00239
00240
00241 last_timestamp_backwards_ = last_timestamp_;
00242 halfdt_backwards_ = 0.0;
00243 motor_force_backwards_ = 0.0;
00244 last_motor_pos_backwards_ = 0.0;
00245 last_motor_vel_backwards_ = 0.0;
00246 last_motor_acc_backwards_ = 0.0;
00247 last_joint_pos_backwards_ = 0.0;
00248 last_joint_vel_backwards_ = 0.0;
00249
00250 return true;
00251 }
00252
00253 void PR2BeltCompensatorTransmission::propagatePosition(
00254 std::vector<pr2_hardware_interface::Actuator*>& as, std::vector<JointState*>& js)
00255 {
00256 assert(as.size() == 1);
00257 assert(js.size() == 1);
00258 ros::Duration timestamp = as[0]->state_.sample_timestamp_;
00259 dt = (timestamp - last_timestamp_).toSec();
00260 last_timestamp_ = timestamp;
00261
00262
00263
00264
00265
00266
00267
00268 double motor_pos = as[0]->state_.position_ / mechanical_reduction_;
00269 double motor_vel = ( dt>0.0 ? (motor_pos - last_motor_pos_)/dt : 0.0 );
00270
00271 double motor_measured_force = as[0]->state_.last_measured_effort_ * mechanical_reduction_;
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286 double jnt1_pos, jnt1_vel, jnt1_acc;
00287 double lam = (dt*lambda_joint_ < 2.0 ? lambda_joint_ : 2.0/dt);
00288
00289 jnt1_vel = last_jnt1_vel_ + 0.5*dt*( last_jnt1_acc_);
00290 jnt1_pos = last_jnt1_pos_ + 0.5*dt*(jnt1_vel + last_jnt1_vel_);
00291
00292 jnt1_acc = (lam*lam * (motor_pos-jnt1_pos) - 2.0*lam * jnt1_vel)
00293 / (1.0 + 0.5*dt*2.0*lam + 0.25*dt*dt*lam*lam);
00294
00295 jnt1_vel = last_jnt1_vel_ + 0.5*dt*(jnt1_acc + last_jnt1_acc_);
00296 jnt1_pos = last_jnt1_pos_ + 0.5*dt*(jnt1_vel + last_jnt1_vel_);
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311 double defl_pos, defl_vel, defl_acc;
00312 if (trans_tau_ == 0)
00313 {
00314 defl_acc = 0.0;
00315 defl_vel = 0.0;
00316 defl_pos = trans_compl_ * motor_measured_force;
00317 }
00318 else
00319 {
00320 double tau = (dt < 2.0*trans_tau_ ? trans_tau_ : dt/2.0);
00321
00322 defl_vel = last_defl_vel_ + 0.5*dt*( last_defl_acc_);
00323 defl_pos = last_defl_pos_ + 0.5*dt*(defl_vel + last_defl_vel_);
00324
00325 defl_acc = (trans_compl_ * motor_measured_force
00326 - defl_pos - 2.0*tau * defl_vel)
00327 / (tau*tau + 2.0*tau*0.5*dt + 0.25*dt*dt);
00328
00329 defl_vel = last_defl_vel_ + 0.5*dt*(defl_acc + last_defl_acc_);
00330 defl_pos = last_defl_pos_ + 0.5*dt*(defl_vel + last_defl_vel_);
00331 }
00332
00333 double jnt2_pos = motor_pos - defl_pos;
00334
00335
00336
00337
00338
00339
00340
00341 double joint_pos, joint_vel;
00342 if (lambda_combo_ == 0.0)
00343 {
00344 joint_pos = jnt1_pos;
00345 joint_vel = jnt1_vel;
00346 }
00347 else
00348 {
00349 lam = (dt*lambda_combo_ < 2.0 ? lambda_combo_ : 2.0/dt);
00350
00351 joint_pos = last_joint_pos_ + 0.5*dt*(last_joint_vel_);
00352 joint_vel = (jnt1_vel + lam * (jnt2_pos - joint_pos))
00353 / (1.0 + 0.5*dt*lam);
00354 joint_pos = last_joint_pos_ + 0.5*dt*(joint_vel + last_joint_vel_);
00355 }
00356
00357
00358
00359 js[0]->position_ = joint_pos + js[0]->reference_position_;
00360 js[0]->velocity_ = joint_vel;
00361 js[0]->measured_effort_ = as[0]->state_.last_measured_effort_ * mechanical_reduction_;
00362
00363
00364
00365 delta_motor_vel_ = motor_vel - last_motor_vel_;
00366
00367
00368
00369 last_motor_pos_ = motor_pos;
00370 last_motor_vel_ = motor_vel;
00371
00372 last_jnt1_pos_ = jnt1_pos;
00373 last_jnt1_vel_ = jnt1_vel;
00374 last_jnt1_acc_ = jnt1_acc;
00375
00376 last_defl_pos_ = defl_pos;
00377 last_defl_vel_ = defl_vel;
00378 last_defl_acc_ = defl_acc;
00379
00380 last_joint_pos_ = joint_pos;
00381 last_joint_vel_ = joint_vel;
00382 }
00383
00384 void PR2BeltCompensatorTransmission::propagateEffort(
00385 std::vector<JointState*>& js, std::vector<pr2_hardware_interface::Actuator*>& as)
00386 {
00387 assert(as.size() == 1);
00388 assert(js.size() == 1);
00389
00390
00391
00392
00393
00394
00395
00396 double motor_damping_force;
00397 if (lambda_motor_ == 0.0)
00398 {
00399 motor_damping_force = - Kd_motor_ * last_motor_vel_;
00400 }
00401 else
00402 {
00403 double lam = (dt*lambda_motor_ < 2.0 ? lambda_motor_ : 2.0/dt);
00404
00405 motor_damping_force = ((1.0-0.5*dt*lam) * last_motor_damping_force_
00406 - Kd_motor_ * delta_motor_vel_)
00407 / (1.0+0.5*dt*lam);
00408 }
00409
00410
00411 double motor_force = js[0]->commanded_effort_ + motor_damping_force;
00412
00413
00414 as[0]->command_.effort_ = motor_force / mechanical_reduction_;
00415
00416 last_motor_damping_force_ = motor_damping_force;
00417 }
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432 void PR2BeltCompensatorTransmission::propagateEffortBackwards(
00433 std::vector<pr2_hardware_interface::Actuator*>& as, std::vector<JointState*>& js)
00434 {
00435
00436 assert(as.size() == 1);
00437 assert(js.size() == 1);
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447 double motor_pos, motor_vel, motor_acc;
00448 double joint_pos, joint_vel;
00449 double motor_force;
00450 double spring_force;
00451 double halfdt;
00452
00453
00454
00455
00456
00457
00458
00459
00460 ros::Duration timestamp = as[0]->state_.sample_timestamp_;
00461 halfdt = 0.5*(timestamp - last_timestamp_backwards_).toSec();
00462 last_timestamp_backwards_ = timestamp;
00463
00464
00465
00466
00467
00468 motor_force = as[0]->command_.effort_ * mechanical_reduction_;
00469
00470
00471
00472
00473
00474 if ((trans_compl_ == 0.0) || (trans_tau_ == 0.0))
00475 {
00476
00477
00478 spring_force = motor_force;
00479 }
00480 else
00481 {
00482
00483
00484
00485
00486 double tau = (halfdt < trans_tau_ ? trans_tau_ : halfdt);
00487
00488
00489
00490 motor_vel = last_motor_vel_backwards_ + halfdt*(last_motor_acc_backwards_ + 0 );
00491 motor_pos = last_motor_pos_backwards_ + halfdt*(last_motor_vel_backwards_ + motor_vel);
00492
00493
00494
00495
00496
00497
00498 joint_vel = last_joint_vel_backwards_;
00499 joint_pos = last_joint_pos_backwards_ + halfdt*(last_joint_vel_backwards_ + joint_vel);
00500
00501
00502 spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) / trans_compl_;
00503
00504
00505
00506 motor_acc = (motor_force - spring_force) * trans_compl_ / (tau*tau + 2.0*tau*halfdt + halfdt*halfdt);
00507
00508
00509 motor_vel = last_motor_vel_backwards_ + halfdt*(last_motor_acc_backwards_ + motor_acc);
00510 motor_pos = last_motor_pos_backwards_ + halfdt*(last_motor_vel_backwards_ + motor_vel);
00511
00512
00513 spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) / trans_compl_;
00514 }
00515
00516
00517 js[0]->commanded_effort_ = spring_force;
00518
00519
00520
00521
00522 halfdt_backwards_ = halfdt;
00523 motor_force_backwards_ = motor_force;
00524 }
00525
00526
00527 void PR2BeltCompensatorTransmission::propagatePositionBackwards(
00528 std::vector<JointState*>& js, std::vector<pr2_hardware_interface::Actuator*>& as)
00529 {
00530
00531 assert(as.size() == 1);
00532 assert(js.size() == 1);
00533
00534
00535
00536
00537
00538 double motor_pos, motor_vel, motor_acc;
00539 double joint_pos, joint_vel;
00540 double motor_force;
00541 double spring_force;
00542 double halfdt;
00543
00544
00545
00546 halfdt = halfdt_backwards_;
00547 motor_force = motor_force_backwards_;
00548
00549
00550
00551 joint_pos = js[0]->position_ - js[0]->reference_position_;
00552 joint_vel = js[0]->velocity_;
00553
00554
00555
00556
00557 if ((trans_compl_ == 0.0) || (trans_tau_ == 0.0))
00558 {
00559
00560
00561 motor_acc = 0.0;
00562 motor_vel = joint_vel;
00563 motor_pos = joint_pos;
00564 }
00565 else
00566 {
00567
00568
00569 double tau = (halfdt < trans_tau_ ? trans_tau_ : halfdt);
00570
00571
00572
00573 motor_vel = last_motor_vel_backwards_ + halfdt*(last_motor_acc_backwards_ + 0 );
00574 motor_pos = last_motor_pos_backwards_ + halfdt*(last_motor_vel_backwards_ + motor_vel);
00575
00576
00577 spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) / trans_compl_;
00578
00579
00580 motor_acc = (motor_force - spring_force) * trans_compl_ / (tau*tau + 2.0*tau*halfdt + halfdt*halfdt);
00581
00582
00583 motor_vel = last_motor_vel_backwards_ + halfdt*(last_motor_acc_backwards_ + motor_acc);
00584 motor_pos = last_motor_pos_backwards_ + halfdt*(last_motor_vel_backwards_ + motor_vel);
00585 }
00586
00587
00588 last_motor_pos_backwards_ = motor_pos;
00589 last_motor_vel_backwards_ = motor_vel;
00590 last_motor_acc_backwards_ = motor_acc;
00591
00592 last_joint_pos_backwards_ = joint_pos;
00593 last_joint_vel_backwards_ = joint_vel;
00594
00595
00596
00597 as[0]->state_.position_ = motor_pos * mechanical_reduction_;
00598 as[0]->state_.velocity_ = motor_vel * mechanical_reduction_;
00599
00600
00601
00602
00603 as[0]->state_.last_measured_effort_ = motor_force / mechanical_reduction_;
00604
00605
00606
00607
00608 if (! simulated_actuator_timestamp_initialized_)
00609 {
00610
00611 as[0]->state_.sample_timestamp_ = ros::Duration(0);
00612
00613
00614 if (ros::isStarted())
00615 {
00616 simulated_actuator_start_time_ = ros::Time::now();
00617 simulated_actuator_timestamp_initialized_ = true;
00618 }
00619 }
00620 else
00621 {
00622
00623 as[0]->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
00624 }
00625
00626 as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.toSec();
00627
00628
00629
00630 this->joint_calibration_simulator_.simulateJointCalibration(js[0],as[0]);
00631 }
00632
00633 }