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/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
00340 if (! simulated_actuator_timestamp_initialized_)
00341 {
00342
00343 as[0]->state_.sample_timestamp_ = ros::Duration(0);
00344 as[1]->state_.sample_timestamp_ = ros::Duration(0);
00345
00346
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
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
00360 as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.toSec();
00361 as[1]->state_.timestamp_ = as[1]->state_.sample_timestamp_.toSec();
00362
00363
00364
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