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