Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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);
00104
00105
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
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);
00165
00166
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
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;
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
00219 if (! simulated_actuator_timestamp_initialized_)
00220 {
00221
00222 as[0]->state_.sample_timestamp_ = ros::Duration(0);
00223
00224
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
00234 as[0]->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
00235 }
00236
00237 as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.toSec();
00238
00239
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
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