pr2_belt_transmission.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
36 #include <math.h>
38 
41 
42 namespace pr2_mechanism_model {
43 
44 bool PR2BeltCompensatorTransmission::initXml(TiXmlElement *elt, Robot *robot)
45 {
46  const char *name = elt->Attribute("name");
47  name_ = name ? name : "";
48 
49  TiXmlElement *jel = elt->FirstChildElement("joint");
50  const char *joint_name = jel ? jel->Attribute("name") : NULL;
51  if (!joint_name)
52  {
53  ROS_ERROR("PR2BeltCompensatorTransmission did not specify joint name");
54  return false;
55  }
56 
57  const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
58  if (!joint)
59  {
60  ROS_ERROR("PR2BeltCompensatorTransmission could not find joint named \"%s\"", joint_name);
61  return false;
62  }
63  joint_names_.push_back(joint_name);
64 
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 )
69  {
70  ROS_ERROR("PR2BeltCompensatorTransmission could not find actuator named \"%s\"", actuator_name);
71  return false;
72  }
73  a->command_.enable_ = true;
74  actuator_names_.push_back(actuator_name);
75 
76  mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
77 
78  TiXmlElement *c = elt->FirstChildElement("compensator");
79  if (!c)
80  {
81  ROS_ERROR("No compensator element given for transmission %s", name_.c_str());
82  return false;
83  }
84 
85  double k_belt;
86  const char *k_belt_str = c->Attribute("k_belt");
87  if (!k_belt_str) {
88  ROS_ERROR("No k_belt given for transmission %s", name_.c_str());
89  return false;
90  }
91  k_belt = atof(k_belt_str);
92 
93  double mass_motor;
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());
97  return false;
98  }
99  mass_motor = atof(mass_motor_str);
100 
101  trans_compl_ = (k_belt > 0.0 ? 1.0 / k_belt : 0.0);
102  trans_tau_ = sqrt(mass_motor * trans_compl_);
103 
104  const char *kd_motor_str = c->Attribute("kd_motor");
105  if (!kd_motor_str) {
106  ROS_ERROR("No kd_motor given for transmission %s", name_.c_str());
107  return false;
108  }
109  Kd_motor_ = atof(kd_motor_str);
110 
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());
114  return false;
115  }
116  lambda_motor_ = atof(lambda_motor_str);
117 
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());
121  return false;
122  }
123  lambda_joint_ = atof(lambda_joint_str);
124 
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());
128  return false;
129  }
130  lambda_combo_ = atof(lambda_combined_str);
131 
132  // Initializes the filters
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;
139 
140  last_timestamp_ = ros::Duration(0);
141 
142  // Initialize the backward transmission state variables.
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;
151 
152  return true;
153 }
154 
155 bool PR2BeltCompensatorTransmission::initXml(TiXmlElement *elt)
156 {
157  const char *name = elt->Attribute("name");
158  name_ = name ? name : "";
159 
160  TiXmlElement *jel = elt->FirstChildElement("joint");
161  const char *joint_name = jel ? jel->Attribute("name") : NULL;
162  if (!joint_name)
163  {
164  ROS_ERROR("PR2BeltCompensatorTransmission did not specify joint name");
165  return false;
166  }
167 
168  joint_names_.push_back(joint_name);
169 
170  TiXmlElement *ael = elt->FirstChildElement("actuator");
171  const char *actuator_name = ael ? ael->Attribute("name") : NULL;
172  actuator_names_.push_back(actuator_name);
173 
174  mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
175 
176  TiXmlElement *c = elt->FirstChildElement("compensator");
177  if (!c)
178  {
179  ROS_ERROR("No compensator element given for transmission %s", name_.c_str());
180  return false;
181  }
182 
183  double k_belt;
184  const char *k_belt_str = c->Attribute("k_belt");
185  if (!k_belt_str) {
186  ROS_ERROR("No k_belt given for transmission %s", name_.c_str());
187  return false;
188  }
189  k_belt = atof(k_belt_str);
190 
191  double mass_motor;
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());
195  return false;
196  }
197  mass_motor = atof(mass_motor_str);
198 
199  trans_compl_ = (k_belt > 0.0 ? 1.0 / k_belt : 0.0);
200  trans_tau_ = sqrt(mass_motor * trans_compl_);
201 
202  const char *kd_motor_str = c->Attribute("kd_motor");
203  if (!kd_motor_str) {
204  ROS_ERROR("No kd_motor given for transmission %s", name_.c_str());
205  return false;
206  }
207  Kd_motor_ = atof(kd_motor_str);
208 
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());
212  return false;
213  }
214  lambda_motor_ = atof(lambda_motor_str);
215 
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());
219  return false;
220  }
221  lambda_joint_ = atof(lambda_joint_str);
222 
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());
226  return false;
227  }
228  lambda_combo_ = atof(lambda_combined_str);
229 
230  // Initializes the filters
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;
237 
238  last_timestamp_ = ros::Duration(0);
239 
240  // Initialize the backward transmission state variables.
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;
249 
250  return true;
251 }
252 
253 void PR2BeltCompensatorTransmission::propagatePosition(
254  std::vector<pr2_hardware_interface::Actuator*>& as, std::vector<JointState*>& js)
255 {
256  assert(as.size() == 1);
257  assert(js.size() == 1);
258  ros::Duration timestamp = as[0]->state_.sample_timestamp_;
259  dt = (timestamp - last_timestamp_).toSec();
260  last_timestamp_ = timestamp;
261 
262  // These are not the actual "motor" positions. They are the
263  // theoretical joint positions if there were no belt involved.
264 
265  // Get the motor position, velocity, measured force. Note we do not
266  // filter the motor velocity, because we want the least lag for the
267  // motor damping feedback. We will filter the joint velocity.
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 );
270 
271  double motor_measured_force = as[0]->state_.last_measured_effort_ * mechanical_reduction_;
272 
273 
274  // Estimate the actual joint position and velocity. We use two
275  // distinct methods (each with their own pros/cons and combine).
276 
277  // Method 1: Twice filter the motor position at the resonance mode
278  // of the joint against the transmission and locked motor. This
279  // duplicates the mechanical filtering occuring in the
280  // transmission. Obviously, this is an approximation. In
281  // particular the resonance frequency is unknown (we have to assume
282  // that lambda_joint_ over-estimates it). Also, this doesn't
283  // account for steady-state forces, i.e. transmission stretch. So
284  // the method is better at higher frequencies. For numerical
285  // stability, upper bound the bandwidth by 2/dt.
286  double jnt1_pos, jnt1_vel, jnt1_acc;
287  double lam = (dt*lambda_joint_ < 2.0 ? lambda_joint_ : 2.0/dt);
288 
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_);
291 
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);
294 
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_);
297 
298  // Method 2: Estimate the transmission deflection explicitly. This
299  // uses only the transmission stiffness (compliance) and motor mass.
300  // The later is combined with the compliance to give a transmission
301  // time constant (tau). This method assumes the motor mass is much
302  // smaller than the joint mass and the resonance of the motor
303  // against the transmission is damped. It does NOT need to know the
304  // joint/link mass (i.e. is independent of joint configurations) and
305  // adds the appropriate DC transmission stretch. However, it
306  // assumes zero motor friction to ground and no Coulomb fiction.
307  // Also, it uses the encoder directly at high frequency, remaining
308  // noisy. For numerical stability, if the time constant is zero,
309  // implement a 0th order system. Else lower bound the time constant
310  // by dt/2.
311  double defl_pos, defl_vel, defl_acc;
312  if (trans_tau_ == 0)
313  {
314  defl_acc = 0.0;
315  defl_vel = 0.0;
316  defl_pos = trans_compl_ * motor_measured_force;
317  }
318  else
319  {
320  double tau = (dt < 2.0*trans_tau_ ? trans_tau_ : dt/2.0);
321 
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_);
324 
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);
328 
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_);
331  }
332 
333  double jnt2_pos = motor_pos - defl_pos;
334 
335  // Combine the two joint position estimates and calculate the
336  // velocity: High pass method 1, low pass method 2. In the end, the
337  // encoder and measured force are both filtered at least once before
338  // reaching the joint estimate. I.e. the velocity is smooth. For
339  // numerical stability, upper bound the combination bandwidth. If
340  // the bandwidth is zero, take just method 1.
341  double joint_pos, joint_vel;
342  if (lambda_combo_ == 0.0)
343  {
344  joint_pos = jnt1_pos;
345  joint_vel = jnt1_vel;
346  }
347  else
348  {
349  lam = (dt*lambda_combo_ < 2.0 ? lambda_combo_ : 2.0/dt);
350 
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_);
355  }
356 
357 
358  // Push the joint info out.
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_;
362 
363 
364  // Stores values used by propogateEffort
365  delta_motor_vel_ = motor_vel - last_motor_vel_;
366 
367  // Saves the current values for use in the next servo cycle. These
368  // are used to filter the signals
369  last_motor_pos_ = motor_pos;
370  last_motor_vel_ = motor_vel;
371 
372  last_jnt1_pos_ = jnt1_pos;
373  last_jnt1_vel_ = jnt1_vel;
374  last_jnt1_acc_ = jnt1_acc;
375 
376  last_defl_pos_ = defl_pos;
377  last_defl_vel_ = defl_vel;
378  last_defl_acc_ = defl_acc;
379 
380  last_joint_pos_ = joint_pos;
381  last_joint_vel_ = joint_vel;
382 }
383 
384 void PR2BeltCompensatorTransmission::propagateEffort(
385  std::vector<JointState*>& js, std::vector<pr2_hardware_interface::Actuator*>& as)
386 {
387  assert(as.size() == 1);
388  assert(js.size() == 1);
389 
390 
391  // Calculate the damping force for the motor vibrations against the
392  // transmission. As we don't have a true vibration measurement,
393  // dampen the motor simply at high frequency. For numerical
394  // stability, upper bound the cutoff bandwidth. If the bandwidth is
395  // zero, use damping over all frequencies, i.e. regular damping.
396  double motor_damping_force;
397  if (lambda_motor_ == 0.0)
398  {
399  motor_damping_force = - Kd_motor_ * last_motor_vel_;
400  }
401  else
402  {
403  double lam = (dt*lambda_motor_ < 2.0 ? lambda_motor_ : 2.0/dt);
404 
405  motor_damping_force = ((1.0-0.5*dt*lam) * last_motor_damping_force_
406  - Kd_motor_ * delta_motor_vel_)
407  / (1.0+0.5*dt*lam);
408  }
409 
410  // Add to the joint force.
411  double motor_force = js[0]->commanded_effort_ + motor_damping_force;
412 
413  // Send out!
414  as[0]->command_.effort_ = motor_force / mechanical_reduction_;
415 
416  last_motor_damping_force_ = motor_damping_force;
417 }
418 
419 
420 
421 // The backward transmission is a entirely separate entity for the
422 // forward transmission. Both contain state information, that is NOT
423 // shared. In particular, the backward transmission implements a
424 // 4th-order model of the motor mass, belt stiffness, and joint mass,
425 // which is what the forward transmission expects. It should be used
426 // ONLY in Gazebo, i.e. when simulating the robot. As such, we assume
427 // here that a cycle starts with a given actuator effort,
428 // propagateEffortBackwards() is called first (setting the time step) to
429 // set the joint effort, Gazebo then calculates a new joint position,
430 // and propagatePositionBackwards() is called last to provide the new
431 // actuator position.
432 void PR2BeltCompensatorTransmission::propagateEffortBackwards(
433  std::vector<pr2_hardware_interface::Actuator*>& as, std::vector<JointState*>& js)
434 {
435  // Check the arguments. This acts only on a single actuator/joint.
436  assert(as.size() == 1);
437  assert(js.size() == 1);
438 
439  // We are simulating a motor-mass / belt-spring-damper / joint-mass
440  // system. Note all calculations are done if joint-space units, so
441  // that the motor position/velocity/acceleration/force are scaled
442  // appropriately by the mechanical reduction. Also note, the joint
443  // mass is actually simulated in Gazebo, so for the purposes of this
444  // belt simulation, we assume an infinite joint mass and hence zero
445  // joint acceleration. Finally, we assume the belt-spring/motor-mass
446  // dynamics are critically damped and set the damping accordingly.
447  double motor_pos, motor_vel, motor_acc;
448  double joint_pos, joint_vel;
449  double motor_force;
450  double spring_force;
451  double halfdt;
452 
453  // Calculate the time step. Should be the same as the forward
454  // transmission, but we don't want/need to assume that. Furthermore,
455  // given this is used only to simulate a transmission, the time-steps
456  // should be perfectly constant and known in advance. But to keep
457  // this clean we recalculate. The question remains who defines the
458  // time step. Like the forward transmission, we can only use the time
459  // step in the actuator state, though this makes little sense here...
460  ros::Duration timestamp = as[0]->state_.sample_timestamp_;
461  halfdt = 0.5*(timestamp - last_timestamp_backwards_).toSec();
462  last_timestamp_backwards_ = timestamp;
463 
464  // Get the actuator force acting on the motor mass, multipled by the
465  // mechanical reduction to be in joint-space units. Note we are
466  // assuming the command is perfectly executed, i.e. will completely
467  // act on the motor.
468  motor_force = as[0]->command_.effort_ * mechanical_reduction_;
469 
470  // If the transmission compliance is zero (infinitely stiff) or the
471  // motor mass is zero (infinitely light), then the transmission time
472  // constant is also zero (infinitely fast) and the entire transmission
473  // collapses to a regular rigid connection.
474  if ((trans_compl_ == 0.0) || (trans_tau_ == 0.0))
475  {
476  // Immediately propagate the motor force to the spring, ignoring
477  // the (infinitely fast) model dynamics.
478  spring_force = motor_force;
479  }
480  else
481  {
482  // Update the model. Note for numerical stability, the
483  // transmission dynamics need to be slower than the integration
484  // time step. Specifically we need to lower bound the tranmission
485  // time constant by dt/2.
486  double tau = (halfdt < trans_tau_ ? trans_tau_ : halfdt);
487 
488  // Calculate the new motor position/velocity assuming a new motor
489  // acceleration of zero (simply integrate the last information).
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);
492 
493  // Calculate the new joint position/velocity assuming a new joint
494  // acceleration of zero, equivalent to an extremely large joint
495  // mass (relatively to the motor). This is also "fixed" in the
496  // second half of the backward transmission after the simulator
497  // has provided a new joint position/velocity.
498  joint_vel = last_joint_vel_backwards_;
499  joint_pos = last_joint_pos_backwards_ + halfdt*(last_joint_vel_backwards_ + joint_vel);
500 
501  // Calculate the spring force between the two masses.
502  spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) / trans_compl_;
503 
504  // This gives us the new motor acceleration (still assuming no
505  // joint acceleration).
506  motor_acc = (motor_force - spring_force) * trans_compl_ / (tau*tau + 2.0*tau*halfdt + halfdt*halfdt);
507 
508  // Recalculate the motor position/velocity, using this new acceleration.
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);
511 
512  // Recalculate the spring force.
513  spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) / trans_compl_;
514  }
515 
516  // The spring force becomes the force seen by the joint.
517  js[0]->commanded_effort_ = spring_force;
518 
519  // Save the information that is to be used in the second half, i.e. in
520  // propagatePositionBackwards(). This includes the motor force
521  // (driving this cycle) and the time step (calculated here).
522  halfdt_backwards_ = halfdt;
523  motor_force_backwards_ = motor_force;
524 }
525 
526 
527 void PR2BeltCompensatorTransmission::propagatePositionBackwards(
528  std::vector<JointState*>& js, std::vector<pr2_hardware_interface::Actuator*>& as)
529 {
530  // Check the arguments. This acts only on a single actuator/joint.
531  assert(as.size() == 1);
532  assert(js.size() == 1);
533 
534  // Again (as in the first half of the backward transmission) simulate
535  // the motor-mass / belt-spring-damper / joint-mass system. And
536  // again, all variables are in joint-space units. Only this time use
537  // the joint position/velocity provided by the simulator.
538  double motor_pos, motor_vel, motor_acc;
539  double joint_pos, joint_vel;
540  double motor_force;
541  double spring_force;
542  double halfdt;
543 
544  // Get the time step and motor force from the first half of the
545  // backward transmission.
546  halfdt = halfdt_backwards_;
547  motor_force = motor_force_backwards_;
548 
549  // Get the new joint position and velocity, as calculated by the
550  // simulator.
551  joint_pos = js[0]->position_ - js[0]->reference_position_;
552  joint_vel = js[0]->velocity_;
553 
554  // As in the first half, if the transmission compliance or time
555  // constant are zero, the transmission collapses to a regular rigid
556  // transmission.
557  if ((trans_compl_ == 0.0) || (trans_tau_ == 0.0))
558  {
559  // Immediately propagate the joint position/velocity to the motor,
560  // ignoring the (infinitely fast) model dynamics.
561  motor_acc = 0.0;
562  motor_vel = joint_vel;
563  motor_pos = joint_pos;
564  }
565  else
566  {
567  // Update the model. Note for numerical stability, we again lower
568  // bound the tranmission time constant by dt/2.
569  double tau = (halfdt < trans_tau_ ? trans_tau_ : halfdt);
570 
571  // Calculate the new motor position/velocity assuming a new motor
572  // acceleration of zero.
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);
575 
576  // Calculate the spring force between the two masses.
577  spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) / trans_compl_;
578 
579  // This gives us the new motor acceleration.
580  motor_acc = (motor_force - spring_force) * trans_compl_ / (tau*tau + 2.0*tau*halfdt + halfdt*halfdt);
581 
582  // Recalculate the motor position/velocity, using this new acceleration.
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);
585  }
586 
587  // Save the current state for the next cycle.
588  last_motor_pos_backwards_ = motor_pos;
589  last_motor_vel_backwards_ = motor_vel;
590  last_motor_acc_backwards_ = motor_acc;
591 
592  last_joint_pos_backwards_ = joint_pos;
593  last_joint_vel_backwards_ = joint_vel;
594 
595  // Push the motor position/velocity to the actuator, accounting for
596  // the mechanical reduction.
597  as[0]->state_.position_ = motor_pos * mechanical_reduction_;
598  as[0]->state_.velocity_ = motor_vel * mechanical_reduction_;
599 
600  // Also push the motor force to the actuator. Note we already assumed
601  // that the commanded actuator effort was accurately executed/applied,
602  // so the measured actuator effort is just the motor force.
603  as[0]->state_.last_measured_effort_ = motor_force / mechanical_reduction_;
604 
605 
606  // By storing the new actuator data, we are advancing to the next
607  // servo cycle. Always make sure the timing has been initialized.
608  if (! simulated_actuator_timestamp_initialized_)
609  {
610  // Set the time stamp to zero (it is measured relative to the start time).
611  as[0]->state_.sample_timestamp_ = ros::Duration(0);
612 
613  // Try to set the start time. Only then do we claim initialized.
614  if (ros::isStarted())
615  {
616  simulated_actuator_start_time_ = ros::Time::now();
617  simulated_actuator_timestamp_initialized_ = true;
618  }
619  }
620  else
621  {
622  // Measure the time stamp relative to the start time.
623  as[0]->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
624  }
625  // Set the historical (double) timestamp accordingly.
626  as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.toSec();
627 
628 
629  // Simulate calibration sensors by filling out actuator states
630  this->joint_calibration_simulator_.simulateJointCalibration(js[0],as[0]);
631 }
632 
633 } // namespace
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()
static Time now()
#define ROS_ERROR(...)


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Jun 10 2019 14:19:04