simple_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: Stuart Glaser
00036  */
00037 #include <math.h>
00038 #include <pluginlib/class_list_macros.h>
00039 #include "pr2_mechanism_model/robot.h"
00040 #include "pr2_mechanism_model/simple_transmission.h"
00041 
00042 using namespace pr2_mechanism_model;
00043 using namespace pr2_hardware_interface;
00044 
00045 PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::SimpleTransmission,
00046                          pr2_mechanism_model::Transmission)
00047 
00048 
00049 bool SimpleTransmission::initXml(TiXmlElement *elt, Robot *robot)
00050 {
00051   const char *name = elt->Attribute("name");
00052   name_ = name ? name : "";
00053 
00054   TiXmlElement *jel = elt->FirstChildElement("joint");
00055   const char *joint_name = jel ? jel->Attribute("name") : NULL;
00056   if (!joint_name)
00057   {
00058     ROS_ERROR("SimpleTransmission did not specify joint name");
00059     return false;
00060   }
00061 
00062   const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
00063   if (!joint)
00064   {
00065     ROS_ERROR("SimpleTransmission could not find joint named \"%s\"", joint_name);
00066     return false;
00067   }
00068   joint_names_.push_back(joint_name);
00069 
00070   TiXmlElement *ael = elt->FirstChildElement("actuator");
00071   const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00072   Actuator *a;
00073   if (!actuator_name || (a = robot->getActuator(actuator_name)) == NULL )
00074   {
00075     ROS_ERROR("SimpleTransmission could not find actuator named \"%s\"", actuator_name);
00076     return false;
00077   }
00078   a->command_.enable_ = true;
00079   actuator_names_.push_back(actuator_name);
00080 
00081   mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
00082 
00083   // Get screw joint informations
00084   for (TiXmlElement *j = elt->FirstChildElement("simulated_actuated_joint"); j; j = j->NextSiblingElement("simulated_actuated_joint"))
00085   {
00086     const char *joint_name = j->Attribute("name");
00087     if (!joint_name)
00088     {
00089       ROS_ERROR("SimpleTransmission did not specify screw joint name");
00090       use_simulated_actuated_joint_=false;
00091     }
00092     else
00093     {
00094       const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
00095       if (!joint)
00096       {
00097         ROS_ERROR("SimpleTransmission could not find screw joint named \"%s\"", joint_name);
00098         use_simulated_actuated_joint_=false;
00099       }
00100       else
00101       {
00102         use_simulated_actuated_joint_=true;
00103         joint_names_.push_back(joint_name);  // The first joint is the gap joint
00104 
00105         // get the thread pitch
00106         const char *simulated_reduction = j->Attribute("simulated_reduction");
00107         if (!simulated_reduction)
00108         {
00109           ROS_ERROR("SimpleTransmission's joint \"%s\" has no coefficient: simulated_reduction.", joint_name);
00110           return false;
00111         }
00112         try
00113         {
00114           simulated_reduction_ = boost::lexical_cast<double>(simulated_reduction);
00115         }
00116         catch (boost::bad_lexical_cast &e)
00117         {
00118           ROS_ERROR("simulated_reduction (%s) is not a float",simulated_reduction);
00119           return false;
00120         }
00121       }
00122     }
00123   }
00124   return true;
00125 }
00126 
00127 bool SimpleTransmission::initXml(TiXmlElement *elt)
00128 {
00129   const char *name = elt->Attribute("name");
00130   name_ = name ? name : "";
00131 
00132   TiXmlElement *jel = elt->FirstChildElement("joint");
00133   const char *joint_name = jel ? jel->Attribute("name") : NULL;
00134   if (!joint_name)
00135   {
00136     ROS_ERROR("SimpleTransmission did not specify joint name");
00137     return false;
00138   }
00139   joint_names_.push_back(joint_name);
00140 
00141   TiXmlElement *ael = elt->FirstChildElement("actuator");
00142   const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00143   if (!actuator_name)
00144   {
00145     ROS_ERROR("SimpleTransmission could not find actuator named \"%s\"", actuator_name);
00146     return false;
00147   }
00148   actuator_names_.push_back(actuator_name);
00149 
00150   mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
00151 
00152   // Get screw joint informations
00153   for (TiXmlElement *j = elt->FirstChildElement("simulated_actuated_joint"); j; j = j->NextSiblingElement("simulated_actuated_joint"))
00154   {
00155     const char *joint_name = j->Attribute("name");
00156     if (!joint_name)
00157     {
00158       ROS_ERROR("SimpleTransmission screw joint did not specify joint name");
00159       use_simulated_actuated_joint_=false;
00160     }
00161     else
00162     {
00163       use_simulated_actuated_joint_=true;
00164       joint_names_.push_back(joint_name);  // The first joint is the gap joint
00165 
00166       // get the thread pitch
00167       const char *simulated_reduction = j->Attribute("simulated_reduction");
00168       if (!simulated_reduction)
00169       {
00170         ROS_ERROR("SimpleTransmission's joint \"%s\" has no coefficient: simulated_reduction.", joint_name);
00171         return false;
00172       }
00173       try
00174       {
00175         simulated_reduction_ = boost::lexical_cast<double>(simulated_reduction);
00176       }
00177       catch (boost::bad_lexical_cast &e)
00178       {
00179         ROS_ERROR("simulated_reduction (%s) is not a float",simulated_reduction);
00180         return false;
00181       }
00182     }
00183   }
00184   return true;
00185 }
00186 
00187 void SimpleTransmission::propagatePosition(
00188   std::vector<Actuator*>& as, std::vector<JointState*>& js)
00189 {
00190   assert(as.size() == 1);
00191   if (use_simulated_actuated_joint_) {assert(js.size() == 2);}
00192   else                              {assert(js.size() == 1);}
00193   js[0]->position_ = (as[0]->state_.position_ / mechanical_reduction_) + js[0]->reference_position_;
00194   js[0]->velocity_ = as[0]->state_.velocity_ / mechanical_reduction_;
00195   js[0]->measured_effort_ = as[0]->state_.last_measured_effort_ * mechanical_reduction_;
00196 
00197   if (use_simulated_actuated_joint_)
00198   {
00199     // screw joint state is not important to us, fill with zeros
00200     js[1]->position_ = 0;
00201     js[1]->velocity_ = 0;
00202     js[1]->measured_effort_ = 0;
00203     js[1]->reference_position_ = 0;
00204     js[1]->calibrated_ = true; // treat passive simulation joints as "calibrated"
00205   }
00206 }
00207 
00208 void SimpleTransmission::propagatePositionBackwards(
00209   std::vector<JointState*>& js, std::vector<Actuator*>& as)
00210 {
00211   assert(as.size() == 1);
00212   if (use_simulated_actuated_joint_) {assert(js.size() == 2);}
00213   else                              {assert(js.size() == 1);}
00214   as[0]->state_.position_ = (js[0]->position_ - js[0]->reference_position_) * mechanical_reduction_;
00215   as[0]->state_.velocity_ = js[0]->velocity_ * mechanical_reduction_;
00216   as[0]->state_.last_measured_effort_ = js[0]->measured_effort_ / mechanical_reduction_;
00217 
00218   // Update the timing (making sure it's initialized).
00219   if (! simulated_actuator_timestamp_initialized_)
00220     {
00221       // Set the time stamp to zero (it is measured relative to the start time).
00222       as[0]->state_.sample_timestamp_ = ros::Duration(0);
00223 
00224       // Try to set the start time.  Only then do we claim initialized.
00225       if (ros::isStarted())
00226         {
00227           simulated_actuator_start_time_ = ros::Time::now();
00228           simulated_actuator_timestamp_initialized_ = true;
00229         }
00230     }
00231   else
00232     {
00233       // Measure the time stamp relative to the start time.
00234       as[0]->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
00235     }
00236   // Set the historical (double) timestamp accordingly.
00237   as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.toSec();
00238 
00239   // simulate calibration sensors by filling out actuator states
00240   this->joint_calibration_simulator_.simulateJointCalibration(js[0],as[0]);
00241 }
00242 
00243 void SimpleTransmission::propagateEffort(
00244   std::vector<JointState*>& js, std::vector<Actuator*>& as)
00245 {
00246   assert(as.size() == 1);
00247   if (use_simulated_actuated_joint_) {assert(js.size() == 2);}
00248   else                               {assert(js.size() == 1);}
00249   as[0]->command_.enable_ = true;
00250   as[0]->command_.effort_ = js[0]->commanded_effort_ / mechanical_reduction_;
00251 }
00252 
00253 void SimpleTransmission::propagateEffortBackwards(
00254   std::vector<Actuator*>& as, std::vector<JointState*>& js)
00255 {
00256   assert(as.size() == 1);
00257   if (use_simulated_actuated_joint_) {assert(js.size() == 2);}
00258   else                               {assert(js.size() == 1);}
00259   if (use_simulated_actuated_joint_)
00260   {
00261     // set screw joint effort if simulated
00262     js[1]->commanded_effort_  = as[0]->command_.effort_ * mechanical_reduction_/simulated_reduction_;
00263   }
00264   else
00265     js[0]->commanded_effort_ = as[0]->command_.effort_ * mechanical_reduction_;
00266 }
00267 


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