wrist_transmission.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 /*
00035  * Author: Melonee Wise
00036  */
00037 #include <math.h>
00038 #include <pluginlib/class_list_macros.h>
00039 #include "pr2_mechanism_model/robot.h"
00040 #include "pr2_mechanism_model/wrist_transmission.h"
00041 
00042 using namespace pr2_mechanism_model;
00043 using namespace pr2_hardware_interface;
00044 
00045 PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::WristTransmission,
00046                         pr2_mechanism_model::Transmission)
00047 
00048 
00049 static bool convertDouble(const char* val_str, double &value)
00050 {
00051   char *endptr=NULL;
00052   value = strtod(val_str, &endptr);
00053   if ((endptr == val_str) || (endptr < (val_str+strlen(val_str))))
00054   {
00055     return false;
00056   }
00057 
00058   return true;
00059 }
00060 
00061 WristTransmission::WristTransmission()
00062 {
00063   joint_offset_[0] = 0.0;
00064   joint_offset_[1] = 0.0;
00065 }
00066 
00067 
00068 bool WristTransmission::initXml(TiXmlElement *elt, Robot *robot)
00069 {
00070   const char *name = elt->Attribute("name");
00071   name_ = name ? name : "";
00072 
00073 
00074   TiXmlElement *ael = elt->FirstChildElement("rightActuator");
00075   const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00076   Actuator *a;
00077   if (!actuator_name || (a = robot->getActuator(actuator_name)) == NULL )
00078   {
00079     ROS_WARN("WristTransmission could not find actuator named \"%s\"", actuator_name);
00080     return false;
00081   }
00082   a->command_.enable_ = true;
00083   actuator_names_.push_back(actuator_name);
00084   const char *act_red = ael->Attribute("mechanicalReduction");
00085   if (!act_red)
00086     {
00087       ROS_WARN("WristTransmission's actuator \"%s\" was not given a reduction.", actuator_name);
00088       return false;
00089     }
00090   actuator_reduction_.push_back(atof(act_red));
00091 
00092   ael = elt->FirstChildElement("leftActuator");
00093   actuator_name = ael ? ael->Attribute("name") : NULL;
00094   if (!actuator_name || (a = robot->getActuator(actuator_name)) == NULL )
00095   {
00096     ROS_WARN("WristTransmission could not find actuator named \"%s\"", actuator_name);
00097     return false;
00098   }
00099   a->command_.enable_ = true;
00100   actuator_names_.push_back(actuator_name);
00101   act_red = ael->Attribute("mechanicalReduction");
00102   if (!act_red)
00103     {
00104       ROS_WARN("WristTransmission's actuator \"%s\" was not given a reduction.", actuator_name);
00105       return false;
00106     }
00107   actuator_reduction_.push_back(atof(act_red));
00108 
00109 
00110   TiXmlElement *j = elt->FirstChildElement("flexJoint");
00111   const char *joint_name = j->Attribute("name");
00112   if (!joint_name)
00113   {
00114     ROS_ERROR("WristTransmission did not specify joint name");
00115     return false;
00116   }
00117   const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
00118 
00119   if (!joint)
00120   {
00121     ROS_ERROR("WristTransmission could not find joint named \"%s\"", joint_name);
00122     return false;
00123   }
00124   joint_names_.push_back(joint_name);
00125   const char *joint_red = j->Attribute("mechanicalReduction");
00126   if (!joint_red)
00127   {
00128     ROS_WARN("WristTransmission's joint \"%s\" was not given a reduction.", joint_name);
00129     return false;
00130   }
00131   joint_reduction_.push_back(atof(joint_red));
00132   const char *joint_offset = j->Attribute("offset");
00133   if (!joint_offset)
00134   {
00135     joint_offset_[0] = 0.0;
00136   }
00137   else
00138   {
00139     if (!convertDouble(joint_offset, joint_offset_[0]))
00140     {
00141       ROS_WARN("WristTransmission's joint \"%s\", cannot convert jointOffset attribute \"%s\" to floating point.",
00142                joint_name, joint_offset);
00143       return false;
00144     }
00145     else
00146     {
00147       ROS_WARN("Joint offset of %f for joint %s.", joint_offset_[0], joint_name);
00148     }
00149   }
00150 
00151   j = elt->FirstChildElement("rollJoint");
00152   joint_name = j->Attribute("name");
00153   if (!joint_name)
00154   {
00155     ROS_ERROR("WristTransmission did not specify joint name");
00156     return false;
00157   }
00158   const boost::shared_ptr<const urdf::Joint> joint2 = robot->robot_model_.getJoint(joint_name);
00159 
00160   if (!joint2)
00161   {
00162     ROS_ERROR("WristTransmission could not find joint named \"%s\"", joint_name);
00163     return false;
00164   }
00165   joint_names_.push_back(joint_name);
00166   joint_red = j->Attribute("mechanicalReduction");
00167   if (!joint_red)
00168   {
00169     ROS_WARN("WristTransmission's joint \"%s\" was not given a reduction.", joint_name);
00170     return false;
00171   }
00172   joint_reduction_.push_back(atof(joint_red));
00173   joint_offset = j->Attribute("offset");
00174   if (!joint_offset)
00175   {
00176     joint_offset_[1] = 0.0;
00177   }
00178   else
00179   {
00180     if (!convertDouble(joint_offset, joint_offset_[1]))
00181     {
00182       ROS_WARN("WristTransmission's joint \"%s\", cannot convert jointOffset attribute \"%s\" to floating point.",
00183                joint_name, joint_offset);
00184       return false;
00185     }
00186     else
00187     {
00188       ROS_WARN("Joint offset of %f for joint %s.", joint_offset_[1], joint_name); 
00189     }
00190   }
00191 
00192 
00193   return true;
00194 }
00195 
00196 bool WristTransmission::initXml(TiXmlElement *elt)
00197 {
00198   const char *name = elt->Attribute("name");
00199   name_ = name ? name : "";
00200 
00201 
00202   TiXmlElement *ael = elt->FirstChildElement("rightActuator");
00203   const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00204   if (!actuator_name)
00205   {
00206     ROS_WARN("WristTransmission could not find actuator named \"%s\"", actuator_name);
00207     return false;
00208   }
00209   actuator_names_.push_back(actuator_name);
00210   const char *act_red = ael->Attribute("mechanicalReduction");
00211   if (!act_red)
00212   {
00213     ROS_WARN("WristTransmission's actuator \"%s\" was not given a reduction.", actuator_name);
00214     return false;
00215   }
00216   actuator_reduction_.push_back(atof(act_red));
00217 
00218   ael = elt->FirstChildElement("leftActuator");
00219   actuator_name = ael ? ael->Attribute("name") : NULL;
00220   if (!actuator_name)
00221   {
00222     ROS_WARN("WristTransmission could not find actuator named \"%s\"", actuator_name);
00223     return false;
00224   }
00225   actuator_names_.push_back(actuator_name);
00226   act_red = ael->Attribute("mechanicalReduction");
00227   if (!act_red)
00228   {
00229     ROS_WARN("WristTransmission's actuator \"%s\" was not given a reduction.", actuator_name);
00230     return false;
00231   }
00232   actuator_reduction_.push_back(atof(act_red));
00233 
00234 
00235   TiXmlElement *j = elt->FirstChildElement("flexJoint");
00236   const char *joint_name = j->Attribute("name");
00237   if (!joint_name)
00238   {
00239     ROS_ERROR("WristTransmission did not specify joint name");
00240     return false;
00241   }
00242   joint_names_.push_back(joint_name);
00243   const char *joint_red = j->Attribute("mechanicalReduction");
00244   if (!joint_red)
00245   {
00246     ROS_WARN("WristTransmission's joint \"%s\" was not given a reduction.", joint_name);
00247     return false;
00248   }
00249   joint_reduction_.push_back(atof(joint_red));
00250   const char *joint_offset = j->Attribute("offset");
00251   if (!joint_offset)
00252   {
00253     joint_offset_[0] = 0.0;
00254   }
00255   else
00256   {
00257     double offset;
00258     if (!convertDouble(joint_offset, joint_offset_[0]))
00259     {
00260       ROS_WARN("WristTransmission's joint \"%s\", cannot convert jointOffset attribute \"%s\" to floating point.",
00261                joint_name, joint_offset);
00262       return false;
00263     }
00264     else
00265     {
00266       ROS_WARN("Joint offset of %f for joint %s.", joint_offset_[0], joint_name);
00267     }
00268   }
00269 
00270   j = elt->FirstChildElement("rollJoint");
00271   joint_name = j->Attribute("name");
00272   if (!joint_name)
00273   {
00274     ROS_ERROR("WristTransmission did not specify joint name");
00275     return false;
00276   }
00277   joint_names_.push_back(joint_name);
00278   joint_red = j->Attribute("mechanicalReduction");
00279   if (!joint_red)
00280   {
00281     ROS_WARN("WristTransmission's joint \"%s\" was not given a reduction.", joint_name);
00282     return false;
00283   }
00284   joint_reduction_.push_back(atof(joint_red));
00285   if (!joint_offset)
00286   {
00287     joint_offset_[1] = 0.0;
00288   }
00289   else
00290   {
00291     if (!convertDouble(joint_offset, joint_offset_[1]))
00292     {
00293       ROS_WARN("WristTransmission's joint \"%s\", cannot convert jointOffset attribute \"%s\" to floating point.",
00294                joint_name, joint_offset);
00295       return false;
00296     }
00297     else
00298     {
00299       ROS_WARN("Joint offset of %f for joint %s.", joint_offset_[1], joint_name);
00300     }
00301   }
00302 
00303   return true;
00304 }
00305 
00306 void WristTransmission::propagatePosition(
00307   std::vector<Actuator*>& as, std::vector<JointState*>& js)
00308 {
00309   assert(as.size() == 2);
00310   assert(js.size() == 2);
00311 
00312   js[0]->position_ = ((as[0]->state_.position_ / actuator_reduction_[0] - as[1]->state_.position_ / actuator_reduction_[1])/
00313                       (2*joint_reduction_[0])) + js[0]->reference_position_+joint_offset_[0];
00314   js[0]->velocity_ = (as[0]->state_.velocity_ / actuator_reduction_[0] - as[1]->state_.velocity_ / actuator_reduction_[1])/(2*joint_reduction_[0]);
00315   js[0]->measured_effort_ = joint_reduction_[0]*(as[0]->state_.last_measured_effort_ * actuator_reduction_[0] - as[1]->state_.last_measured_effort_ * actuator_reduction_[1]);
00316 
00317   js[1]->position_ = ((-as[0]->state_.position_ / actuator_reduction_[0] - as[1]->state_.position_ / actuator_reduction_[1])/
00318                       (2*joint_reduction_[1]))+js[1]->reference_position_+joint_offset_[1];
00319   js[1]->velocity_ = (-as[0]->state_.velocity_ / actuator_reduction_[0] - as[1]->state_.velocity_ / actuator_reduction_[1])/(2*joint_reduction_[1]);
00320   js[1]->measured_effort_ = joint_reduction_[1]*(-as[0]->state_.last_measured_effort_ * actuator_reduction_[0] - as[1]->state_.last_measured_effort_ * actuator_reduction_[1]);
00321 }
00322 
00323 void WristTransmission::propagatePositionBackwards(
00324   std::vector<JointState*>& js, std::vector<Actuator*>& as)
00325 {
00326   assert(as.size() == 2);
00327   assert(js.size() == 2);
00328 
00329   as[0]->state_.position_ = (((js[0]->position_-js[0]->reference_position_-joint_offset_[0])*joint_reduction_[0] -
00330                               (js[1]->position_-js[1]->reference_position_-joint_offset_[1])*joint_reduction_[1]) * actuator_reduction_[0]);
00331   as[0]->state_.velocity_ = ((js[0]->velocity_*joint_reduction_[0] - js[1]->velocity_*joint_reduction_[1]) * actuator_reduction_[0]);
00332   as[0]->state_.last_measured_effort_ = (js[0]->measured_effort_/joint_reduction_[0] - js[1]->measured_effort_/joint_reduction_[1]) /(2.0*actuator_reduction_[0]);
00333 
00334   as[1]->state_.position_ = ((-(js[0]->position_-js[0]->reference_position_-joint_offset_[0])*joint_reduction_[0] -
00335                                (js[1]->position_-js[1]->reference_position_-joint_offset_[1])*joint_reduction_[1]) * actuator_reduction_[1]);
00336   as[1]->state_.velocity_ = ((-js[0]->velocity_*joint_reduction_[0] - js[1]->velocity_*joint_reduction_[1]) * actuator_reduction_[1]);
00337   as[1]->state_.last_measured_effort_ = (-js[0]->measured_effort_/joint_reduction_[0] - js[1]->measured_effort_/joint_reduction_[1]) /(2.0*actuator_reduction_[1]);
00338 
00339   // Update the timing (making sure it's initialized).
00340   if (! simulated_actuator_timestamp_initialized_)
00341     {
00342       // Set the time stamp to zero (it is measured relative to the start time).
00343       as[0]->state_.sample_timestamp_ = ros::Duration(0);
00344       as[1]->state_.sample_timestamp_ = ros::Duration(0);
00345 
00346       // Try to set the start time.  Only then do we claim initialized.
00347       if (ros::isStarted())
00348         {
00349           simulated_actuator_start_time_ = ros::Time::now();
00350           simulated_actuator_timestamp_initialized_ = true;
00351         }
00352     }
00353   else
00354     {
00355       // Measure the time stamp relative to the start time.
00356       as[0]->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
00357       as[1]->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
00358     }
00359   // Set the historical (double) timestamp accordingly.
00360   as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.toSec();
00361   as[1]->state_.timestamp_ = as[1]->state_.sample_timestamp_.toSec();
00362 
00363   // simulate calibration sensors by filling out actuator states
00364   // this is where to embed the hack which joint connects to which mcb
00365   this->joint_calibration_simulator_[0].simulateJointCalibration(js[0],as[1]);
00366   this->joint_calibration_simulator_[1].simulateJointCalibration(js[1],as[0]);
00367 }
00368 
00369 void WristTransmission::propagateEffort(
00370   std::vector<JointState*>& js, std::vector<Actuator*>& as)
00371 {
00372   assert(as.size() == 2);
00373   assert(js.size() == 2);
00374 
00375   as[0]->command_.enable_ = true;
00376   as[1]->command_.enable_ = true;
00377 
00378   as[0]->command_.effort_ = (js[0]->commanded_effort_/joint_reduction_[0] - js[1]->commanded_effort_/joint_reduction_[1]) /(2.0*actuator_reduction_[0]);
00379   as[1]->command_.effort_ = (-js[0]->commanded_effort_/joint_reduction_[0] - js[1]->commanded_effort_/joint_reduction_[1]) /(2.0*actuator_reduction_[1]);
00380 }
00381 
00382 void WristTransmission::propagateEffortBackwards(
00383   std::vector<Actuator*>& as, std::vector<JointState*>& js)
00384 {
00385   assert(as.size() == 2);
00386   assert(js.size() == 2);
00387 
00388   js[0]->commanded_effort_ = joint_reduction_[0]*(as[0]->command_.effort_ * actuator_reduction_[0] - as[1]->command_.effort_ * actuator_reduction_[1]);
00389   js[1]->commanded_effort_ = joint_reduction_[1]*(-as[0]->command_.effort_ * actuator_reduction_[0] - as[1]->command_.effort_ * actuator_reduction_[1]);
00390 }
00391 
00392 
00393 void WristTransmission::setReductions(std::vector<double>& ar, std::vector<double>& jr)
00394 {
00395   actuator_reduction_ = ar;
00396   joint_reduction_ = jr;
00397 }
00398 


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