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 #if URDFDOM_1_0_0_API
58  urdf::JointConstSharedPtr joint = robot->robot_model_.getJoint(joint_name);
59 #else
60  const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
61 #endif
62  if (!joint)
63  {
64  ROS_ERROR("PR2BeltCompensatorTransmission could not find joint named \"%s\"", joint_name);
65  return false;
66  }
67  joint_names_.push_back(joint_name);
68 
69  TiXmlElement *ael = elt->FirstChildElement("actuator");
70  const char *actuator_name = ael ? ael->Attribute("name") : NULL;
72  if (!actuator_name || (a = robot->getActuator(actuator_name)) == NULL )
73  {
74  ROS_ERROR("PR2BeltCompensatorTransmission could not find actuator named \"%s\"", actuator_name);
75  return false;
76  }
77  a->command_.enable_ = true;
78  actuator_names_.push_back(actuator_name);
79 
80  mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
81 
82  TiXmlElement *c = elt->FirstChildElement("compensator");
83  if (!c)
84  {
85  ROS_ERROR("No compensator element given for transmission %s", name_.c_str());
86  return false;
87  }
88 
89  double k_belt;
90  const char *k_belt_str = c->Attribute("k_belt");
91  if (!k_belt_str) {
92  ROS_ERROR("No k_belt given for transmission %s", name_.c_str());
93  return false;
94  }
95  k_belt = atof(k_belt_str);
96 
97  double mass_motor;
98  const char *mass_motor_str = c->Attribute("mass_motor");
99  if (!mass_motor_str) {
100  ROS_ERROR("No mass_motor given for transmission %s", name_.c_str());
101  return false;
102  }
103  mass_motor = atof(mass_motor_str);
104 
105  trans_compl_ = (k_belt > 0.0 ? 1.0 / k_belt : 0.0);
106  trans_tau_ = sqrt(mass_motor * trans_compl_);
107 
108  const char *kd_motor_str = c->Attribute("kd_motor");
109  if (!kd_motor_str) {
110  ROS_ERROR("No kd_motor given for transmission %s", name_.c_str());
111  return false;
112  }
113  Kd_motor_ = atof(kd_motor_str);
114 
115  const char *lambda_motor_str = c->Attribute("lambda_motor");
116  if (!lambda_motor_str) {
117  ROS_ERROR("No lambda_motor given for transmission %s", name_.c_str());
118  return false;
119  }
120  lambda_motor_ = atof(lambda_motor_str);
121 
122  const char *lambda_joint_str = c->Attribute("lambda_joint");
123  if (!lambda_joint_str) {
124  ROS_ERROR("No lambda_joint given for transmission %s", name_.c_str());
125  return false;
126  }
127  lambda_joint_ = atof(lambda_joint_str);
128 
129  const char *lambda_combined_str = c->Attribute("lambda_combined");
130  if (!lambda_combined_str) {
131  ROS_ERROR("No lambda_combined given for transmission %s", name_.c_str());
132  return false;
133  }
134  lambda_combo_ = atof(lambda_combined_str);
135 
136  // Initializes the filters
141  delta_motor_vel_ = 0;
143 
145 
146  // Initialize the backward transmission state variables.
148  halfdt_backwards_ = 0.0;
155 
156  return true;
157 }
158 
159 bool PR2BeltCompensatorTransmission::initXml(TiXmlElement *elt)
160 {
161  const char *name = elt->Attribute("name");
162  name_ = name ? name : "";
163 
164  TiXmlElement *jel = elt->FirstChildElement("joint");
165  const char *joint_name = jel ? jel->Attribute("name") : NULL;
166  if (!joint_name)
167  {
168  ROS_ERROR("PR2BeltCompensatorTransmission did not specify joint name");
169  return false;
170  }
171 
172  joint_names_.push_back(joint_name);
173 
174  TiXmlElement *ael = elt->FirstChildElement("actuator");
175  const char *actuator_name = ael ? ael->Attribute("name") : NULL;
176  actuator_names_.push_back(actuator_name);
177 
178  mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
179 
180  TiXmlElement *c = elt->FirstChildElement("compensator");
181  if (!c)
182  {
183  ROS_ERROR("No compensator element given for transmission %s", name_.c_str());
184  return false;
185  }
186 
187  double k_belt;
188  const char *k_belt_str = c->Attribute("k_belt");
189  if (!k_belt_str) {
190  ROS_ERROR("No k_belt given for transmission %s", name_.c_str());
191  return false;
192  }
193  k_belt = atof(k_belt_str);
194 
195  double mass_motor;
196  const char *mass_motor_str = c->Attribute("mass_motor");
197  if (!mass_motor_str) {
198  ROS_ERROR("No mass_motor given for transmission %s", name_.c_str());
199  return false;
200  }
201  mass_motor = atof(mass_motor_str);
202 
203  trans_compl_ = (k_belt > 0.0 ? 1.0 / k_belt : 0.0);
204  trans_tau_ = sqrt(mass_motor * trans_compl_);
205 
206  const char *kd_motor_str = c->Attribute("kd_motor");
207  if (!kd_motor_str) {
208  ROS_ERROR("No kd_motor given for transmission %s", name_.c_str());
209  return false;
210  }
211  Kd_motor_ = atof(kd_motor_str);
212 
213  const char *lambda_motor_str = c->Attribute("lambda_motor");
214  if (!lambda_motor_str) {
215  ROS_ERROR("No lambda_motor given for transmission %s", name_.c_str());
216  return false;
217  }
218  lambda_motor_ = atof(lambda_motor_str);
219 
220  const char *lambda_joint_str = c->Attribute("lambda_joint");
221  if (!lambda_joint_str) {
222  ROS_ERROR("No lambda_joint given for transmission %s", name_.c_str());
223  return false;
224  }
225  lambda_joint_ = atof(lambda_joint_str);
226 
227  const char *lambda_combined_str = c->Attribute("lambda_combined");
228  if (!lambda_combined_str) {
229  ROS_ERROR("No lambda_combined given for transmission %s", name_.c_str());
230  return false;
231  }
232  lambda_combo_ = atof(lambda_combined_str);
233 
234  // Initializes the filters
239  delta_motor_vel_ = 0;
241 
243 
244  // Initialize the backward transmission state variables.
246  halfdt_backwards_ = 0.0;
253 
254  return true;
255 }
256 
258  std::vector<pr2_hardware_interface::Actuator*>& as, std::vector<JointState*>& js)
259 {
260  assert(as.size() == 1);
261  assert(js.size() == 1);
262  ros::Duration timestamp = as[0]->state_.sample_timestamp_;
263  dt = (timestamp - last_timestamp_).toSec();
264  last_timestamp_ = timestamp;
265 
266  // These are not the actual "motor" positions. They are the
267  // theoretical joint positions if there were no belt involved.
268 
269  // Get the motor position, velocity, measured force. Note we do not
270  // filter the motor velocity, because we want the least lag for the
271  // motor damping feedback. We will filter the joint velocity.
272  double motor_pos = as[0]->state_.position_ / mechanical_reduction_;
273  double motor_vel = ( dt>0.0 ? (motor_pos - last_motor_pos_)/dt : 0.0 );
274 
275  double motor_measured_force = as[0]->state_.last_measured_effort_ * mechanical_reduction_;
276 
277 
278  // Estimate the actual joint position and velocity. We use two
279  // distinct methods (each with their own pros/cons and combine).
280 
281  // Method 1: Twice filter the motor position at the resonance mode
282  // of the joint against the transmission and locked motor. This
283  // duplicates the mechanical filtering occuring in the
284  // transmission. Obviously, this is an approximation. In
285  // particular the resonance frequency is unknown (we have to assume
286  // that lambda_joint_ over-estimates it). Also, this doesn't
287  // account for steady-state forces, i.e. transmission stretch. So
288  // the method is better at higher frequencies. For numerical
289  // stability, upper bound the bandwidth by 2/dt.
290  double jnt1_pos, jnt1_vel, jnt1_acc;
291  double lam = (dt*lambda_joint_ < 2.0 ? lambda_joint_ : 2.0/dt);
292 
293  jnt1_vel = last_jnt1_vel_ + 0.5*dt*( last_jnt1_acc_);
294  jnt1_pos = last_jnt1_pos_ + 0.5*dt*(jnt1_vel + last_jnt1_vel_);
295 
296  jnt1_acc = (lam*lam * (motor_pos-jnt1_pos) - 2.0*lam * jnt1_vel)
297  / (1.0 + 0.5*dt*2.0*lam + 0.25*dt*dt*lam*lam);
298 
299  jnt1_vel = last_jnt1_vel_ + 0.5*dt*(jnt1_acc + last_jnt1_acc_);
300  jnt1_pos = last_jnt1_pos_ + 0.5*dt*(jnt1_vel + last_jnt1_vel_);
301 
302  // Method 2: Estimate the transmission deflection explicitly. This
303  // uses only the transmission stiffness (compliance) and motor mass.
304  // The later is combined with the compliance to give a transmission
305  // time constant (tau). This method assumes the motor mass is much
306  // smaller than the joint mass and the resonance of the motor
307  // against the transmission is damped. It does NOT need to know the
308  // joint/link mass (i.e. is independent of joint configurations) and
309  // adds the appropriate DC transmission stretch. However, it
310  // assumes zero motor friction to ground and no Coulomb fiction.
311  // Also, it uses the encoder directly at high frequency, remaining
312  // noisy. For numerical stability, if the time constant is zero,
313  // implement a 0th order system. Else lower bound the time constant
314  // by dt/2.
315  double defl_pos, defl_vel, defl_acc;
316  if (trans_tau_ == 0)
317  {
318  defl_acc = 0.0;
319  defl_vel = 0.0;
320  defl_pos = trans_compl_ * motor_measured_force;
321  }
322  else
323  {
324  double tau = (dt < 2.0*trans_tau_ ? trans_tau_ : dt/2.0);
325 
326  defl_vel = last_defl_vel_ + 0.5*dt*( last_defl_acc_);
327  defl_pos = last_defl_pos_ + 0.5*dt*(defl_vel + last_defl_vel_);
328 
329  defl_acc = (trans_compl_ * motor_measured_force
330  - defl_pos - 2.0*tau * defl_vel)
331  / (tau*tau + 2.0*tau*0.5*dt + 0.25*dt*dt);
332 
333  defl_vel = last_defl_vel_ + 0.5*dt*(defl_acc + last_defl_acc_);
334  defl_pos = last_defl_pos_ + 0.5*dt*(defl_vel + last_defl_vel_);
335  }
336 
337  double jnt2_pos = motor_pos - defl_pos;
338 
339  // Combine the two joint position estimates and calculate the
340  // velocity: High pass method 1, low pass method 2. In the end, the
341  // encoder and measured force are both filtered at least once before
342  // reaching the joint estimate. I.e. the velocity is smooth. For
343  // numerical stability, upper bound the combination bandwidth. If
344  // the bandwidth is zero, take just method 1.
345  double joint_pos, joint_vel;
346  if (lambda_combo_ == 0.0)
347  {
348  joint_pos = jnt1_pos;
349  joint_vel = jnt1_vel;
350  }
351  else
352  {
353  lam = (dt*lambda_combo_ < 2.0 ? lambda_combo_ : 2.0/dt);
354 
355  joint_pos = last_joint_pos_ + 0.5*dt*(last_joint_vel_);
356  joint_vel = (jnt1_vel + lam * (jnt2_pos - joint_pos))
357  / (1.0 + 0.5*dt*lam);
358  joint_pos = last_joint_pos_ + 0.5*dt*(joint_vel + last_joint_vel_);
359  }
360 
361 
362  // Push the joint info out.
363  js[0]->position_ = joint_pos + js[0]->reference_position_;
364  js[0]->velocity_ = joint_vel;
365  js[0]->measured_effort_ = as[0]->state_.last_measured_effort_ * mechanical_reduction_;
366 
367 
368  // Stores values used by propogateEffort
369  delta_motor_vel_ = motor_vel - last_motor_vel_;
370 
371  // Saves the current values for use in the next servo cycle. These
372  // are used to filter the signals
373  last_motor_pos_ = motor_pos;
374  last_motor_vel_ = motor_vel;
375 
376  last_jnt1_pos_ = jnt1_pos;
377  last_jnt1_vel_ = jnt1_vel;
378  last_jnt1_acc_ = jnt1_acc;
379 
380  last_defl_pos_ = defl_pos;
381  last_defl_vel_ = defl_vel;
382  last_defl_acc_ = defl_acc;
383 
384  last_joint_pos_ = joint_pos;
385  last_joint_vel_ = joint_vel;
386 }
387 
389  std::vector<JointState*>& js, std::vector<pr2_hardware_interface::Actuator*>& as)
390 {
391  assert(as.size() == 1);
392  assert(js.size() == 1);
393 
394 
395  // Calculate the damping force for the motor vibrations against the
396  // transmission. As we don't have a true vibration measurement,
397  // dampen the motor simply at high frequency. For numerical
398  // stability, upper bound the cutoff bandwidth. If the bandwidth is
399  // zero, use damping over all frequencies, i.e. regular damping.
400  double motor_damping_force;
401  if (lambda_motor_ == 0.0)
402  {
403  motor_damping_force = - Kd_motor_ * last_motor_vel_;
404  }
405  else
406  {
407  double lam = (dt*lambda_motor_ < 2.0 ? lambda_motor_ : 2.0/dt);
408 
409  motor_damping_force = ((1.0-0.5*dt*lam) * last_motor_damping_force_
411  / (1.0+0.5*dt*lam);
412  }
413 
414  // Add to the joint force.
415  double motor_force = js[0]->commanded_effort_ + motor_damping_force;
416 
417  // Send out!
418  as[0]->command_.effort_ = motor_force / mechanical_reduction_;
419 
420  last_motor_damping_force_ = motor_damping_force;
421 }
422 
423 
424 
425 // The backward transmission is a entirely separate entity for the
426 // forward transmission. Both contain state information, that is NOT
427 // shared. In particular, the backward transmission implements a
428 // 4th-order model of the motor mass, belt stiffness, and joint mass,
429 // which is what the forward transmission expects. It should be used
430 // ONLY in Gazebo, i.e. when simulating the robot. As such, we assume
431 // here that a cycle starts with a given actuator effort,
432 // propagateEffortBackwards() is called first (setting the time step) to
433 // set the joint effort, Gazebo then calculates a new joint position,
434 // and propagatePositionBackwards() is called last to provide the new
435 // actuator position.
437  std::vector<pr2_hardware_interface::Actuator*>& as, std::vector<JointState*>& js)
438 {
439  // Check the arguments. This acts only on a single actuator/joint.
440  assert(as.size() == 1);
441  assert(js.size() == 1);
442 
443  // We are simulating a motor-mass / belt-spring-damper / joint-mass
444  // system. Note all calculations are done if joint-space units, so
445  // that the motor position/velocity/acceleration/force are scaled
446  // appropriately by the mechanical reduction. Also note, the joint
447  // mass is actually simulated in Gazebo, so for the purposes of this
448  // belt simulation, we assume an infinite joint mass and hence zero
449  // joint acceleration. Finally, we assume the belt-spring/motor-mass
450  // dynamics are critically damped and set the damping accordingly.
451  double motor_pos, motor_vel, motor_acc;
452  double joint_pos, joint_vel;
453  double motor_force;
454  double spring_force;
455  double halfdt;
456 
457  // Calculate the time step. Should be the same as the forward
458  // transmission, but we don't want/need to assume that. Furthermore,
459  // given this is used only to simulate a transmission, the time-steps
460  // should be perfectly constant and known in advance. But to keep
461  // this clean we recalculate. The question remains who defines the
462  // time step. Like the forward transmission, we can only use the time
463  // step in the actuator state, though this makes little sense here...
464  ros::Duration timestamp = as[0]->state_.sample_timestamp_;
465  halfdt = 0.5*(timestamp - last_timestamp_backwards_).toSec();
466  last_timestamp_backwards_ = timestamp;
467 
468  // Get the actuator force acting on the motor mass, multipled by the
469  // mechanical reduction to be in joint-space units. Note we are
470  // assuming the command is perfectly executed, i.e. will completely
471  // act on the motor.
472  motor_force = as[0]->command_.effort_ * mechanical_reduction_;
473 
474  // If the transmission compliance is zero (infinitely stiff) or the
475  // motor mass is zero (infinitely light), then the transmission time
476  // constant is also zero (infinitely fast) and the entire transmission
477  // collapses to a regular rigid connection.
478  if ((trans_compl_ == 0.0) || (trans_tau_ == 0.0))
479  {
480  // Immediately propagate the motor force to the spring, ignoring
481  // the (infinitely fast) model dynamics.
482  spring_force = motor_force;
483  }
484  else
485  {
486  // Update the model. Note for numerical stability, the
487  // transmission dynamics need to be slower than the integration
488  // time step. Specifically we need to lower bound the tranmission
489  // time constant by dt/2.
490  double tau = (halfdt < trans_tau_ ? trans_tau_ : halfdt);
491 
492  // Calculate the new motor position/velocity assuming a new motor
493  // acceleration of zero (simply integrate the last information).
494  motor_vel = last_motor_vel_backwards_ + halfdt*(last_motor_acc_backwards_ + 0 );
495  motor_pos = last_motor_pos_backwards_ + halfdt*(last_motor_vel_backwards_ + motor_vel);
496 
497  // Calculate the new joint position/velocity assuming a new joint
498  // acceleration of zero, equivalent to an extremely large joint
499  // mass (relatively to the motor). This is also "fixed" in the
500  // second half of the backward transmission after the simulator
501  // has provided a new joint position/velocity.
502  joint_vel = last_joint_vel_backwards_;
503  joint_pos = last_joint_pos_backwards_ + halfdt*(last_joint_vel_backwards_ + joint_vel);
504 
505  // Calculate the spring force between the two masses.
506  spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) / trans_compl_;
507 
508  // This gives us the new motor acceleration (still assuming no
509  // joint acceleration).
510  motor_acc = (motor_force - spring_force) * trans_compl_ / (tau*tau + 2.0*tau*halfdt + halfdt*halfdt);
511 
512  // Recalculate the motor position/velocity, using this new acceleration.
513  motor_vel = last_motor_vel_backwards_ + halfdt*(last_motor_acc_backwards_ + motor_acc);
514  motor_pos = last_motor_pos_backwards_ + halfdt*(last_motor_vel_backwards_ + motor_vel);
515 
516  // Recalculate the spring force.
517  spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) / trans_compl_;
518  }
519 
520  // The spring force becomes the force seen by the joint.
521  js[0]->commanded_effort_ = spring_force;
522 
523  // Save the information that is to be used in the second half, i.e. in
524  // propagatePositionBackwards(). This includes the motor force
525  // (driving this cycle) and the time step (calculated here).
526  halfdt_backwards_ = halfdt;
527  motor_force_backwards_ = motor_force;
528 }
529 
530 
532  std::vector<JointState*>& js, std::vector<pr2_hardware_interface::Actuator*>& as)
533 {
534  // Check the arguments. This acts only on a single actuator/joint.
535  assert(as.size() == 1);
536  assert(js.size() == 1);
537 
538  // Again (as in the first half of the backward transmission) simulate
539  // the motor-mass / belt-spring-damper / joint-mass system. And
540  // again, all variables are in joint-space units. Only this time use
541  // the joint position/velocity provided by the simulator.
542  double motor_pos, motor_vel, motor_acc;
543  double joint_pos, joint_vel;
544  double motor_force;
545  double spring_force;
546  double halfdt;
547 
548  // Get the time step and motor force from the first half of the
549  // backward transmission.
550  halfdt = halfdt_backwards_;
551  motor_force = motor_force_backwards_;
552 
553  // Get the new joint position and velocity, as calculated by the
554  // simulator.
555  joint_pos = js[0]->position_ - js[0]->reference_position_;
556  joint_vel = js[0]->velocity_;
557 
558  // As in the first half, if the transmission compliance or time
559  // constant are zero, the transmission collapses to a regular rigid
560  // transmission.
561  if ((trans_compl_ == 0.0) || (trans_tau_ == 0.0))
562  {
563  // Immediately propagate the joint position/velocity to the motor,
564  // ignoring the (infinitely fast) model dynamics.
565  motor_acc = 0.0;
566  motor_vel = joint_vel;
567  motor_pos = joint_pos;
568  }
569  else
570  {
571  // Update the model. Note for numerical stability, we again lower
572  // bound the tranmission time constant by dt/2.
573  double tau = (halfdt < trans_tau_ ? trans_tau_ : halfdt);
574 
575  // Calculate the new motor position/velocity assuming a new motor
576  // acceleration of zero.
577  motor_vel = last_motor_vel_backwards_ + halfdt*(last_motor_acc_backwards_ + 0 );
578  motor_pos = last_motor_pos_backwards_ + halfdt*(last_motor_vel_backwards_ + motor_vel);
579 
580  // Calculate the spring force between the two masses.
581  spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) / trans_compl_;
582 
583  // This gives us the new motor acceleration.
584  motor_acc = (motor_force - spring_force) * trans_compl_ / (tau*tau + 2.0*tau*halfdt + halfdt*halfdt);
585 
586  // Recalculate the motor position/velocity, using this new acceleration.
587  motor_vel = last_motor_vel_backwards_ + halfdt*(last_motor_acc_backwards_ + motor_acc);
588  motor_pos = last_motor_pos_backwards_ + halfdt*(last_motor_vel_backwards_ + motor_vel);
589  }
590 
591  // Save the current state for the next cycle.
592  last_motor_pos_backwards_ = motor_pos;
593  last_motor_vel_backwards_ = motor_vel;
594  last_motor_acc_backwards_ = motor_acc;
595 
596  last_joint_pos_backwards_ = joint_pos;
597  last_joint_vel_backwards_ = joint_vel;
598 
599  // Push the motor position/velocity to the actuator, accounting for
600  // the mechanical reduction.
601  as[0]->state_.position_ = motor_pos * mechanical_reduction_;
602  as[0]->state_.velocity_ = motor_vel * mechanical_reduction_;
603 
604  // Also push the motor force to the actuator. Note we already assumed
605  // that the commanded actuator effort was accurately executed/applied,
606  // so the measured actuator effort is just the motor force.
607  as[0]->state_.last_measured_effort_ = motor_force / mechanical_reduction_;
608 
609 
610  // By storing the new actuator data, we are advancing to the next
611  // servo cycle. Always make sure the timing has been initialized.
613  {
614  // Set the time stamp to zero (it is measured relative to the start time).
615  as[0]->state_.sample_timestamp_ = ros::Duration(0);
616 
617  // Try to set the start time. Only then do we claim initialized.
618  if (ros::isStarted())
619  {
622  }
623  }
624  else
625  {
626  // Measure the time stamp relative to the start time.
627  as[0]->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
628  }
629  // Set the historical (double) timestamp accordingly.
630  as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.toSec();
631 
632 
633  // Simulate calibration sensors by filling out actuator states
635 }
636 
637 } // namespace
void simulateJointCalibration(pr2_mechanism_model::JointState *, pr2_hardware_interface::Actuator *)
void propagatePosition(std::vector< pr2_hardware_interface::Actuator *> &, std::vector< pr2_mechanism_model::JointState *> &)
Uses encoder data to fill out joint position and velocities.
void propagatePositionBackwards(std::vector< pr2_mechanism_model::JointState *> &, std::vector< pr2_hardware_interface::Actuator *> &)
Uses the joint position to fill out the actuator&#39;s encoder.
void propagateEffort(std::vector< pr2_mechanism_model::JointState *> &, std::vector< pr2_hardware_interface::Actuator *> &)
Uses commanded joint efforts to fill out commanded motor currents.
#define ROS_ERROR(...)
bool initXml(TiXmlElement *config, Robot *robot)
Initializes the transmission from XML data.
std::vector< std::string > actuator_names_
Definition: transmission.h:85
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()
std::vector< std::string > joint_names_
Definition: transmission.h:93
static Time now()
std::string name_
the name of the transmission
Definition: transmission.h:77
void propagateEffortBackwards(std::vector< pr2_hardware_interface::Actuator *> &, std::vector< pr2_mechanism_model::JointState *> &)
Uses the actuator&#39;s commanded effort to fill out the torque on the joint.


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Tue Mar 7 2023 03:54:53