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 #include "pr2_mechanism_model/pr2_gripper_transmission.h"
00053 #include <pluginlib/class_list_macros.h>
00054 #include <algorithm>
00055 #include <numeric>
00056 #include <angles/angles.h>
00057 #include <boost/lexical_cast.hpp>
00058 
00059 using namespace pr2_hardware_interface;
00060 using namespace pr2_mechanism_model;
00061 
00062 PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::PR2GripperTransmission,
00063                          pr2_mechanism_model::Transmission)
00064 
00065 bool PR2GripperTransmission::initXml(TiXmlElement *config, Robot *robot)
00066 {
00067   const char *name = config->Attribute("name");
00068   name_ = name ? name : "";
00069   //myfile.open("transmission_data.txt");
00070   TiXmlElement *ael = config->FirstChildElement("actuator");
00071   const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00072   if (!actuator_name || !robot->getActuator(actuator_name))
00073   {
00074     ROS_ERROR("PR2GripperTransmission could not find actuator named \"%s\"", actuator_name);
00075     return false;
00076   }
00077   robot->getActuator(actuator_name)->command_.enable_ = true;
00078   actuator_names_.push_back(actuator_name);
00079 
00080   for (TiXmlElement *j = config->FirstChildElement("gap_joint"); j; j = j->NextSiblingElement("gap_joint"))
00081   {
00082     const char *joint_name = j->Attribute("name");
00083     if (!joint_name)
00084     {
00085       ROS_ERROR("PR2GripperTransmission did not specify joint name");
00086       return false;
00087     }
00088 
00089     const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
00090     if (!joint)
00091     {
00092       ROS_ERROR("PR2GripperTransmission could not find joint named \"%s\"", joint_name);
00093       return false;
00094     }
00095     gap_joint_ = std::string(joint_name);
00096     joint_names_.push_back(joint_name);  // The first joint is the gap joint
00097 
00098     // get the mechanical reduction
00099     const char *joint_reduction = j->Attribute("mechanical_reduction");
00100     if (!joint_reduction)
00101     {
00102       ROS_ERROR("PR2GripperTransmission's joint \"%s\" has no coefficient: mechanical reduction.", joint_name);
00103       return false;
00104     }
00105     try
00106     {
00107       gap_mechanical_reduction_ = boost::lexical_cast<double>(joint_reduction);
00108     }
00109     catch (boost::bad_lexical_cast &e)
00110     {
00111       ROS_ERROR("joint_reduction (%s) is not a float",joint_reduction);
00112       return false;
00113     }
00114 
00115     // get the screw drive reduction
00116     const char *screw_reduction_str = j->Attribute("screw_reduction");
00117     if (screw_reduction_str == NULL)
00118     {
00119       screw_reduction_ = 2.0/1000.0;
00120       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: screw drive reduction, using default for PR2 alpha2.", joint_name);
00121     }
00122     else
00123       try
00124       {
00125         screw_reduction_ = boost::lexical_cast<double>(screw_reduction_str);
00126       }
00127       catch (boost::bad_lexical_cast &e)
00128       {
00129         ROS_ERROR("screw_reduction (%s) is not a float",screw_reduction_str);
00130         return false;
00131       }
00132 
00133 
00134     // get the gear_ratio
00135     const char *gear_ratio_str = j->Attribute("gear_ratio");
00136     if (gear_ratio_str == NULL)
00137     {
00138       gear_ratio_ = 29.16;
00139       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: gear_ratio, using default for PR2 alpha2.", joint_name);
00140     }
00141     else
00142       try
00143       {
00144         gear_ratio_ = boost::lexical_cast<double>(gear_ratio_str);
00145       }
00146       catch (boost::bad_lexical_cast &e)
00147       {
00148         ROS_ERROR("gear_ratio (%s) is not a float",gear_ratio_str);
00149         return false;
00150       }
00151 
00152     // get the theta0 coefficient
00153     const char *theta0_str = j->Attribute("theta0");
00154     if (theta0_str == NULL)
00155     {
00156       theta0_ = 2.97571*M_PI/180.0;
00157       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: theta0, using default for PR2 alpha2.", joint_name);
00158     }
00159     else
00160       try
00161       {
00162         theta0_ = boost::lexical_cast<double>(theta0_str);
00163       }
00164       catch (boost::bad_lexical_cast &e)
00165       {
00166         ROS_ERROR("theta0 (%s) is not a float",theta0_str);
00167         return false;
00168       }
00169     // get the phi0 coefficient
00170     const char *phi0_str = j->Attribute("phi0");
00171     if (phi0_str == NULL)
00172     {
00173       phi0_ = 29.98717*M_PI/180.0;
00174       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: phi0, using default for PR2 alpha2.", joint_name);
00175     }
00176     else
00177       try
00178       {
00179         phi0_ = boost::lexical_cast<double>(phi0_str);
00180       }
00181       catch (boost::bad_lexical_cast &e)
00182       {
00183         ROS_ERROR("phi0 (%s) is not a float",phi0_str);
00184         return false;
00185       }
00186     // get the t0 coefficient
00187     const char *t0_str = j->Attribute("t0");
00188     if (t0_str == NULL)
00189     {
00190       t0_ = -0.19543/1000.0;
00191       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: t0, using default for PR2 alpha2.", joint_name);
00192     }
00193     else
00194       try
00195       {
00196         t0_ = boost::lexical_cast<double>(t0_str);
00197       }
00198       catch (boost::bad_lexical_cast &e)
00199       {
00200         ROS_ERROR("t0 (%s) is not a float",t0_str);
00201         return false;
00202       }
00203     // get the L0 coefficient
00204     const char *L0_str = j->Attribute("L0");
00205     if (L0_str == NULL)
00206     {
00207       L0_ = 34.70821/1000.0;
00208       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: L0, using default for PR2 alpha2.", joint_name);
00209     }
00210     else
00211       try
00212       {
00213         L0_ = boost::lexical_cast<double>(L0_str);
00214       }
00215       catch (boost::bad_lexical_cast &e)
00216       {
00217         ROS_ERROR("L0 (%s) is not a float",L0_str);
00218         return false;
00219       }
00220     // get the h coefficient
00221     const char *h_str = j->Attribute("h");
00222     if (h_str == NULL)
00223     {
00224       h_ = 5.200/1000.0;
00225       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: h, using default for PR2 alpha2.", joint_name);
00226     }
00227     else
00228       try
00229       {
00230         h_ = boost::lexical_cast<double>(h_str);
00231       }
00232       catch (boost::bad_lexical_cast &e)
00233       {
00234         ROS_ERROR("h (%s) is not a float",h_str);
00235         return false;
00236       }
00237     // get the a coefficient
00238     const char *a_str = j->Attribute("a");
00239     if (a_str == NULL)
00240     {
00241       a_ = 67.56801/1000.0;
00242       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: a, using default for PR2 alpha2.", joint_name);
00243     }
00244     else
00245       try
00246       {
00247         a_ = boost::lexical_cast<double>(a_str);
00248       }
00249       catch (boost::bad_lexical_cast &e)
00250       {
00251         ROS_ERROR("a (%s) is not a float",a_str);
00252         return false;
00253       }
00254     // get the b coefficient
00255     const char *b_str = j->Attribute("b");
00256     if (b_str == NULL)
00257     {
00258       b_ = 48.97193/1000.0;
00259       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: b, using default for PR2 alpha2.", joint_name);
00260     }
00261     else
00262       try
00263       {
00264         b_ = boost::lexical_cast<double>(b_str);
00265       }
00266       catch (boost::bad_lexical_cast &e)
00267       {
00268         ROS_ERROR("b (%s) is not a float",b_str);
00269         return false;
00270       }
00271     // get the r coefficient
00272     const char *r_str = j->Attribute("r");
00273     if (r_str == NULL)
00274     {
00275       r_ = 91.50000/1000.0;
00276       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: r, using default for PR2 alpha2.", joint_name);
00277     }
00278     else
00279       try
00280       {
00281         r_ = boost::lexical_cast<double>(r_str);
00282       }
00283       catch (boost::bad_lexical_cast &e)
00284       {
00285         ROS_ERROR("r (%s) is not a float",r_str);
00286         return false;
00287       }
00288   }
00289 
00290   // Print all coefficients
00291   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",
00292             name_.c_str(), a_, b_, r_, h_, L0_, t0_, theta0_, phi0_, gear_ratio_, screw_reduction_);
00293 
00294   // Get passive joint informations
00295   for (TiXmlElement *j = config->FirstChildElement("passive_joint"); j; j = j->NextSiblingElement("passive_joint"))
00296   {
00297     const char *joint_name = j->Attribute("name");
00298     if (!joint_name)
00299     {
00300       ROS_ERROR("PR2GripperTransmission did not specify joint name");
00301       return false;
00302     }
00303     const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
00304 
00305     if (!joint)
00306     {
00307       ROS_ERROR("PR2GripperTransmission could not find joint named \"%s\"", joint_name);
00308       return false;
00309     }
00310 
00311     // add joint name to list
00312     joint_names_.push_back(joint_name);  // Adds the passive joints after the gap joint
00313     passive_joints_.push_back(joint_name);
00314   }
00315 
00316   // Get screw joint informations
00317   for (TiXmlElement *j = config->FirstChildElement("simulated_actuated_joint"); j; j = j->NextSiblingElement("simulated_actuated_joint"))
00318   {
00319     const char *joint_name = j->Attribute("name");
00320     if (!joint_name)
00321     {
00322       ROS_ERROR("PR2GripperTransmission simulated_actuated_joint did snot specify joint name");
00323       use_simulated_actuated_joint_=false;
00324     }
00325     else
00326     {
00327       const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
00328       if (!joint)
00329       {
00330         ROS_ERROR("PR2GripperTransmission could not find joint named \"%s\"", joint_name);
00331         use_simulated_actuated_joint_=false;
00332       }
00333       else
00334       {
00335         use_simulated_actuated_joint_=true;
00336         joint_names_.push_back(joint_name);  // The first joint is the gap joint
00337 
00338         // get the thread pitch
00339         const char *simulated_reduction = j->Attribute("simulated_reduction");
00340         if (!simulated_reduction)
00341         {
00342           ROS_ERROR("PR2GripperTransmission's joint \"%s\" has no coefficient: simulated_reduction.", joint_name);
00343           return false;
00344         }
00345         try
00346         {
00347           simulated_reduction_ = boost::lexical_cast<double>(simulated_reduction);
00348         }
00349         catch (boost::bad_lexical_cast &e)
00350         {
00351           ROS_ERROR("simulated_reduction (%s) is not a float",simulated_reduction);
00352           return false;
00353         }
00354 
00355         // get any additional joint introduced from this screw joint implementation
00356         // for the gripper, this is due to the limitation that screw constraint
00357         // requires axis of rotation to be aligned with line between CG's of the two
00358         // connected bodies.  For this reason, an additional slider joint was introduced
00359         // thus, requiring joint state to be published for motion planning packages
00360         // and that's why we're here.
00361         const char *passive_actuated_joint_name = j->Attribute("passive_actuated_joint");
00362         if (passive_actuated_joint_name)
00363         {
00364           const boost::shared_ptr<const urdf::Joint> passive_actuated_joint = robot->robot_model_.getJoint(passive_actuated_joint_name);
00365           if (passive_actuated_joint)
00366           {
00367             has_simulated_passive_actuated_joint_ = true;
00368             joint_names_.push_back(passive_actuated_joint_name);  // The first joint is the gap joint
00369           }
00370         }
00371 
00372       }
00373     }
00374   }
00375 
00376   // assuming simulated gripper prismatic joint exists, use it
00377 
00378   return true;
00379 }
00380 
00381 bool PR2GripperTransmission::initXml(TiXmlElement *config)
00382 {
00383   const char *name = config->Attribute("name");
00384   name_ = name ? name : "";
00385   //myfile.open("transmission_data.txt");
00386   TiXmlElement *ael = config->FirstChildElement("actuator");
00387   const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00388   if (!actuator_name)
00389   {
00390     ROS_ERROR("PR2GripperTransmission could not find actuator named \"%s\"", actuator_name);
00391     return false;
00392   }
00393   actuator_names_.push_back(actuator_name);
00394 
00395   for (TiXmlElement *j = config->FirstChildElement("gap_joint"); j; j = j->NextSiblingElement("gap_joint"))
00396   {
00397     const char *joint_name = j->Attribute("name");
00398     if (!joint_name)
00399     {
00400       ROS_ERROR("PR2GripperTransmission did not specify joint name");
00401       return false;
00402     }
00403     gap_joint_ = std::string(joint_name);
00404     joint_names_.push_back(joint_name);  // The first joint is the gap joint
00405 
00406     // get the mechanical reduction
00407     const char *joint_reduction = j->Attribute("mechanical_reduction");
00408     if (!joint_reduction)
00409     {
00410       ROS_ERROR("PR2GripperTransmission's joint \"%s\" has no coefficient: mechanical reduction.", joint_name);
00411       return false;
00412     }
00413     try
00414     {
00415       gap_mechanical_reduction_ = boost::lexical_cast<double>(joint_reduction);
00416     }
00417     catch (boost::bad_lexical_cast &e)
00418     {
00419       ROS_ERROR("joint_reduction (%s) is not a float",joint_reduction);
00420       return false;
00421     }
00422 
00423     // get the screw drive reduction
00424     const char *screw_reduction_str = j->Attribute("screw_reduction");
00425     if (screw_reduction_str == NULL)
00426     {
00427       screw_reduction_ = 2.0/1000.0;
00428       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: screw drive reduction, using default for PR2 alpha2.", joint_name);
00429     }
00430     else
00431       try
00432       {
00433         screw_reduction_ = boost::lexical_cast<double>(screw_reduction_str);
00434       }
00435       catch (boost::bad_lexical_cast &e)
00436       {
00437         ROS_ERROR("screw_reduction (%s) is not a float",screw_reduction_str);
00438         return false;
00439       }
00440     //ROS_INFO("screw drive reduction. %f", screw_reduction_);
00441 
00442     // get the gear_ratio
00443     const char *gear_ratio_str = j->Attribute("gear_ratio");
00444     if (gear_ratio_str == NULL)
00445     {
00446       gear_ratio_ = 29.16;
00447       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: gear_ratio, using default for PR2 alpha2.", joint_name);
00448     }
00449     else
00450       try
00451       {
00452         gear_ratio_ = boost::lexical_cast<double>(gear_ratio_str);
00453       }
00454       catch (boost::bad_lexical_cast &e)
00455       {
00456         ROS_ERROR("gear_ratio (%s) is not a float",gear_ratio_str);
00457         return false;
00458       }
00459     //ROS_INFO("gear_ratio. %f", gear_ratio_);
00460 
00461     // get the theta0 coefficient
00462     const char *theta0_str = j->Attribute("theta0");
00463     if (theta0_str == NULL)
00464     {
00465       theta0_ = 2.97571*M_PI/180.0;
00466       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: theta0, using default for PR2 alpha2.", joint_name);
00467     }
00468     else
00469       try
00470       {
00471         theta0_ = boost::lexical_cast<double>(theta0_str);
00472       }
00473       catch (boost::bad_lexical_cast &e)
00474       {
00475         ROS_ERROR("theta0 (%s) is not a float",theta0_str);
00476         return false;
00477       }
00478     // get the phi0 coefficient
00479     const char *phi0_str = j->Attribute("phi0");
00480     if (phi0_str == NULL)
00481     {
00482       phi0_ = 29.98717*M_PI/180.0;
00483       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: phi0, using default for PR2 alpha2.", joint_name);
00484     }
00485     else
00486       try
00487       {
00488         phi0_ = boost::lexical_cast<double>(phi0_str);
00489       }
00490       catch (boost::bad_lexical_cast &e)
00491       {
00492         ROS_ERROR("phi0 (%s) is not a float",phi0_str);
00493         return false;
00494       }
00495     // get the t0 coefficient
00496     const char *t0_str = j->Attribute("t0");
00497     if (t0_str == NULL)
00498     {
00499       t0_ = -0.19543/1000.0;
00500       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: t0, using default for PR2 alpha2.", joint_name);
00501     }
00502     else
00503       try
00504       {
00505         t0_ = boost::lexical_cast<double>(t0_str);
00506       }
00507       catch (boost::bad_lexical_cast &e)
00508       {
00509         ROS_ERROR("t0 (%s) is not a float",t0_str);
00510         return false;
00511       }
00512     // get the L0 coefficient
00513     const char *L0_str = j->Attribute("L0");
00514     if (L0_str == NULL)
00515     {
00516       L0_ = 34.70821/1000.0;
00517       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: L0, using default for PR2 alpha2.", joint_name);
00518     }
00519     else
00520       try
00521       {
00522         L0_ = boost::lexical_cast<double>(L0_str);
00523       }
00524       catch (boost::bad_lexical_cast &e)
00525       {
00526         ROS_ERROR("L0 (%s) is not a float",L0_str);
00527         return false;
00528       }
00529     // get the h coefficient
00530     const char *h_str = j->Attribute("h");
00531     if (h_str == NULL)
00532     {
00533       h_ = 5.200/1000.0;
00534       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: h, using default for PR2 alpha2.", joint_name);
00535     }
00536     else
00537       try
00538       {
00539         h_ = boost::lexical_cast<double>(h_str);
00540       }
00541       catch (boost::bad_lexical_cast &e)
00542       {
00543         ROS_ERROR("h (%s) is not a float",h_str);
00544         return false;
00545       }
00546     // get the a coefficient
00547     const char *a_str = j->Attribute("a");
00548     if (a_str == NULL)
00549     {
00550       a_ = 67.56801/1000.0;
00551       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: a, using default for PR2 alpha2.", joint_name);
00552     }
00553     else
00554       try
00555       {
00556         a_ = boost::lexical_cast<double>(a_str);
00557       }
00558       catch (boost::bad_lexical_cast &e)
00559       {
00560         ROS_ERROR("a (%s) is not a float",a_str);
00561         return false;
00562       }
00563     // get the b coefficient
00564     const char *b_str = j->Attribute("b");
00565     if (b_str == NULL)
00566     {
00567       b_ = 48.97193/1000.0;
00568       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: b, using default for PR2 alpha2.", joint_name);
00569     }
00570     else
00571       try
00572       {
00573         b_ = boost::lexical_cast<double>(b_str);
00574       }
00575       catch (boost::bad_lexical_cast &e)
00576       {
00577         ROS_ERROR("b (%s) is not a float",b_str);
00578         return false;
00579       }
00580     // get the r coefficient
00581     const char *r_str = j->Attribute("r");
00582     if (r_str == NULL)
00583     {
00584       r_ = 91.50000/1000.0;
00585       ROS_WARN("PR2GripperTransmission's joint \"%s\" has no coefficient: r, using default for PR2 alpha2.", joint_name);
00586     }
00587     else
00588       try
00589       {
00590         r_ = boost::lexical_cast<double>(r_str);
00591       }
00592       catch (boost::bad_lexical_cast &e)
00593       {
00594         ROS_ERROR("r (%s) is not a float",r_str);
00595         return false;
00596       }
00597   }
00598 
00599   // Print all coefficients
00600   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",
00601             name_.c_str(), a_, b_, r_, h_, L0_, t0_, theta0_, phi0_, gear_ratio_, screw_reduction_);
00602 
00603   // Get passive joint informations
00604   for (TiXmlElement *j = config->FirstChildElement("passive_joint"); j; j = j->NextSiblingElement("passive_joint"))
00605   {
00606     const char *joint_name = j->Attribute("name");
00607     if (!joint_name)
00608     {
00609       ROS_ERROR("PR2GripperTransmission did not specify joint name");
00610       return false;
00611     }
00612 
00613     // add joint name to list
00614     joint_names_.push_back(joint_name);  // Adds the passive joints after the gap joint
00615     passive_joints_.push_back(joint_name);
00616   }
00617 
00618   // Get screw joint informations
00619   for (TiXmlElement *j = config->FirstChildElement("simulated_actuated_joint"); j; j = j->NextSiblingElement("simulated_actuated_joint"))
00620   {
00621     const char *joint_name = j->Attribute("name");
00622     if (!joint_name)
00623     {
00624       ROS_ERROR("PR2GripperTransmission screw joint did not specify joint name");
00625       use_simulated_actuated_joint_=false;
00626     }
00627     else
00628     {
00629       use_simulated_actuated_joint_=true;
00630       joint_names_.push_back(joint_name);  // The first joint is the gap joint
00631 
00632       // get the thread pitch
00633       const char *simulated_reduction = j->Attribute("simulated_reduction");
00634       if (!simulated_reduction)
00635       {
00636         ROS_ERROR("PR2GripperTransmission's joint \"%s\" has no coefficient: simulated_reduction.", joint_name);
00637         return false;
00638       }
00639       try
00640       {
00641         simulated_reduction_ = boost::lexical_cast<double>(simulated_reduction);
00642       }
00643       catch (boost::bad_lexical_cast &e)
00644       {
00645         ROS_ERROR("simulated_reduction (%s) is not a float",simulated_reduction);
00646         return false;
00647       }
00648 
00649       // get any additional joint introduced from this screw joint implementation
00650       // for the gripper, this is due to the limitation that screw constraint
00651       // requires axis of rotation to be aligned with line between CG's of the two
00652       // connected bodies.  For this reason, an additional slider joint was introduced
00653       // thus, requiring joint state to be published for motion planning packages
00654       // and that's why we're here.
00655       const char *passive_actuated_joint_name = j->Attribute("passive_actuated_joint");
00656       if (passive_actuated_joint_name)
00657       {
00658         has_simulated_passive_actuated_joint_ = true;
00659         joint_names_.push_back(passive_actuated_joint_name);  // The first joint is the gap joint
00660       }
00661 
00662     }
00663   }
00664 
00665   // assuming simulated gripper prismatic joint exists, use it
00666 
00667   return true;
00668 }
00669 
00672 void PR2GripperTransmission::computeGapStates(
00673   double MR,double MR_dot,double MT,
00674   double &theta,double &dtheta_dMR,double &dt_dtheta,double &dt_dMR,double &gap_size,double &gap_velocity,double &gap_effort)
00675 {
00676   //
00677   // below transforms from encoder value to gap size, based on 090224_link_data.xls provided by Functions Engineering
00678   //
00679   double u            = (a_*a_+b_*b_-h_*h_
00680                          -pow(L0_+MR*screw_reduction_/gear_ratio_,2))/(2.0*a_*b_);
00681   u                   = u < -1.0 ? -1.0 : u > 1.0 ? 1.0 : u;
00682   theta               = theta0_ - phi0_ + acos(u);
00683   // limit theta
00684   //theta = theta > 0 ? theta : 0;
00685   // force theta to be greater than theta0_
00686   //theta = angles::normalize_angle_positive(angles::shortest_angular_distance(theta0_,theta))+theta0_;
00687 
00688   gap_size            = t0_ + r_ * ( sin(theta) - sin(theta0_) );
00689 
00690   //
00691   // compute jacobians based on transforms, get the velocity of the gripper gap size based on encoder velocity
00692   //
00693   // for jacobian, we want to limit MR >= 0
00694   MR = MR >= 0.0 ? MR : 0.0;
00695   // then recompute u and theta based on restricted MR
00696   u                   = (a_*a_+b_*b_-h_*h_
00697                          -pow(L0_+MR*screw_reduction_/gear_ratio_,2))/(2.0*a_*b_);
00698   u                   = u < -1.0 ? -1.0 : u > 1.0 ? 1.0 : u;
00699   double tmp_theta    = theta0_ - phi0_ + acos(u);
00700 
00701   //
00702   double arg          = 1.0 - pow(u,2);
00703   arg                 = arg > TOL ? arg : TOL; //LIMIT: CAP u at TOL artificially
00704 
00705   double du_dMR       = -(L0_ * screw_reduction_)/(gear_ratio_*a_*b_) // d(arg)/d(MR)
00706                         -MR/(a_*b_)*pow(screw_reduction_/gear_ratio_,2);
00707 
00708   dtheta_dMR          = -1.0/sqrt(arg) * du_dMR; // derivative of acos
00709 
00710   dt_dtheta           = r_ * cos( tmp_theta );
00711   dt_dMR              = dt_dtheta * dtheta_dMR;
00712   gap_velocity        = MR_dot * dt_dMR;
00713 
00714   //
00715   // get the effort at the gripper gap based on torque at the motor
00716   // gap effort = motor torque         * dmotor_theta/dt
00717   //            = MT                   * dmotor_theta_dt
00718   //            = MT                   * dMR_dt          / (2*pi)
00719   //            = MT                   / dt_dMR          * 2*pi
00720   //
00721   gap_effort          = MT      / dt_dMR / RAD2MR ;
00722   //ROS_WARN("debug: %f %f %f",gap_effort,MT,dt_dMR,RAD2MR);
00723 }
00724 
00729 void PR2GripperTransmission::inverseGapStates(
00730   double gap_size,double &MR,double &dMR_dtheta,double &dtheta_dt,double &dMR_dt)
00731 {
00732     // get theta for jacobian calculation
00733     double sin_theta        = (gap_size - t0_)/r_ + sin(theta0_);
00734     sin_theta = sin_theta > 1.0 ? 1.0 : sin_theta < -1.0 ? -1.0 : sin_theta;
00735     double theta            = asin(sin_theta);
00736 
00737     // compute inverse transform for the gap joint, returns MR and dMR_dtheta
00738     inverseGapStates1(theta,MR,dMR_dtheta,dtheta_dt,dMR_dt);
00739 }
00740 
00745 void PR2GripperTransmission::inverseGapStates1(
00746   double theta,double &MR,double &dMR_dtheta,double &dtheta_dt,double &dMR_dt)
00747 {
00748   // limit theta
00749   //theta = theta > 0 ? theta : 0;
00750   // force theta to be greater than theta0_
00751   //theta = angles::normalize_angle_positive(angles::shortest_angular_distance(theta0_,theta))+theta0_;
00752 
00753   // now do the reverse transform
00754   double arg         = -2.0*a_*b_*cos(theta-theta0_+phi0_)
00755                                    -h_*h_+a_*a_+b_*b_;
00756   if (arg > 0.0)
00757   {
00758     MR               = gear_ratio_/screw_reduction_ * ( sqrt(arg) - L0_ );
00759     dMR_dtheta       = gear_ratio_/(2.0 * screw_reduction_) / sqrt(arg)
00760                        * 2.0 * a_ * b_ * sin(theta + phi0_ - theta0_);
00761   }
00762   else
00763   {
00764     MR               = gear_ratio_/screw_reduction_ * ( 0.0       - L0_ );
00765     dMR_dtheta       = 0.0;
00766   }
00767 
00768   // compute gap_size from theta
00769   double gap_size = t0_ + r_ * ( sin(theta) - sin(theta0_) ); // in mm
00770 
00771   // compute inverse jacobians for the transform
00772   // for this, enforce dMR_dtheta >= 0
00773   // since there are two roots, take the positive root
00774   // @todo: this affects sim only, need to check this for sim.
00775   double tmp_dMR_dtheta = fabs(dMR_dtheta);
00776 
00777   double u           = (gap_size - t0_)/r_ + sin(theta0_);
00778   double arg2        = 1.0 - pow(u,2);
00779   arg2               = arg2 > TOL ? arg2 : TOL; //LIMIT: CAP arg2 at TOL artificially
00780   dtheta_dt          = 1.0 / sqrt( arg2 ) / r_;  // derivative of asin
00781   dMR_dt             = tmp_dMR_dtheta * dtheta_dt;  // remember, here, t is gap_size
00782 }
00783 
00784 
00788 void PR2GripperTransmission::propagatePosition(
00789   std::vector<Actuator*>& as, std::vector<JointState*>& js)
00790 {
00791 
00792   ROS_ASSERT(as.size() == 1);
00793   // js has passive joints and 1 gap joint and 1 screw joint
00794   if (use_simulated_actuated_joint_ && has_simulated_passive_actuated_joint_)
00795     {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);}
00796   else if (use_simulated_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 1);}
00797   else                  {ROS_ASSERT(js.size() == 1 + passive_joints_.size());}
00798 
00802   double MR        = as[0]->state_.position_ / gap_mechanical_reduction_ * RAD2MR ;
00803 
00805   double MR_dot    = as[0]->state_.velocity_ / gap_mechanical_reduction_ * RAD2MR ;
00806 
00816   //double MT        = as[0]->state_.last_measured_effort_ / gap_mechanical_reduction_ * RAD2MR ;
00817 
00818 
00820   double MT        = as[0]->state_.last_measured_effort_ / gap_mechanical_reduction_;
00821 
00823   double theta, dtheta_dMR, dt_dtheta, dt_dMR;
00825   double gap_size,gap_velocity,gap_effort;
00826 
00827   // compute gap position, velocity, measured_effort from actuator states
00828   computeGapStates(MR,MR_dot,MT,theta,dtheta_dMR, dt_dtheta, dt_dMR,gap_size,gap_velocity,gap_effort);
00829 
00830   // Determines the state of the gap joint.
00831   js[0]->position_        = gap_size*2.0; // function engineering's transmission give half the total gripper size
00832   js[0]->velocity_        = gap_velocity*2.0;
00833   js[0]->measured_effort_ = gap_effort/2.0;
00834   //ROS_ERROR("prop pos eff=%f",js[0]->measured_effort_);
00835 
00836   // Determines the states of the passive joints.
00837   // we need to do this for each finger, in simulation, each finger has it's state filled out
00838   for (size_t i = 1; i < passive_joints_.size()+1; ++i)
00839   {
00840     js[i]->position_           = theta - theta0_;
00841     js[i]->velocity_           = dtheta_dMR * MR_dot;
00842     js[i]->measured_effort_    = MT / dtheta_dMR / RAD2MR;
00843     js[i]->reference_position_ = MT / dtheta_dMR / RAD2MR;
00844   }
00845 
00846   if (use_simulated_actuated_joint_)
00847   {
00848     // screw joint state is not important to us, fill with zeros
00849     js[passive_joints_.size()+1]->position_           = 0.0;
00850     js[passive_joints_.size()+1]->velocity_           = 0.0;
00851     js[passive_joints_.size()+1]->measured_effort_    = 0.0;
00852     js[passive_joints_.size()+1]->reference_position_ = 0.0;
00853     js[passive_joints_.size()+1]->calibrated_         = true; // treat passive simulation joints as "calibrated"
00854   }
00855   if (has_simulated_passive_actuated_joint_)
00856   {
00857     // screw joint state is not important to us, fill with zeros
00858     js[passive_joints_.size()+2]->position_           = 0.0;
00859     js[passive_joints_.size()+2]->velocity_           = 0.0;
00860     js[passive_joints_.size()+2]->measured_effort_    = 0.0;
00861     js[passive_joints_.size()+2]->reference_position_ = 0.0;
00862     js[passive_joints_.size()+2]->calibrated_         = true; // treat passive simulation joints as "calibrated"
00863   }
00864 }
00865 
00866 // this is needed for simulation, so we can recover encoder value given joint angles
00867 void PR2GripperTransmission::propagatePositionBackwards(
00868   std::vector<JointState*>& js, std::vector<Actuator*>& as)
00869 {
00870   ROS_ASSERT(as.size() == 1);
00871   if (use_simulated_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 1);}
00872   else                  {ROS_ASSERT(js.size() == 1 + passive_joints_.size());}
00873 
00874   // keep the simulation stable by using the minimum rate joint to compute gripper gap rate
00875   double MR,dMR_dtheta,dtheta_dt,dMR_dt;
00876   double joint_rate;
00877   {
00878     // new gripper model has an actual physical slider joint in simulation
00879     // use the new slider joint for determining gipper position, so forward/backward are consistent
00880     double gap_size         = js[0]->position_/2.0; // js position should be normalized
00881     // compute inverse transform for the gap joint, returns MR and dMR_dtheta
00882     inverseGapStates(gap_size,MR,dMR_dtheta,dtheta_dt,dMR_dt);
00883     double gap_rate         = js[0]->velocity_/2.0;
00884     joint_rate              = gap_rate * dtheta_dt;
00885   }
00886   double gap_effort         = js[0]->commanded_effort_;
00887 
00888   //ROS_ERROR("prop pos back eff=%f",gap_effort);
00889 
00891   as[0]->state_.position_             = MR                        * gap_mechanical_reduction_ / RAD2MR ;
00892 
00895   as[0]->state_.velocity_             = joint_rate   * dMR_dtheta * gap_mechanical_reduction_ / RAD2MR ;
00896 
00900   as[0]->state_.last_measured_effort_ = 2.0*gap_effort / dMR_dt * RAD2MR * gap_mechanical_reduction_;
00901 
00902   // Update the timing (making sure it's initialized).
00903   if (! simulated_actuator_timestamp_initialized_)
00904     {
00905       // Set the time stamp to zero (it is measured relative to the start time).
00906       as[0]->state_.sample_timestamp_ = ros::Duration(0);
00907 
00908       // Try to set the start time.  Only then do we claim initialized.
00909       if (ros::isStarted())
00910       {
00911         simulated_actuator_start_time_ = ros::Time::now();
00912         simulated_actuator_timestamp_initialized_ = true;
00913       }
00914     }
00915   else
00916     {
00917       // Measure the time stamp relative to the start time.
00918       as[0]->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
00919     }
00920   // Set the historical (double) timestamp accordingly.
00921   as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.toSec();
00922 
00923   // simulate calibration sensors by filling out actuator states
00924   this->joint_calibration_simulator_.simulateJointCalibration(js[0],as[0]);
00925 }
00926 
00927 void PR2GripperTransmission::propagateEffort(
00928   std::vector<JointState*>& js, std::vector<Actuator*>& as)
00929 {
00930   ROS_ASSERT(as.size() == 1);
00931   if (use_simulated_actuated_joint_ && has_simulated_passive_actuated_joint_)
00932     {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 2);}
00933   else if (use_simulated_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 1);}
00934   else                  {ROS_ASSERT(js.size() == 1 + passive_joints_.size());}
00935 
00936   //
00937   // in hardware, the position of passive joints are set by propagatePosition, so they should be identical and
00938   // the inverse transform should be consistent.
00939   // note for simulation:
00940   //   new gripper model has an actual physical slider joint in simulation
00941   //   use the new slider joint for determining gipper position, so forward/backward are consistent
00942   double gap_size         = js[0]->position_/2.0; // js position should be normalized
00943 
00944   // now do the reverse transform
00945   double MR,dMR_dtheta,dtheta_dt,dMR_dt;
00946   // compute inverse transform for the gap joint, returns MR and dMR_dtheta
00947   inverseGapStates(gap_size,MR,dMR_dtheta,dtheta_dt,dMR_dt);
00948 
00949   double gap_effort       = js[0]->commanded_effort_; 
00950 
00951   //ROS_ERROR("prop eff eff=%f",gap_effort);
00952 
00954   as[0]->command_.enable_ = true;
00955   as[0]->command_.effort_ = 2.0*gap_effort / dMR_dt * RAD2MR * gap_mechanical_reduction_;
00956 }
00957 
00958 void PR2GripperTransmission::propagateEffortBackwards(
00959   std::vector<Actuator*>& as, std::vector<JointState*>& js)
00960 {
00961   ROS_ASSERT(as.size() == 1);
00962   if (use_simulated_actuated_joint_) {ROS_ASSERT(js.size() == 1 + passive_joints_.size() + 1);}
00963   else                  {ROS_ASSERT(js.size() == 1 + passive_joints_.size());}
00964   ROS_ASSERT(simulated_reduction_>0.0);
00965 
00966   //
00967   // below transforms from encoder value to gap size, based on 090224_link_data.xls provided by Functions Engineering
00968   //
00970   double MR        = as[0]->state_.position_ / gap_mechanical_reduction_ * RAD2MR ;
00971   double MR_dot    = as[0]->state_.velocity_ / gap_mechanical_reduction_ * RAD2MR ;
00972   double MT        = as[0]->command_.effort_ / gap_mechanical_reduction_;
00973 
00975   double theta, dtheta_dMR, dt_dtheta, dt_dMR;
00977   double gap_size,gap_velocity,gap_effort;
00978 
00979   // compute gap position, velocity, measured_effort from actuator states
00980   computeGapStates(MR,MR_dot,MT,theta,dtheta_dMR, dt_dtheta, dt_dMR,gap_size,gap_velocity,gap_effort);
00981 
00982   {
00983     // propagate fictitious joint effort backwards
00984     //ROS_ERROR("prop eff back eff=%f",js[0]->commanded_effort_);
00985     if (use_simulated_actuated_joint_)
00986     {
00987       // set screw joint effort if simulated
00988       js[passive_joints_.size()+1]->commanded_effort_  = gap_effort/simulated_reduction_;
00989       //js[0]->commanded_effort_                         = gap_effort/2.0;
00990     }
00991     else
00992     {
00993       // an ugly hack to lessen instability due to gripper gains
00994       double eps=0.01;
00995       js[0]->commanded_effort_  = (1.0-eps)*js[0]->commanded_effort_ + eps*gap_effort/2.0; // skip slider joint effort
00996     }
00997   }
00998 }
00999 


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Dec 2 2013 13:13:02