pr2_gripper_transmission.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Author: Stuart Glaser
00032  */
00033 
00034 /*
00035  * propagatePosition (from as to js)
00036  *   as position and velocity are doubled to get js, since gripper is two sided
00037  *   as torque is directly converted to gap_effort
00038  *   as torque to passive joint is /4 since there are 4 links
00039  * propagateEffort (from js to as)
00040  *   popluate only as->commanded_.effort_
00041  *     this is directly transferred as torque and gap effort is 1to1
00042  *
00043  * below only for simulation
00044  *
00045  * propagatePositionBackwards (from js to as)
00046  *   as position and velocity transfers 1to1 to joint_angle (1-sided)
00047  *   as last_measured_effort_ should be 1to1 gap_effort of non-passive js->commanded_effort
00048  * propagateEffortBackwards
00049  *   non-passive js->commanded_effort_ is 1to1 with MT
00050  *   passive js->commanded_effort_ is 1/2?? of MT converted to joint torques
00051 
00052  * Example transmission block
00053  *
00054  * <transmission name="r_gripper_trans" type="pr2_mechanism_model/PR2GripperTransmission">
00055  *   <actuator name="r_gripper_motor"/>
00056  *   <gap_joint L0="0.0375528" a="0.0683698" b="0.0433849" gear_ratio="40.095" h="0.0"
00057  *              mechanical_reduction="1.0" name="r_gripper_joint" phi0="0.518518122146"
00058  *              r="0.0915" screw_reduction="0.004" t0="-0.0001914" theta0="0.0628824676201"/>
00059  *   <!-- if a gazebo joint exists as [l|r]_gripper_joint, use this tag to have
00060  *        gripper transmission apply torque directly to prismatic joint
00061  *        this should be the default behavior in diamondback, deprecating this flag -->
00062  *
00063  *   <!-- set passive joint angles so things look nice in rviz -->
00064  *   <passive_joint name="r_gripper_l_finger_joint"/>
00065  *   <passive_joint name="r_gripper_r_finger_joint"/>
00066  *   <passive_joint name="r_gripper_r_finger_tip_joint"/>
00067  *   <passive_joint name="r_gripper_l_finger_tip_joint"/>
00068  *
00069  *   <!-- screw joint used to actuate gripper
00070  *     TODO: rename "name" to something like actuated_screw_joint
00071  *   -->
00072  *   <simulated_actuated_joint name="r_gripper_motor_screw_joint"
00073  *                             passive_actuated_joint="r_gripper_motor_slider_joint"
00074  *                             simulated_reduction="3141.6"/>
00075  * </transmission>
00076  *
00077  */
00078 #include "pr2_mechanism_model/pr2_gripper_transmission.h"
00079 #include <pluginlib/class_list_macros.h>
00080 #include <algorithm>
00081 #include <numeric>
00082 #include <angles/angles.h>
00083 #include <boost/lexical_cast.hpp>
00084 
00085 #define PASSIVE_JOINTS 1
00086 
00087 using namespace pr2_hardware_interface;
00088 using namespace pr2_mechanism_model;
00089 
00090 PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::PR2GripperTransmission,
00091                          pr2_mechanism_model::Transmission)
00092 
00093 bool PR2GripperTransmission::initXml(TiXmlElement *config, Robot *robot)
00094 {
00095   if (this->initXml(config))
00096   {
00097     // set robot actuator enabled
00098     for (std::vector<std::string>::iterator actuator_name = actuator_names_.begin(); actuator_name != actuator_names_.end(); ++actuator_name)
00099     {
00100       if (robot->getActuator(*actuator_name))
00101       {
00102         robot->getActuator(*actuator_name)->command_.enable_ = true;
00103       }
00104       else
00105       {
00106         ROS_ERROR("PR2GripperTransmission actuator named \"%s\" not loaded in Robot", actuator_name->c_str());
00107         return false;
00108       }
00109     }
00110 
00111     // look for joint_names_ in robot
00112     for (std::vector<std::string>::iterator joint_name = joint_names_.begin(); joint_name != joint_names_.end(); ++joint_name)
00113     {
00114       if (!robot->robot_model_.getJoint(*joint_name))
00115       {
00116         ROS_ERROR("PR2GripperTransmission joint named \"%s\" not loaded in Robot", joint_name->c_str());
00117         return false;
00118       }
00119     }
00120 
00121     // init successful
00122     return true;
00123   }
00124   else
00125     return false;
00126 }
00127 
00128 bool PR2GripperTransmission::initXml(TiXmlElement *config)
00129 {
00130   const char *name = config->Attribute("name");
00131   name_ = name ? name : "";
00132   //myfile.open("transmission_data.txt");
00133   TiXmlElement *ael = config->FirstChildElement("actuator");
00134   const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00135   if (!actuator_name)
00136   {
00137     ROS_ERROR("PR2GripperTransmission could not find actuator named \"%s\"", actuator_name);
00138     return false;
00139   }
00140   actuator_names_.push_back(actuator_name);
00141 
00142   for (TiXmlElement *j = config->FirstChildElement("gap_joint"); j; j = j->NextSiblingElement("gap_joint"))
00143   {
00144     const char *gap_joint_name = j->Attribute("name");
00145     if (!gap_joint_name)
00146     {
00147       ROS_ERROR("PR2GripperTransmission did not specify joint name");
00148       return false;
00149     }
00150     gap_joint_ = std::string(gap_joint_name);
00151     joint_names_.push_back(gap_joint_name);
00152 
00153     // get the mechanical reduction
00154     const char *joint_reduction = j->Attribute("mechanical_reduction");
00155     if (!joint_reduction)
00156     {
00157       ROS_ERROR("PR2GripperTransmission's joint \"%s\" has no coefficient: mechanical reduction.", gap_joint_name);
00158       return false;
00159     }
00160     try
00161     {
00162       gap_mechanical_reduction_ = boost::lexical_cast<double>(joint_reduction);
00163     }
00164     catch (boost::bad_lexical_cast &e)
00165     {
00166       ROS_ERROR("joint_reduction (%s) is not a float",joint_reduction);
00167       return false;
00168     }
00169 
00170     // get the screw drive reduction
00171     const char *screw_reduction_str = j->Attribute("screw_reduction");
00172     if (screw_reduction_str == NULL)
00173     {
00174       screw_reduction_ = 2.0/1000.0;
00175       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: screw drive reduction, using default for PR2 alpha2.", gap_joint_name);
00176     }
00177     else
00178       try
00179       {
00180         screw_reduction_ = boost::lexical_cast<double>(screw_reduction_str);
00181       }
00182       catch (boost::bad_lexical_cast &e)
00183       {
00184         ROS_ERROR("screw_reduction (%s) is not a float",screw_reduction_str);
00185         return false;
00186       }
00187 
00188     // get the gear_ratio
00189     const char *gear_ratio_str = j->Attribute("gear_ratio");
00190     if (gear_ratio_str == NULL)
00191     {
00192       gear_ratio_ = 29.16;
00193       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: gear_ratio, using default for PR2 alpha2.", gap_joint_name);
00194     }
00195     else
00196       try
00197       {
00198         gear_ratio_ = boost::lexical_cast<double>(gear_ratio_str);
00199       }
00200       catch (boost::bad_lexical_cast &e)
00201       {
00202         ROS_ERROR("gear_ratio (%s) is not a float",gear_ratio_str);
00203         return false;
00204       }
00205 
00206     // get the theta0 coefficient
00207     const char *theta0_str = j->Attribute("theta0");
00208     if (theta0_str == NULL)
00209     {
00210       theta0_ = 2.97571*M_PI/180.0;
00211       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: theta0, using default for PR2 alpha2.", gap_joint_name);
00212     }
00213     else
00214       try
00215       {
00216         theta0_ = boost::lexical_cast<double>(theta0_str);
00217       }
00218       catch (boost::bad_lexical_cast &e)
00219       {
00220         ROS_ERROR("theta0 (%s) is not a float",theta0_str);
00221         return false;
00222       }
00223     // get the phi0 coefficient
00224     const char *phi0_str = j->Attribute("phi0");
00225     if (phi0_str == NULL)
00226     {
00227       phi0_ = 29.98717*M_PI/180.0;
00228       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: phi0, using default for PR2 alpha2.", gap_joint_name);
00229     }
00230     else
00231       try
00232       {
00233         phi0_ = boost::lexical_cast<double>(phi0_str);
00234       }
00235       catch (boost::bad_lexical_cast &e)
00236       {
00237         ROS_ERROR("phi0 (%s) is not a float",phi0_str);
00238         return false;
00239       }
00240     // get the t0 coefficient
00241     const char *t0_str = j->Attribute("t0");
00242     if (t0_str == NULL)
00243     {
00244       t0_ = -0.19543/1000.0;
00245       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: t0, using default for PR2 alpha2.", gap_joint_name);
00246     }
00247     else
00248       try
00249       {
00250         t0_ = boost::lexical_cast<double>(t0_str);
00251       }
00252       catch (boost::bad_lexical_cast &e)
00253       {
00254         ROS_ERROR("t0 (%s) is not a float",t0_str);
00255         return false;
00256       }
00257     // get the L0 coefficient
00258     const char *L0_str = j->Attribute("L0");
00259     if (L0_str == NULL)
00260     {
00261       L0_ = 34.70821/1000.0;
00262       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: L0, using default for PR2 alpha2.", gap_joint_name);
00263     }
00264     else
00265       try
00266       {
00267         L0_ = boost::lexical_cast<double>(L0_str);
00268       }
00269       catch (boost::bad_lexical_cast &e)
00270       {
00271         ROS_ERROR("L0 (%s) is not a float",L0_str);
00272         return false;
00273       }
00274     // get the h coefficient
00275     const char *h_str = j->Attribute("h");
00276     if (h_str == NULL)
00277     {
00278       h_ = 5.200/1000.0;
00279       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: h, using default for PR2 alpha2.", gap_joint_name);
00280     }
00281     else
00282       try
00283       {
00284         h_ = boost::lexical_cast<double>(h_str);
00285       }
00286       catch (boost::bad_lexical_cast &e)
00287       {
00288         ROS_ERROR("h (%s) is not a float",h_str);
00289         return false;
00290       }
00291     // get the a coefficient
00292     const char *a_str = j->Attribute("a");
00293     if (a_str == NULL)
00294     {
00295       a_ = 67.56801/1000.0;
00296       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: a, using default for PR2 alpha2.", gap_joint_name);
00297     }
00298     else
00299       try
00300       {
00301         a_ = boost::lexical_cast<double>(a_str);
00302       }
00303       catch (boost::bad_lexical_cast &e)
00304       {
00305         ROS_ERROR("a (%s) is not a float",a_str);
00306         return false;
00307       }
00308     // get the b coefficient
00309     const char *b_str = j->Attribute("b");
00310     if (b_str == NULL)
00311     {
00312       b_ = 48.97193/1000.0;
00313       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: b, using default for PR2 alpha2.", gap_joint_name);
00314     }
00315     else
00316       try
00317       {
00318         b_ = boost::lexical_cast<double>(b_str);
00319       }
00320       catch (boost::bad_lexical_cast &e)
00321       {
00322         ROS_ERROR("b (%s) is not a float",b_str);
00323         return false;
00324       }
00325     // get the r coefficient
00326     const char *r_str = j->Attribute("r");
00327     if (r_str == NULL)
00328     {
00329       r_ = 91.50000/1000.0;
00330       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: r, using default for PR2 alpha2.", gap_joint_name);
00331     }
00332     else
00333       try
00334       {
00335         r_ = boost::lexical_cast<double>(r_str);
00336       }
00337       catch (boost::bad_lexical_cast &e)
00338       {
00339         ROS_ERROR("r (%s) is not a float",r_str);
00340         return false;
00341       }
00342   }
00343 
00344   // Print all coefficients
00345   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",
00346             name_.c_str(), a_, b_, r_, h_, L0_, t0_, theta0_, phi0_, gear_ratio_, screw_reduction_);
00347 
00348   // Get passive joint informations
00349   for (TiXmlElement *j = config->FirstChildElement("passive_joint"); j; j = j->NextSiblingElement("passive_joint"))
00350   {
00351     const char *passive_joint_name = j->Attribute("name");
00352     if (!passive_joint_name)
00353     {
00354       ROS_ERROR("PR2GripperTransmission did not specify joint name");
00355       return false;
00356     }
00357 
00358     // add joint name to list
00359 #if PASSIVE_JOINTS
00360     joint_names_.push_back(passive_joint_name);
00361 #endif
00362     passive_joints_.push_back(passive_joint_name);
00363   }
00364 
00365   // Get screw joint informations
00366   for (TiXmlElement *j = config->FirstChildElement("simulated_actuated_joint"); j; j = j->NextSiblingElement("simulated_actuated_joint"))
00367   {
00368     const char *simulated_actuated_joint_name = j->Attribute("name");
00369     if (simulated_actuated_joint_name)
00370     {
00371 #if PASSIVE_JOINTS
00372       joint_names_.push_back(simulated_actuated_joint_name);
00373 #endif
00374     }
00375     else
00376     {
00377       ROS_ERROR("PR2GripperTransmission simulated_actuated_joint did snot specify joint name");
00378       return false;
00379     }
00380 
00381     // get the thread pitch
00382     const char *simulated_reduction = j->Attribute("simulated_reduction");
00383     if (!simulated_reduction)
00384     {
00385       ROS_ERROR("PR2GripperTransmission's simulated_actuated_joint \"%s\" has no coefficient: simulated_reduction.", simulated_actuated_joint_name);
00386       return false;
00387     }
00388     try
00389     {
00390       simulated_reduction_ = boost::lexical_cast<double>(simulated_reduction);
00391     }
00392     catch (boost::bad_lexical_cast &e)
00393     {
00394       ROS_ERROR("simulated_reduction (%s) is not a float",simulated_reduction);
00395       return false;
00396     }
00397 
00398     // get any additional joint introduced from this screw joint implementation
00399     // for the gripper, this is due to the limitation that screw constraint
00400     // requires axis of rotation to be aligned with line between CG's of the two
00401     // connected bodies.  For this reason, an additional slider joint was introduced
00402     // thus, requiring joint state to be published for motion planning packages
00403     // and that's why we're here.
00404     const char *passive_actuated_joint_name = j->Attribute("passive_actuated_joint");
00405     if (passive_actuated_joint_name)
00406     {
00407       has_simulated_passive_actuated_joint_ = true;
00408 #if PASSIVE_JOINTS
00409       joint_names_.push_back(passive_actuated_joint_name);
00410 #endif
00411     }
00412   }
00413 
00414   // assuming simulated gripper prismatic joint exists, use it
00415 
00416   return true;
00417 }
00418 
00421 void PR2GripperTransmission::computeGapStates(
00422   double MR,double MR_dot,double MT,
00423   double &theta,double &dtheta_dMR,double &dt_dtheta,double &dt_dMR,double &gap_size,double &gap_velocity,double &gap_effort)
00424 {
00425   //
00426   // below transforms from encoder value to gap size, based on 090224_link_data.xls provided by Functions Engineering
00427   //
00428   double u            = (a_*a_+b_*b_-h_*h_
00429                          -pow(L0_+MR*screw_reduction_/gear_ratio_,2))/(2.0*a_*b_);
00430   u                   = u < -1.0 ? -1.0 : u > 1.0 ? 1.0 : u;
00431   theta               = theta0_ - phi0_ + acos(u);
00432   // limit theta
00433   //theta = theta > 0 ? theta : 0;
00434   // force theta to be greater than theta0_
00435   //theta = angles::normalize_angle_positive(angles::shortest_angular_distance(theta0_,theta))+theta0_;
00436 
00437   gap_size            = t0_ + r_ * ( sin(theta) - sin(theta0_) );
00438 
00439   //
00440   // compute jacobians based on transforms, get the velocity of the gripper gap size based on encoder velocity
00441   //
00442   // for jacobian, we want to limit MR >= 0
00443   MR = MR >= 0.0 ? MR : 0.0;
00444   // then recompute u and theta based on restricted MR
00445   u                   = (a_*a_+b_*b_-h_*h_
00446                          -pow(L0_+MR*screw_reduction_/gear_ratio_,2))/(2.0*a_*b_);
00447   u                   = u < -1.0 ? -1.0 : u > 1.0 ? 1.0 : u;
00448   double tmp_theta    = theta0_ - phi0_ + acos(u);
00449 
00450   //
00451   double arg          = 1.0 - pow(u,2);
00452   arg                 = arg > TOL ? arg : TOL; //LIMIT: CAP u at TOL artificially
00453 
00454   double du_dMR       = -(L0_ * screw_reduction_)/(gear_ratio_*a_*b_) // d(arg)/d(MR)
00455                         -MR/(a_*b_)*pow(screw_reduction_/gear_ratio_,2);
00456 
00457   dtheta_dMR          = -1.0/sqrt(arg) * du_dMR; // derivative of acos
00458 
00459   dt_dtheta           = r_ * cos( tmp_theta );
00460   dt_dMR              = dt_dtheta * dtheta_dMR;
00461   gap_velocity        = MR_dot * dt_dMR;
00462 
00463   //
00464   // get the effort at the gripper gap based on torque at the motor
00465   // gap effort = motor torque         * dmotor_theta/dt
00466   //            = MT                   * dmotor_theta_dt
00467   //            = MT                   * dMR_dt          / (2*pi)
00468   //            = MT                   / dt_dMR          * 2*pi
00469   //
00470   gap_effort          = MT      / dt_dMR / RAD2MR ;
00471   //ROS_WARN("debug: %f %f %f",gap_effort,MT,dt_dMR,RAD2MR);
00472 }
00473 
00478 void PR2GripperTransmission::inverseGapStates(
00479   double gap_size,double &MR,double &dMR_dtheta,double &dtheta_dt,double &dMR_dt)
00480 {
00481     // get theta for jacobian calculation
00482     double sin_theta        = (gap_size - t0_)/r_ + sin(theta0_);
00483     sin_theta = sin_theta > 1.0 ? 1.0 : sin_theta < -1.0 ? -1.0 : sin_theta;
00484     double theta            = asin(sin_theta);
00485 
00486     // compute inverse transform for the gap joint, returns MR and dMR_dtheta
00487     inverseGapStates1(theta,MR,dMR_dtheta,dtheta_dt,dMR_dt);
00488 }
00489 
00494 void PR2GripperTransmission::inverseGapStates1(
00495   double theta,double &MR,double &dMR_dtheta,double &dtheta_dt,double &dMR_dt)
00496 {
00497   // limit theta
00498   //theta = theta > 0 ? theta : 0;
00499   // force theta to be greater than theta0_
00500   //theta = angles::normalize_angle_positive(angles::shortest_angular_distance(theta0_,theta))+theta0_;
00501 
00502   // now do the reverse transform
00503   double arg         = -2.0*a_*b_*cos(theta-theta0_+phi0_)
00504                                    -h_*h_+a_*a_+b_*b_;
00505   if (arg > 0.0)
00506   {
00507     MR               = gear_ratio_/screw_reduction_ * ( sqrt(arg) - L0_ );
00508     dMR_dtheta       = gear_ratio_/(2.0 * screw_reduction_) / sqrt(arg)
00509                        * 2.0 * a_ * b_ * sin(theta + phi0_ - theta0_);
00510   }
00511   else
00512   {
00513     MR               = gear_ratio_/screw_reduction_ * ( 0.0       - L0_ );
00514     dMR_dtheta       = 0.0;
00515   }
00516 
00517   // compute gap_size from theta
00518   double gap_size = t0_ + r_ * ( sin(theta) - sin(theta0_) ); // in mm
00519 
00520   // compute inverse jacobians for the transform
00521   // for this, enforce dMR_dtheta >= 0
00522   // since there are two roots, take the positive root
00523   // @todo: this affects sim only, need to check this for sim.
00524   double tmp_dMR_dtheta = fabs(dMR_dtheta);
00525 
00526   double u           = (gap_size - t0_)/r_ + sin(theta0_);
00527   double arg2        = 1.0 - pow(u,2);
00528   arg2               = arg2 > TOL ? arg2 : TOL; //LIMIT: CAP arg2 at TOL artificially
00529   dtheta_dt          = 1.0 / sqrt( arg2 ) / r_;  // derivative of asin
00530   dMR_dt             = tmp_dMR_dtheta * dtheta_dt;  // remember, here, t is gap_size
00531 }
00532 
00533 
00537 void PR2GripperTransmission::propagatePosition(
00538   std::vector<Actuator*>& as, std::vector<JointState*>& js)
00539 {
00540 
00541   ROS_ASSERT(as.size() == 1);
00542   // js has passive joints and 1 gap joint and 1 screw joint
00543 #if PASSIVE_JOINTS
00544   if (has_simulated_passive_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);}
00545   else ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 1);
00546 #endif
00547 
00551   double MR        = as[0]->state_.position_ / gap_mechanical_reduction_ * RAD2MR ;
00552 
00554   double MR_dot    = as[0]->state_.velocity_ / gap_mechanical_reduction_ * RAD2MR ;
00555 
00565   //double MT        = as[0]->state_.last_measured_effort_ / gap_mechanical_reduction_ * RAD2MR ;
00566 
00567 
00569   double MT        = as[0]->state_.last_measured_effort_ / gap_mechanical_reduction_;
00570 
00572   double theta, dtheta_dMR, dt_dtheta, dt_dMR;
00574   double gap_size,gap_velocity,gap_effort;
00575 
00576   // compute gap position, velocity, measured_effort from actuator states
00577   computeGapStates(MR,MR_dot,MT,theta,dtheta_dMR, dt_dtheta, dt_dMR,gap_size,gap_velocity,gap_effort);
00578 
00579   // Determines the state of the gap joint.
00580   js[0]->position_        = gap_size*2.0; // function engineering's transmission give half the total gripper size
00581   js[0]->velocity_        = gap_velocity*2.0;
00582   js[0]->measured_effort_ = gap_effort/2.0;
00583   //ROS_ERROR("prop pos eff=%f",js[0]->measured_effort_);
00584 
00585 #if PASSIVE_JOINTS
00586   // Determines the states of the passive joints.
00587   // we need to do this for each finger, in simulation, each finger has it's state filled out
00588   for (size_t i = 1; i < passive_joints_.size()+1; ++i)
00589   {
00590     js[i]->position_           = theta - theta0_;
00591     js[i]->velocity_           = dtheta_dMR * MR_dot;
00592     js[i]->measured_effort_    = MT / dtheta_dMR / RAD2MR;
00593     js[i]->reference_position_ = MT / dtheta_dMR / RAD2MR;
00594   }
00595 
00596   // screw joint state is not important to us, fill with zeros
00597   js[passive_joints_.size()+1]->position_           = 0.0;
00598   js[passive_joints_.size()+1]->velocity_           = 0.0;
00599   js[passive_joints_.size()+1]->measured_effort_    = 0.0;
00600   js[passive_joints_.size()+1]->reference_position_ = 0.0;
00601   js[passive_joints_.size()+1]->calibrated_         = true; // treat passive simulation joints as "calibrated"
00602 
00603   if (has_simulated_passive_actuated_joint_)
00604   {
00605     // screw joint state is not important to us, fill with zeros
00606     js[passive_joints_.size()+2]->position_           = 0.0;
00607     js[passive_joints_.size()+2]->velocity_           = 0.0;
00608     js[passive_joints_.size()+2]->measured_effort_    = 0.0;
00609     js[passive_joints_.size()+2]->reference_position_ = 0.0;
00610     js[passive_joints_.size()+2]->calibrated_         = true; // treat passive simulation joints as "calibrated"
00611   }
00612 #endif
00613 }
00614 
00615 // this is needed for simulation, so we can recover encoder value given joint angles
00616 void PR2GripperTransmission::propagatePositionBackwards(
00617   std::vector<JointState*>& js, std::vector<Actuator*>& as)
00618 {
00619   ROS_ASSERT(as.size() == 1);
00620 #if PASSIVE_JOINTS
00621   if (has_simulated_passive_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);}
00622   else {ROS_ASSERT(js.size() == 1);}
00623 #endif
00624   ROS_DEBUG("js [%zd], pjs [%zd]", js.size(), passive_joints_.size());
00625 
00626   // keep the simulation stable by using the minimum rate joint to compute gripper gap rate
00627   double MR,dMR_dtheta,dtheta_dt,dMR_dt;
00628   double joint_rate;
00629   {
00630     // new gripper model has an actual physical slider joint in simulation
00631     // use the new slider joint for determining gipper position, so forward/backward are consistent
00632     double gap_size         = js[0]->position_/2.0; // js position should be normalized
00633     // compute inverse transform for the gap joint, returns MR and dMR_dtheta
00634     inverseGapStates(gap_size,MR,dMR_dtheta,dtheta_dt,dMR_dt);
00635     double gap_rate         = js[0]->velocity_/2.0;
00636     joint_rate              = gap_rate * dtheta_dt;
00637   }
00638   double gap_effort         = js[0]->commanded_effort_;
00639 
00640   //ROS_ERROR("prop pos back eff=%f",gap_effort);
00641 
00643   as[0]->state_.position_             = MR                        * gap_mechanical_reduction_ / RAD2MR ;
00644 
00647   as[0]->state_.velocity_             = joint_rate   * dMR_dtheta * gap_mechanical_reduction_ / RAD2MR ;
00648 
00652   as[0]->state_.last_measured_effort_ = 2.0*gap_effort / dMR_dt * RAD2MR * gap_mechanical_reduction_;
00653 
00654   // Update the timing (making sure it's initialized).
00655   if (!simulated_actuator_timestamp_initialized_)
00656   {
00657     // Set the time stamp to zero (it is measured relative to the start time).
00658     as[0]->state_.sample_timestamp_ = ros::Duration(0);
00659 
00660     // Try to set the start time.  Only then do we claim initialized.
00661     if (ros::isStarted())
00662     {
00663       simulated_actuator_start_time_ = ros::Time::now();
00664       simulated_actuator_timestamp_initialized_ = true;
00665     }
00666   }
00667   else
00668   {
00669     // Measure the time stamp relative to the start time.
00670     as[0]->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
00671   }
00672   // Set the historical (double) timestamp accordingly.
00673   as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.toSec();
00674 
00675   // simulate calibration sensors by filling out actuator states
00676   this->joint_calibration_simulator_.simulateJointCalibration(js[0],as[0]);
00677 }
00678 
00679 void PR2GripperTransmission::propagateEffort(
00680   std::vector<JointState*>& js, std::vector<Actuator*>& as)
00681 {
00682   ROS_ASSERT(as.size() == 1);
00683 #if PASSIVE_JOINTS
00684   if (has_simulated_passive_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);}
00685   else {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 1);}
00686 #endif
00687 
00688   //
00689   // in hardware, the position of passive joints are set by propagatePosition, so they should be identical and
00690   // the inverse transform should be consistent.
00691   // note for simulation:
00692   //   new gripper model has an actual physical slider joint in simulation
00693   //   use the new slider joint for determining gipper position, so forward/backward are consistent
00694   double gap_size         = js[0]->position_/2.0; // js position should be normalized
00695 
00696   // now do the reverse transform
00697   double MR,dMR_dtheta,dtheta_dt,dMR_dt;
00698   // compute inverse transform for the gap joint, returns MR and dMR_dtheta
00699   inverseGapStates(gap_size,MR,dMR_dtheta,dtheta_dt,dMR_dt);
00700 
00701   double gap_effort       = js[0]->commanded_effort_; 
00702 
00703   //ROS_ERROR("prop eff eff=%f",gap_effort);
00704 
00706   as[0]->command_.enable_ = true;
00707   as[0]->command_.effort_ = 2.0*gap_effort / dMR_dt * RAD2MR * gap_mechanical_reduction_;
00708 }
00709 
00710 void PR2GripperTransmission::propagateEffortBackwards(
00711   std::vector<Actuator*>& as, std::vector<JointState*>& js)
00712 {
00713   ROS_ASSERT(as.size() == 1);
00714 #if PASSIVE_JOINTS
00715   ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);
00716 #endif
00717   ROS_ASSERT(simulated_reduction_>0.0);
00718 
00719   //
00720   // below transforms from encoder value to gap size, based on 090224_link_data.xls provided by Functions Engineering
00721   //
00723   double MR        = as[0]->state_.position_ / gap_mechanical_reduction_ * RAD2MR ;
00724   double MR_dot    = as[0]->state_.velocity_ / gap_mechanical_reduction_ * RAD2MR ;
00725   double MT        = as[0]->command_.effort_ / gap_mechanical_reduction_;
00726 
00728   double theta, dtheta_dMR, dt_dtheta, dt_dMR;
00730   double gap_size,gap_velocity,gap_effort;
00731 
00732   // compute gap position, velocity, measured_effort from actuator states
00733   computeGapStates(MR,MR_dot,MT,theta,dtheta_dMR, dt_dtheta, dt_dMR,gap_size,gap_velocity,gap_effort);
00734 
00735 #if PASSIVE_JOINTS
00736   {
00737     // propagate fictitious joint effort backwards
00738     // ROS_ERROR("prop eff back eff=%f",js[0]->commanded_effort_);
00739 
00740     // set screw joint effort if simulated
00741     js[passive_joints_.size()+1]->commanded_effort_  = gap_effort/simulated_reduction_;
00742   }
00743 #endif
00744 }
00745 


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Thu Jun 6 2019 21:11:31