pr2_gripper_transmission.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 /*
31  * Author: Stuart Glaser
32  */
33 
34 /*
35  * propagatePosition (from as to js)
36  * as position and velocity are doubled to get js, since gripper is two sided
37  * as torque is directly converted to gap_effort
38  * as torque to passive joint is /4 since there are 4 links
39  * propagateEffort (from js to as)
40  * popluate only as->commanded_.effort_
41  * this is directly transferred as torque and gap effort is 1to1
42  *
43  * below only for simulation
44  *
45  * propagatePositionBackwards (from js to as)
46  * as position and velocity transfers 1to1 to joint_angle (1-sided)
47  * as last_measured_effort_ should be 1to1 gap_effort of non-passive js->commanded_effort
48  * propagateEffortBackwards
49  * non-passive js->commanded_effort_ is 1to1 with MT
50  * passive js->commanded_effort_ is 1/2?? of MT converted to joint torques
51 
52  * Example transmission block
53  *
54  * <transmission name="r_gripper_trans" type="pr2_mechanism_model/PR2GripperTransmission">
55  * <actuator name="r_gripper_motor"/>
56  * <gap_joint L0="0.0375528" a="0.0683698" b="0.0433849" gear_ratio="40.095" h="0.0"
57  * mechanical_reduction="1.0" name="r_gripper_joint" phi0="0.518518122146"
58  * r="0.0915" screw_reduction="0.004" t0="-0.0001914" theta0="0.0628824676201"/>
59  * <!-- if a gazebo joint exists as [l|r]_gripper_joint, use this tag to have
60  * gripper transmission apply torque directly to prismatic joint
61  * this should be the default behavior in diamondback, deprecating this flag -->
62  *
63  * <!-- set passive joint angles so things look nice in rviz -->
64  * <passive_joint name="r_gripper_l_finger_joint"/>
65  * <passive_joint name="r_gripper_r_finger_joint"/>
66  * <passive_joint name="r_gripper_r_finger_tip_joint"/>
67  * <passive_joint name="r_gripper_l_finger_tip_joint"/>
68  *
69  * <!-- screw joint used to actuate gripper
70  * TODO: rename "name" to something like actuated_screw_joint
71  * -->
72  * <simulated_actuated_joint name="r_gripper_motor_screw_joint"
73  * passive_actuated_joint="r_gripper_motor_slider_joint"
74  * simulated_reduction="3141.6"/>
75  * </transmission>
76  *
77  */
80 #include <algorithm>
81 #include <numeric>
82 #include <angles/angles.h>
83 #include <boost/lexical_cast.hpp>
84 
85 #define PASSIVE_JOINTS 1
86 
87 using namespace pr2_hardware_interface;
88 using namespace pr2_mechanism_model;
89 
92 
93 bool PR2GripperTransmission::initXml(TiXmlElement *config, Robot *robot)
94 {
95  if (this->initXml(config))
96  {
97  // set robot actuator enabled
98  for (std::vector<std::string>::iterator actuator_name = actuator_names_.begin(); actuator_name != actuator_names_.end(); ++actuator_name)
99  {
100  if (robot->getActuator(*actuator_name))
101  {
102  robot->getActuator(*actuator_name)->command_.enable_ = true;
103  }
104  else
105  {
106  ROS_ERROR("PR2GripperTransmission actuator named \"%s\" not loaded in Robot", actuator_name->c_str());
107  return false;
108  }
109  }
110 
111  // look for joint_names_ in robot
112  for (std::vector<std::string>::iterator joint_name = joint_names_.begin(); joint_name != joint_names_.end(); ++joint_name)
113  {
114  if (!robot->robot_model_.getJoint(*joint_name))
115  {
116  ROS_ERROR("PR2GripperTransmission joint named \"%s\" not loaded in Robot", joint_name->c_str());
117  return false;
118  }
119  }
120 
121  // init successful
122  return true;
123  }
124  else
125  return false;
126 }
127 
128 bool PR2GripperTransmission::initXml(TiXmlElement *config)
129 {
130  const char *name = config->Attribute("name");
131  name_ = name ? name : "";
132  //myfile.open("transmission_data.txt");
133  TiXmlElement *ael = config->FirstChildElement("actuator");
134  const char *actuator_name = ael ? ael->Attribute("name") : NULL;
135  if (!actuator_name)
136  {
137  ROS_ERROR("PR2GripperTransmission could not find actuator named \"%s\"", actuator_name);
138  return false;
139  }
140  actuator_names_.push_back(actuator_name);
141 
142  for (TiXmlElement *j = config->FirstChildElement("gap_joint"); j; j = j->NextSiblingElement("gap_joint"))
143  {
144  const char *gap_joint_name = j->Attribute("name");
145  if (!gap_joint_name)
146  {
147  ROS_ERROR("PR2GripperTransmission did not specify joint name");
148  return false;
149  }
150  gap_joint_ = std::string(gap_joint_name);
151  joint_names_.push_back(gap_joint_name);
152 
153  // get the mechanical reduction
154  const char *joint_reduction = j->Attribute("mechanical_reduction");
155  if (!joint_reduction)
156  {
157  ROS_ERROR("PR2GripperTransmission's joint \"%s\" has no coefficient: mechanical reduction.", gap_joint_name);
158  return false;
159  }
160  try
161  {
162  gap_mechanical_reduction_ = boost::lexical_cast<double>(joint_reduction);
163  }
164  catch (boost::bad_lexical_cast &e)
165  {
166  ROS_ERROR("joint_reduction (%s) is not a float",joint_reduction);
167  return false;
168  }
169 
170  // get the screw drive reduction
171  const char *screw_reduction_str = j->Attribute("screw_reduction");
172  if (screw_reduction_str == NULL)
173  {
174  screw_reduction_ = 2.0/1000.0;
175  ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: screw drive reduction, using default for PR2 alpha2.", gap_joint_name);
176  }
177  else
178  try
179  {
180  screw_reduction_ = boost::lexical_cast<double>(screw_reduction_str);
181  }
182  catch (boost::bad_lexical_cast &e)
183  {
184  ROS_ERROR("screw_reduction (%s) is not a float",screw_reduction_str);
185  return false;
186  }
187 
188  // get the gear_ratio
189  const char *gear_ratio_str = j->Attribute("gear_ratio");
190  if (gear_ratio_str == NULL)
191  {
192  gear_ratio_ = 29.16;
193  ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: gear_ratio, using default for PR2 alpha2.", gap_joint_name);
194  }
195  else
196  try
197  {
198  gear_ratio_ = boost::lexical_cast<double>(gear_ratio_str);
199  }
200  catch (boost::bad_lexical_cast &e)
201  {
202  ROS_ERROR("gear_ratio (%s) is not a float",gear_ratio_str);
203  return false;
204  }
205 
206  // get the theta0 coefficient
207  const char *theta0_str = j->Attribute("theta0");
208  if (theta0_str == NULL)
209  {
210  theta0_ = 2.97571*M_PI/180.0;
211  ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: theta0, using default for PR2 alpha2.", gap_joint_name);
212  }
213  else
214  try
215  {
216  theta0_ = boost::lexical_cast<double>(theta0_str);
217  }
218  catch (boost::bad_lexical_cast &e)
219  {
220  ROS_ERROR("theta0 (%s) is not a float",theta0_str);
221  return false;
222  }
223  // get the phi0 coefficient
224  const char *phi0_str = j->Attribute("phi0");
225  if (phi0_str == NULL)
226  {
227  phi0_ = 29.98717*M_PI/180.0;
228  ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: phi0, using default for PR2 alpha2.", gap_joint_name);
229  }
230  else
231  try
232  {
233  phi0_ = boost::lexical_cast<double>(phi0_str);
234  }
235  catch (boost::bad_lexical_cast &e)
236  {
237  ROS_ERROR("phi0 (%s) is not a float",phi0_str);
238  return false;
239  }
240  // get the t0 coefficient
241  const char *t0_str = j->Attribute("t0");
242  if (t0_str == NULL)
243  {
244  t0_ = -0.19543/1000.0;
245  ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: t0, using default for PR2 alpha2.", gap_joint_name);
246  }
247  else
248  try
249  {
250  t0_ = boost::lexical_cast<double>(t0_str);
251  }
252  catch (boost::bad_lexical_cast &e)
253  {
254  ROS_ERROR("t0 (%s) is not a float",t0_str);
255  return false;
256  }
257  // get the L0 coefficient
258  const char *L0_str = j->Attribute("L0");
259  if (L0_str == NULL)
260  {
261  L0_ = 34.70821/1000.0;
262  ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: L0, using default for PR2 alpha2.", gap_joint_name);
263  }
264  else
265  try
266  {
267  L0_ = boost::lexical_cast<double>(L0_str);
268  }
269  catch (boost::bad_lexical_cast &e)
270  {
271  ROS_ERROR("L0 (%s) is not a float",L0_str);
272  return false;
273  }
274  // get the h coefficient
275  const char *h_str = j->Attribute("h");
276  if (h_str == NULL)
277  {
278  h_ = 5.200/1000.0;
279  ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: h, using default for PR2 alpha2.", gap_joint_name);
280  }
281  else
282  try
283  {
284  h_ = boost::lexical_cast<double>(h_str);
285  }
286  catch (boost::bad_lexical_cast &e)
287  {
288  ROS_ERROR("h (%s) is not a float",h_str);
289  return false;
290  }
291  // get the a coefficient
292  const char *a_str = j->Attribute("a");
293  if (a_str == NULL)
294  {
295  a_ = 67.56801/1000.0;
296  ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: a, using default for PR2 alpha2.", gap_joint_name);
297  }
298  else
299  try
300  {
301  a_ = boost::lexical_cast<double>(a_str);
302  }
303  catch (boost::bad_lexical_cast &e)
304  {
305  ROS_ERROR("a (%s) is not a float",a_str);
306  return false;
307  }
308  // get the b coefficient
309  const char *b_str = j->Attribute("b");
310  if (b_str == NULL)
311  {
312  b_ = 48.97193/1000.0;
313  ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: b, using default for PR2 alpha2.", gap_joint_name);
314  }
315  else
316  try
317  {
318  b_ = boost::lexical_cast<double>(b_str);
319  }
320  catch (boost::bad_lexical_cast &e)
321  {
322  ROS_ERROR("b (%s) is not a float",b_str);
323  return false;
324  }
325  // get the r coefficient
326  const char *r_str = j->Attribute("r");
327  if (r_str == NULL)
328  {
329  r_ = 91.50000/1000.0;
330  ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: r, using default for PR2 alpha2.", gap_joint_name);
331  }
332  else
333  try
334  {
335  r_ = boost::lexical_cast<double>(r_str);
336  }
337  catch (boost::bad_lexical_cast &e)
338  {
339  ROS_ERROR("r (%s) is not a float",r_str);
340  return false;
341  }
342  }
343 
344  // Print all coefficients
345  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",
346  name_.c_str(), a_, b_, r_, h_, L0_, t0_, theta0_, phi0_, gear_ratio_, screw_reduction_);
347 
348  // Get passive joint informations
349  for (TiXmlElement *j = config->FirstChildElement("passive_joint"); j; j = j->NextSiblingElement("passive_joint"))
350  {
351  const char *passive_joint_name = j->Attribute("name");
352  if (!passive_joint_name)
353  {
354  ROS_ERROR("PR2GripperTransmission did not specify joint name");
355  return false;
356  }
357 
358  // add joint name to list
359 #if PASSIVE_JOINTS
360  joint_names_.push_back(passive_joint_name);
361 #endif
362  passive_joints_.push_back(passive_joint_name);
363  }
364 
365  // Get screw joint informations
366  for (TiXmlElement *j = config->FirstChildElement("simulated_actuated_joint"); j; j = j->NextSiblingElement("simulated_actuated_joint"))
367  {
368  const char *simulated_actuated_joint_name = j->Attribute("name");
369  if (simulated_actuated_joint_name)
370  {
371 #if PASSIVE_JOINTS
372  joint_names_.push_back(simulated_actuated_joint_name);
373 #endif
374  }
375  else
376  {
377  ROS_ERROR("PR2GripperTransmission simulated_actuated_joint did snot specify joint name");
378  return false;
379  }
380 
381  // get the thread pitch
382  const char *simulated_reduction = j->Attribute("simulated_reduction");
383  if (!simulated_reduction)
384  {
385  ROS_ERROR("PR2GripperTransmission's simulated_actuated_joint \"%s\" has no coefficient: simulated_reduction.", simulated_actuated_joint_name);
386  return false;
387  }
388  try
389  {
390  simulated_reduction_ = boost::lexical_cast<double>(simulated_reduction);
391  }
392  catch (boost::bad_lexical_cast &e)
393  {
394  ROS_ERROR("simulated_reduction (%s) is not a float",simulated_reduction);
395  return false;
396  }
397 
398  // get any additional joint introduced from this screw joint implementation
399  // for the gripper, this is due to the limitation that screw constraint
400  // requires axis of rotation to be aligned with line between CG's of the two
401  // connected bodies. For this reason, an additional slider joint was introduced
402  // thus, requiring joint state to be published for motion planning packages
403  // and that's why we're here.
404  const char *passive_actuated_joint_name = j->Attribute("passive_actuated_joint");
405  if (passive_actuated_joint_name)
406  {
407  has_simulated_passive_actuated_joint_ = true;
408 #if PASSIVE_JOINTS
409  joint_names_.push_back(passive_actuated_joint_name);
410 #endif
411  }
412  }
413 
414  // assuming simulated gripper prismatic joint exists, use it
415 
416  return true;
417 }
418 
421 void PR2GripperTransmission::computeGapStates(
422  double MR,double MR_dot,double MT,
423  double &theta,double &dtheta_dMR,double &dt_dtheta,double &dt_dMR,double &gap_size,double &gap_velocity,double &gap_effort)
424 {
425  //
426  // below transforms from encoder value to gap size, based on 090224_link_data.xls provided by Functions Engineering
427  //
428  double u = (a_*a_+b_*b_-h_*h_
429  -pow(L0_+MR*screw_reduction_/gear_ratio_,2))/(2.0*a_*b_);
430  u = u < -1.0 ? -1.0 : u > 1.0 ? 1.0 : u;
431  theta = theta0_ - phi0_ + acos(u);
432  // limit theta
433  //theta = theta > 0 ? theta : 0;
434  // force theta to be greater than theta0_
435  //theta = angles::normalize_angle_positive(angles::shortest_angular_distance(theta0_,theta))+theta0_;
436 
437  gap_size = t0_ + r_ * ( sin(theta) - sin(theta0_) );
438 
439  //
440  // compute jacobians based on transforms, get the velocity of the gripper gap size based on encoder velocity
441  //
442  // for jacobian, we want to limit MR >= 0
443  MR = MR >= 0.0 ? MR : 0.0;
444  // then recompute u and theta based on restricted MR
445  u = (a_*a_+b_*b_-h_*h_
446  -pow(L0_+MR*screw_reduction_/gear_ratio_,2))/(2.0*a_*b_);
447  u = u < -1.0 ? -1.0 : u > 1.0 ? 1.0 : u;
448  double tmp_theta = theta0_ - phi0_ + acos(u);
449 
450  //
451  double arg = 1.0 - pow(u,2);
452  arg = arg > TOL ? arg : TOL; //LIMIT: CAP u at TOL artificially
453 
454  double du_dMR = -(L0_ * screw_reduction_)/(gear_ratio_*a_*b_) // d(arg)/d(MR)
455  -MR/(a_*b_)*pow(screw_reduction_/gear_ratio_,2);
456 
457  dtheta_dMR = -1.0/sqrt(arg) * du_dMR; // derivative of acos
458 
459  dt_dtheta = r_ * cos( tmp_theta );
460  dt_dMR = dt_dtheta * dtheta_dMR;
461  gap_velocity = MR_dot * dt_dMR;
462 
463  //
464  // get the effort at the gripper gap based on torque at the motor
465  // gap effort = motor torque * dmotor_theta/dt
466  // = MT * dmotor_theta_dt
467  // = MT * dMR_dt / (2*pi)
468  // = MT / dt_dMR * 2*pi
469  //
470  gap_effort = MT / dt_dMR / RAD2MR ;
471  //ROS_WARN("debug: %f %f %f",gap_effort,MT,dt_dMR,RAD2MR);
472 }
473 
478 void PR2GripperTransmission::inverseGapStates(
479  double gap_size,double &MR,double &dMR_dtheta,double &dtheta_dt,double &dMR_dt)
480 {
481  // get theta for jacobian calculation
482  double sin_theta = (gap_size - t0_)/r_ + sin(theta0_);
483  sin_theta = sin_theta > 1.0 ? 1.0 : sin_theta < -1.0 ? -1.0 : sin_theta;
484  double theta = asin(sin_theta);
485 
486  // compute inverse transform for the gap joint, returns MR and dMR_dtheta
487  inverseGapStates1(theta,MR,dMR_dtheta,dtheta_dt,dMR_dt);
488 }
489 
494 void PR2GripperTransmission::inverseGapStates1(
495  double theta,double &MR,double &dMR_dtheta,double &dtheta_dt,double &dMR_dt)
496 {
497  // limit theta
498  //theta = theta > 0 ? theta : 0;
499  // force theta to be greater than theta0_
500  //theta = angles::normalize_angle_positive(angles::shortest_angular_distance(theta0_,theta))+theta0_;
501 
502  // now do the reverse transform
503  double arg = -2.0*a_*b_*cos(theta-theta0_+phi0_)
504  -h_*h_+a_*a_+b_*b_;
505  if (arg > 0.0)
506  {
507  MR = gear_ratio_/screw_reduction_ * ( sqrt(arg) - L0_ );
508  dMR_dtheta = gear_ratio_/(2.0 * screw_reduction_) / sqrt(arg)
509  * 2.0 * a_ * b_ * sin(theta + phi0_ - theta0_);
510  }
511  else
512  {
513  MR = gear_ratio_/screw_reduction_ * ( 0.0 - L0_ );
514  dMR_dtheta = 0.0;
515  }
516 
517  // compute gap_size from theta
518  double gap_size = t0_ + r_ * ( sin(theta) - sin(theta0_) ); // in mm
519 
520  // compute inverse jacobians for the transform
521  // for this, enforce dMR_dtheta >= 0
522  // since there are two roots, take the positive root
523  // @todo: this affects sim only, need to check this for sim.
524  double tmp_dMR_dtheta = fabs(dMR_dtheta);
525 
526  double u = (gap_size - t0_)/r_ + sin(theta0_);
527  double arg2 = 1.0 - pow(u,2);
528  arg2 = arg2 > TOL ? arg2 : TOL; //LIMIT: CAP arg2 at TOL artificially
529  dtheta_dt = 1.0 / sqrt( arg2 ) / r_; // derivative of asin
530  dMR_dt = tmp_dMR_dtheta * dtheta_dt; // remember, here, t is gap_size
531 }
532 
533 
537 void PR2GripperTransmission::propagatePosition(
538  std::vector<Actuator*>& as, std::vector<JointState*>& js)
539 {
540 
541  ROS_ASSERT(as.size() == 1);
542  // js has passive joints and 1 gap joint and 1 screw joint
543 #if PASSIVE_JOINTS
544  if (has_simulated_passive_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);}
545  else ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 1);
546 #endif
547 
551  double MR = as[0]->state_.position_ / gap_mechanical_reduction_ * RAD2MR ;
552 
554  double MR_dot = as[0]->state_.velocity_ / gap_mechanical_reduction_ * RAD2MR ;
555 
565  //double MT = as[0]->state_.last_measured_effort_ / gap_mechanical_reduction_ * RAD2MR ;
566 
567 
569  double MT = as[0]->state_.last_measured_effort_ / gap_mechanical_reduction_;
570 
572  double theta, dtheta_dMR, dt_dtheta, dt_dMR;
574  double gap_size,gap_velocity,gap_effort;
575 
576  // compute gap position, velocity, measured_effort from actuator states
577  computeGapStates(MR,MR_dot,MT,theta,dtheta_dMR, dt_dtheta, dt_dMR,gap_size,gap_velocity,gap_effort);
578 
579  // Determines the state of the gap joint.
580  js[0]->position_ = gap_size*2.0; // function engineering's transmission give half the total gripper size
581  js[0]->velocity_ = gap_velocity*2.0;
582  js[0]->measured_effort_ = gap_effort/2.0;
583  //ROS_ERROR("prop pos eff=%f",js[0]->measured_effort_);
584 
585 #if PASSIVE_JOINTS
586  // Determines the states of the passive joints.
587  // we need to do this for each finger, in simulation, each finger has it's state filled out
588  for (size_t i = 1; i < passive_joints_.size()+1; ++i)
589  {
590  js[i]->position_ = theta - theta0_;
591  js[i]->velocity_ = dtheta_dMR * MR_dot;
592  js[i]->measured_effort_ = MT / dtheta_dMR / RAD2MR;
593  js[i]->reference_position_ = MT / dtheta_dMR / RAD2MR;
594  }
595 
596  // screw joint state is not important to us, fill with zeros
597  js[passive_joints_.size()+1]->position_ = 0.0;
598  js[passive_joints_.size()+1]->velocity_ = 0.0;
599  js[passive_joints_.size()+1]->measured_effort_ = 0.0;
600  js[passive_joints_.size()+1]->reference_position_ = 0.0;
601  js[passive_joints_.size()+1]->calibrated_ = true; // treat passive simulation joints as "calibrated"
602 
603  if (has_simulated_passive_actuated_joint_)
604  {
605  // screw joint state is not important to us, fill with zeros
606  js[passive_joints_.size()+2]->position_ = 0.0;
607  js[passive_joints_.size()+2]->velocity_ = 0.0;
608  js[passive_joints_.size()+2]->measured_effort_ = 0.0;
609  js[passive_joints_.size()+2]->reference_position_ = 0.0;
610  js[passive_joints_.size()+2]->calibrated_ = true; // treat passive simulation joints as "calibrated"
611  }
612 #endif
613 }
614 
615 // this is needed for simulation, so we can recover encoder value given joint angles
616 void PR2GripperTransmission::propagatePositionBackwards(
617  std::vector<JointState*>& js, std::vector<Actuator*>& as)
618 {
619  ROS_ASSERT(as.size() == 1);
620 #if PASSIVE_JOINTS
621  if (has_simulated_passive_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);}
622  else {ROS_ASSERT(js.size() == 1);}
623 #endif
624  ROS_DEBUG("js [%zd], pjs [%zd]", js.size(), passive_joints_.size());
625 
626  // keep the simulation stable by using the minimum rate joint to compute gripper gap rate
627  double MR,dMR_dtheta,dtheta_dt,dMR_dt;
628  double joint_rate;
629  {
630  // new gripper model has an actual physical slider joint in simulation
631  // use the new slider joint for determining gipper position, so forward/backward are consistent
632  double gap_size = js[0]->position_/2.0; // js position should be normalized
633  // compute inverse transform for the gap joint, returns MR and dMR_dtheta
634  inverseGapStates(gap_size,MR,dMR_dtheta,dtheta_dt,dMR_dt);
635  double gap_rate = js[0]->velocity_/2.0;
636  joint_rate = gap_rate * dtheta_dt;
637  }
638  double gap_effort = js[0]->commanded_effort_;
639 
640  //ROS_ERROR("prop pos back eff=%f",gap_effort);
641 
643  as[0]->state_.position_ = MR * gap_mechanical_reduction_ / RAD2MR ;
644 
647  as[0]->state_.velocity_ = joint_rate * dMR_dtheta * gap_mechanical_reduction_ / RAD2MR ;
648 
652  as[0]->state_.last_measured_effort_ = 2.0*gap_effort / dMR_dt * RAD2MR * gap_mechanical_reduction_;
653 
654  // Update the timing (making sure it's initialized).
655  if (!simulated_actuator_timestamp_initialized_)
656  {
657  // Set the time stamp to zero (it is measured relative to the start time).
658  as[0]->state_.sample_timestamp_ = ros::Duration(0);
659 
660  // Try to set the start time. Only then do we claim initialized.
661  if (ros::isStarted())
662  {
663  simulated_actuator_start_time_ = ros::Time::now();
664  simulated_actuator_timestamp_initialized_ = true;
665  }
666  }
667  else
668  {
669  // Measure the time stamp relative to the start time.
670  as[0]->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
671  }
672  // Set the historical (double) timestamp accordingly.
673  as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.toSec();
674 
675  // simulate calibration sensors by filling out actuator states
676  this->joint_calibration_simulator_.simulateJointCalibration(js[0],as[0]);
677 }
678 
679 void PR2GripperTransmission::propagateEffort(
680  std::vector<JointState*>& js, std::vector<Actuator*>& as)
681 {
682  ROS_ASSERT(as.size() == 1);
683 #if PASSIVE_JOINTS
684  if (has_simulated_passive_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);}
685  else {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 1);}
686 #endif
687 
688  //
689  // in hardware, the position of passive joints are set by propagatePosition, so they should be identical and
690  // the inverse transform should be consistent.
691  // note for simulation:
692  // new gripper model has an actual physical slider joint in simulation
693  // use the new slider joint for determining gipper position, so forward/backward are consistent
694  double gap_size = js[0]->position_/2.0; // js position should be normalized
695 
696  // now do the reverse transform
697  double MR,dMR_dtheta,dtheta_dt,dMR_dt;
698  // compute inverse transform for the gap joint, returns MR and dMR_dtheta
699  inverseGapStates(gap_size,MR,dMR_dtheta,dtheta_dt,dMR_dt);
700 
701  double gap_effort = js[0]->commanded_effort_;
702 
703  //ROS_ERROR("prop eff eff=%f",gap_effort);
704 
706  as[0]->command_.enable_ = true;
707  as[0]->command_.effort_ = 2.0*gap_effort / dMR_dt * RAD2MR * gap_mechanical_reduction_;
708 }
709 
710 void PR2GripperTransmission::propagateEffortBackwards(
711  std::vector<Actuator*>& as, std::vector<JointState*>& js)
712 {
713  ROS_ASSERT(as.size() == 1);
714 #if PASSIVE_JOINTS
715  ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);
716 #endif
717  ROS_ASSERT(simulated_reduction_>0.0);
718 
719  //
720  // below transforms from encoder value to gap size, based on 090224_link_data.xls provided by Functions Engineering
721  //
723  double MR = as[0]->state_.position_ / gap_mechanical_reduction_ * RAD2MR ;
724  double MR_dot = as[0]->state_.velocity_ / gap_mechanical_reduction_ * RAD2MR ;
725  double MT = as[0]->command_.effort_ / gap_mechanical_reduction_;
726 
728  double theta, dtheta_dMR, dt_dtheta, dt_dMR;
730  double gap_size,gap_velocity,gap_effort;
731 
732  // compute gap position, velocity, measured_effort from actuator states
733  computeGapStates(MR,MR_dot,MT,theta,dtheta_dMR, dt_dtheta, dt_dMR,gap_size,gap_velocity,gap_effort);
734 
735 #if PASSIVE_JOINTS
736  {
737  // propagate fictitious joint effort backwards
738  // ROS_ERROR("prop eff back eff=%f",js[0]->commanded_effort_);
739 
740  // set screw joint effort if simulated
741  js[passive_joints_.size()+1]->commanded_effort_ = gap_effort/simulated_reduction_;
742  }
743 #endif
744 }
745 
#define RAD2MR
#define ROS_WARN(...)
PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::PR2GripperTransmission, pr2_mechanism_model::Transmission) bool PR2GripperTransmission
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
ROSCPP_DECL bool isStarted()
#define TOL
static Time now()
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
#define ROS_ASSERT(cond)
#define ROS_ERROR(...)
This class provides the controllers with an interface to the robot model.
Definition: robot.h:78
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
#define ROS_DEBUG(...)


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