44 bool PR2BeltCompensatorTransmission::initXml(TiXmlElement *elt,
Robot *robot)
46 const char *name = elt->Attribute(
"name");
47 name_ = name ? name :
"";
49 TiXmlElement *jel = elt->FirstChildElement(
"joint");
50 const char *joint_name = jel ? jel->Attribute(
"name") : NULL;
53 ROS_ERROR(
"PR2BeltCompensatorTransmission did not specify joint name");
60 ROS_ERROR(
"PR2BeltCompensatorTransmission could not find joint named \"%s\"", joint_name);
63 joint_names_.push_back(joint_name);
65 TiXmlElement *ael = elt->FirstChildElement(
"actuator");
66 const char *actuator_name = ael ? ael->Attribute(
"name") : NULL;
68 if (!actuator_name || (a = robot->getActuator(actuator_name)) == NULL )
70 ROS_ERROR(
"PR2BeltCompensatorTransmission could not find actuator named \"%s\"", actuator_name);
74 actuator_names_.push_back(actuator_name);
76 mechanical_reduction_ = atof(elt->FirstChildElement(
"mechanicalReduction")->GetText());
78 TiXmlElement *c = elt->FirstChildElement(
"compensator");
81 ROS_ERROR(
"No compensator element given for transmission %s", name_.c_str());
86 const char *k_belt_str = c->Attribute(
"k_belt");
88 ROS_ERROR(
"No k_belt given for transmission %s", name_.c_str());
91 k_belt = atof(k_belt_str);
94 const char *mass_motor_str = c->Attribute(
"mass_motor");
95 if (!mass_motor_str) {
96 ROS_ERROR(
"No mass_motor given for transmission %s", name_.c_str());
99 mass_motor = atof(mass_motor_str);
101 trans_compl_ = (k_belt > 0.0 ? 1.0 / k_belt : 0.0);
102 trans_tau_ =
sqrt(mass_motor * trans_compl_);
104 const char *kd_motor_str = c->Attribute(
"kd_motor");
106 ROS_ERROR(
"No kd_motor given for transmission %s", name_.c_str());
109 Kd_motor_ = atof(kd_motor_str);
111 const char *lambda_motor_str = c->Attribute(
"lambda_motor");
112 if (!lambda_motor_str) {
113 ROS_ERROR(
"No lambda_motor given for transmission %s", name_.c_str());
116 lambda_motor_ = atof(lambda_motor_str);
118 const char *lambda_joint_str = c->Attribute(
"lambda_joint");
119 if (!lambda_joint_str) {
120 ROS_ERROR(
"No lambda_joint given for transmission %s", name_.c_str());
123 lambda_joint_ = atof(lambda_joint_str);
125 const char *lambda_combined_str = c->Attribute(
"lambda_combined");
126 if (!lambda_combined_str) {
127 ROS_ERROR(
"No lambda_combined given for transmission %s", name_.c_str());
130 lambda_combo_ = atof(lambda_combined_str);
133 last_motor_pos_ = last_motor_vel_ = 0;
134 last_jnt1_pos_ = last_jnt1_vel_ = last_jnt1_acc_ = 0;
135 last_defl_pos_ = last_defl_vel_ = last_defl_acc_ = 0;
136 last_joint_pos_ = last_joint_vel_ = 0;
137 delta_motor_vel_ = 0;
138 last_motor_damping_force_ = 0;
143 last_timestamp_backwards_ = last_timestamp_;
144 halfdt_backwards_ = 0.0;
145 motor_force_backwards_ = 0.0;
146 last_motor_pos_backwards_ = 0.0;
147 last_motor_vel_backwards_ = 0.0;
148 last_motor_acc_backwards_ = 0.0;
149 last_joint_pos_backwards_ = 0.0;
150 last_joint_vel_backwards_ = 0.0;
155 bool PR2BeltCompensatorTransmission::initXml(TiXmlElement *elt)
157 const char *name = elt->Attribute(
"name");
158 name_ = name ? name :
"";
160 TiXmlElement *jel = elt->FirstChildElement(
"joint");
161 const char *joint_name = jel ? jel->Attribute(
"name") : NULL;
164 ROS_ERROR(
"PR2BeltCompensatorTransmission did not specify joint name");
168 joint_names_.push_back(joint_name);
170 TiXmlElement *ael = elt->FirstChildElement(
"actuator");
171 const char *actuator_name = ael ? ael->Attribute(
"name") : NULL;
172 actuator_names_.push_back(actuator_name);
174 mechanical_reduction_ = atof(elt->FirstChildElement(
"mechanicalReduction")->GetText());
176 TiXmlElement *c = elt->FirstChildElement(
"compensator");
179 ROS_ERROR(
"No compensator element given for transmission %s", name_.c_str());
184 const char *k_belt_str = c->Attribute(
"k_belt");
186 ROS_ERROR(
"No k_belt given for transmission %s", name_.c_str());
189 k_belt = atof(k_belt_str);
192 const char *mass_motor_str = c->Attribute(
"mass_motor");
193 if (!mass_motor_str) {
194 ROS_ERROR(
"No mass_motor given for transmission %s", name_.c_str());
197 mass_motor = atof(mass_motor_str);
199 trans_compl_ = (k_belt > 0.0 ? 1.0 / k_belt : 0.0);
200 trans_tau_ =
sqrt(mass_motor * trans_compl_);
202 const char *kd_motor_str = c->Attribute(
"kd_motor");
204 ROS_ERROR(
"No kd_motor given for transmission %s", name_.c_str());
207 Kd_motor_ = atof(kd_motor_str);
209 const char *lambda_motor_str = c->Attribute(
"lambda_motor");
210 if (!lambda_motor_str) {
211 ROS_ERROR(
"No lambda_motor given for transmission %s", name_.c_str());
214 lambda_motor_ = atof(lambda_motor_str);
216 const char *lambda_joint_str = c->Attribute(
"lambda_joint");
217 if (!lambda_joint_str) {
218 ROS_ERROR(
"No lambda_joint given for transmission %s", name_.c_str());
221 lambda_joint_ = atof(lambda_joint_str);
223 const char *lambda_combined_str = c->Attribute(
"lambda_combined");
224 if (!lambda_combined_str) {
225 ROS_ERROR(
"No lambda_combined given for transmission %s", name_.c_str());
228 lambda_combo_ = atof(lambda_combined_str);
231 last_motor_pos_ = last_motor_vel_ = 0;
232 last_jnt1_pos_ = last_jnt1_vel_ = last_jnt1_acc_ = 0;
233 last_defl_pos_ = last_defl_vel_ = last_defl_acc_ = 0;
234 last_joint_pos_ = last_joint_vel_ = 0;
235 delta_motor_vel_ = 0;
236 last_motor_damping_force_ = 0;
241 last_timestamp_backwards_ = last_timestamp_;
242 halfdt_backwards_ = 0.0;
243 motor_force_backwards_ = 0.0;
244 last_motor_pos_backwards_ = 0.0;
245 last_motor_vel_backwards_ = 0.0;
246 last_motor_acc_backwards_ = 0.0;
247 last_joint_pos_backwards_ = 0.0;
248 last_joint_vel_backwards_ = 0.0;
253 void PR2BeltCompensatorTransmission::propagatePosition(
254 std::vector<pr2_hardware_interface::Actuator*>& as, std::vector<JointState*>& js)
256 assert(as.size() == 1);
257 assert(js.size() == 1);
259 dt = (timestamp - last_timestamp_).toSec();
260 last_timestamp_ = timestamp;
268 double motor_pos = as[0]->state_.position_ / mechanical_reduction_;
269 double motor_vel = ( dt>0.0 ? (motor_pos - last_motor_pos_)/dt : 0.0 );
271 double motor_measured_force = as[0]->state_.last_measured_effort_ * mechanical_reduction_;
286 double jnt1_pos, jnt1_vel, jnt1_acc;
287 double lam = (dt*lambda_joint_ < 2.0 ? lambda_joint_ : 2.0/dt);
289 jnt1_vel = last_jnt1_vel_ + 0.5*dt*( last_jnt1_acc_);
290 jnt1_pos = last_jnt1_pos_ + 0.5*dt*(jnt1_vel + last_jnt1_vel_);
292 jnt1_acc = (lam*lam * (motor_pos-jnt1_pos) - 2.0*lam * jnt1_vel)
293 / (1.0 + 0.5*dt*2.0*lam + 0.25*dt*dt*lam*lam);
295 jnt1_vel = last_jnt1_vel_ + 0.5*dt*(jnt1_acc + last_jnt1_acc_);
296 jnt1_pos = last_jnt1_pos_ + 0.5*dt*(jnt1_vel + last_jnt1_vel_);
311 double defl_pos, defl_vel, defl_acc;
316 defl_pos = trans_compl_ * motor_measured_force;
320 double tau = (dt < 2.0*trans_tau_ ? trans_tau_ : dt/2.0);
322 defl_vel = last_defl_vel_ + 0.5*dt*( last_defl_acc_);
323 defl_pos = last_defl_pos_ + 0.5*dt*(defl_vel + last_defl_vel_);
325 defl_acc = (trans_compl_ * motor_measured_force
326 - defl_pos - 2.0*tau * defl_vel)
327 / (tau*tau + 2.0*tau*0.5*dt + 0.25*dt*dt);
329 defl_vel = last_defl_vel_ + 0.5*dt*(defl_acc + last_defl_acc_);
330 defl_pos = last_defl_pos_ + 0.5*dt*(defl_vel + last_defl_vel_);
333 double jnt2_pos = motor_pos - defl_pos;
341 double joint_pos, joint_vel;
342 if (lambda_combo_ == 0.0)
344 joint_pos = jnt1_pos;
345 joint_vel = jnt1_vel;
349 lam = (dt*lambda_combo_ < 2.0 ? lambda_combo_ : 2.0/dt);
351 joint_pos = last_joint_pos_ + 0.5*dt*(last_joint_vel_);
352 joint_vel = (jnt1_vel + lam * (jnt2_pos - joint_pos))
353 / (1.0 + 0.5*dt*lam);
354 joint_pos = last_joint_pos_ + 0.5*dt*(joint_vel + last_joint_vel_);
359 js[0]->position_ = joint_pos + js[0]->reference_position_;
360 js[0]->velocity_ = joint_vel;
361 js[0]->measured_effort_ = as[0]->state_.last_measured_effort_ * mechanical_reduction_;
365 delta_motor_vel_ = motor_vel - last_motor_vel_;
369 last_motor_pos_ = motor_pos;
370 last_motor_vel_ = motor_vel;
372 last_jnt1_pos_ = jnt1_pos;
373 last_jnt1_vel_ = jnt1_vel;
374 last_jnt1_acc_ = jnt1_acc;
376 last_defl_pos_ = defl_pos;
377 last_defl_vel_ = defl_vel;
378 last_defl_acc_ = defl_acc;
380 last_joint_pos_ = joint_pos;
381 last_joint_vel_ = joint_vel;
384 void PR2BeltCompensatorTransmission::propagateEffort(
385 std::vector<JointState*>& js, std::vector<pr2_hardware_interface::Actuator*>& as)
387 assert(as.size() == 1);
388 assert(js.size() == 1);
396 double motor_damping_force;
397 if (lambda_motor_ == 0.0)
399 motor_damping_force = - Kd_motor_ * last_motor_vel_;
403 double lam = (dt*lambda_motor_ < 2.0 ? lambda_motor_ : 2.0/dt);
405 motor_damping_force = ((1.0-0.5*dt*lam) * last_motor_damping_force_
406 - Kd_motor_ * delta_motor_vel_)
411 double motor_force = js[0]->commanded_effort_ + motor_damping_force;
414 as[0]->command_.effort_ = motor_force / mechanical_reduction_;
416 last_motor_damping_force_ = motor_damping_force;
432 void PR2BeltCompensatorTransmission::propagateEffortBackwards(
433 std::vector<pr2_hardware_interface::Actuator*>& as, std::vector<JointState*>& js)
436 assert(as.size() == 1);
437 assert(js.size() == 1);
447 double motor_pos, motor_vel, motor_acc;
448 double joint_pos, joint_vel;
461 halfdt = 0.5*(timestamp - last_timestamp_backwards_).toSec();
462 last_timestamp_backwards_ = timestamp;
468 motor_force = as[0]->command_.effort_ * mechanical_reduction_;
474 if ((trans_compl_ == 0.0) || (trans_tau_ == 0.0))
478 spring_force = motor_force;
486 double tau = (halfdt < trans_tau_ ? trans_tau_ : halfdt);
490 motor_vel = last_motor_vel_backwards_ + halfdt*(last_motor_acc_backwards_ + 0 );
491 motor_pos = last_motor_pos_backwards_ + halfdt*(last_motor_vel_backwards_ + motor_vel);
498 joint_vel = last_joint_vel_backwards_;
499 joint_pos = last_joint_pos_backwards_ + halfdt*(last_joint_vel_backwards_ + joint_vel);
502 spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) / trans_compl_;
506 motor_acc = (motor_force - spring_force) * trans_compl_ / (tau*tau + 2.0*tau*halfdt + halfdt*halfdt);
509 motor_vel = last_motor_vel_backwards_ + halfdt*(last_motor_acc_backwards_ + motor_acc);
510 motor_pos = last_motor_pos_backwards_ + halfdt*(last_motor_vel_backwards_ + motor_vel);
513 spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) / trans_compl_;
517 js[0]->commanded_effort_ = spring_force;
522 halfdt_backwards_ = halfdt;
523 motor_force_backwards_ = motor_force;
527 void PR2BeltCompensatorTransmission::propagatePositionBackwards(
528 std::vector<JointState*>& js, std::vector<pr2_hardware_interface::Actuator*>& as)
531 assert(as.size() == 1);
532 assert(js.size() == 1);
538 double motor_pos, motor_vel, motor_acc;
539 double joint_pos, joint_vel;
546 halfdt = halfdt_backwards_;
547 motor_force = motor_force_backwards_;
551 joint_pos = js[0]->position_ - js[0]->reference_position_;
552 joint_vel = js[0]->velocity_;
557 if ((trans_compl_ == 0.0) || (trans_tau_ == 0.0))
562 motor_vel = joint_vel;
563 motor_pos = joint_pos;
569 double tau = (halfdt < trans_tau_ ? trans_tau_ : halfdt);
573 motor_vel = last_motor_vel_backwards_ + halfdt*(last_motor_acc_backwards_ + 0 );
574 motor_pos = last_motor_pos_backwards_ + halfdt*(last_motor_vel_backwards_ + motor_vel);
577 spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) / trans_compl_;
580 motor_acc = (motor_force - spring_force) * trans_compl_ / (tau*tau + 2.0*tau*halfdt + halfdt*halfdt);
583 motor_vel = last_motor_vel_backwards_ + halfdt*(last_motor_acc_backwards_ + motor_acc);
584 motor_pos = last_motor_pos_backwards_ + halfdt*(last_motor_vel_backwards_ + motor_vel);
588 last_motor_pos_backwards_ = motor_pos;
589 last_motor_vel_backwards_ = motor_vel;
590 last_motor_acc_backwards_ = motor_acc;
592 last_joint_pos_backwards_ = joint_pos;
593 last_joint_vel_backwards_ = joint_vel;
597 as[0]->state_.position_ = motor_pos * mechanical_reduction_;
598 as[0]->state_.velocity_ = motor_vel * mechanical_reduction_;
603 as[0]->state_.last_measured_effort_ = motor_force / mechanical_reduction_;
608 if (! simulated_actuator_timestamp_initialized_)
617 simulated_actuator_timestamp_initialized_ =
true;
623 as[0]->state_.sample_timestamp_ =
ros::Time::now() - simulated_actuator_start_time_;
626 as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.
toSec();
630 this->joint_calibration_simulator_.simulateJointCalibration(js[0],as[0]);
PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::PR2BeltCompensatorTransmission, pr2_mechanism_model::Transmission) namespace pr2_mechanism_model
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
ROSCPP_DECL bool isStarted()