49 static
bool convertDouble(const
char* val_str,
double &
value)
52 value = strtod(val_str, &endptr);
53 if ((endptr == val_str) || (endptr < (val_str+strlen(val_str))))
63 joint_offset_[0] = 0.0;
64 joint_offset_[1] = 0.0;
70 const char *name = elt->Attribute(
"name");
71 name_ = name ? name :
"";
74 TiXmlElement *ael = elt->FirstChildElement(
"rightActuator");
75 const char *actuator_name = ael ? ael->Attribute(
"name") : NULL;
77 if (!actuator_name || (a = robot->
getActuator(actuator_name)) == NULL )
79 ROS_WARN(
"WristTransmission could not find actuator named \"%s\"", actuator_name);
83 actuator_names_.push_back(actuator_name);
84 const char *act_red = ael->Attribute(
"mechanicalReduction");
87 ROS_WARN(
"WristTransmission's actuator \"%s\" was not given a reduction.", actuator_name);
90 actuator_reduction_.push_back(atof(act_red));
92 ael = elt->FirstChildElement(
"leftActuator");
93 actuator_name = ael ? ael->Attribute(
"name") : NULL;
94 if (!actuator_name || (a = robot->
getActuator(actuator_name)) == NULL )
96 ROS_WARN(
"WristTransmission could not find actuator named \"%s\"", actuator_name);
100 actuator_names_.push_back(actuator_name);
101 act_red = ael->Attribute(
"mechanicalReduction");
104 ROS_WARN(
"WristTransmission's actuator \"%s\" was not given a reduction.", actuator_name);
107 actuator_reduction_.push_back(atof(act_red));
110 TiXmlElement *j = elt->FirstChildElement(
"flexJoint");
111 const char *joint_name = j->Attribute(
"name");
114 ROS_ERROR(
"WristTransmission did not specify joint name");
121 ROS_ERROR(
"WristTransmission could not find joint named \"%s\"", joint_name);
124 joint_names_.push_back(joint_name);
125 const char *joint_red = j->Attribute(
"mechanicalReduction");
128 ROS_WARN(
"WristTransmission's joint \"%s\" was not given a reduction.", joint_name);
131 joint_reduction_.push_back(atof(joint_red));
132 const char *joint_offset = j->Attribute(
"offset");
135 joint_offset_[0] = 0.0;
139 if (!convertDouble(joint_offset, joint_offset_[0]))
141 ROS_WARN(
"WristTransmission's joint \"%s\", cannot convert jointOffset attribute \"%s\" to floating point.",
142 joint_name, joint_offset);
147 ROS_WARN(
"Joint offset of %f for joint %s.", joint_offset_[0], joint_name);
151 j = elt->FirstChildElement(
"rollJoint");
152 joint_name = j->Attribute(
"name");
155 ROS_ERROR(
"WristTransmission did not specify joint name");
162 ROS_ERROR(
"WristTransmission could not find joint named \"%s\"", joint_name);
165 joint_names_.push_back(joint_name);
166 joint_red = j->Attribute(
"mechanicalReduction");
169 ROS_WARN(
"WristTransmission's joint \"%s\" was not given a reduction.", joint_name);
172 joint_reduction_.push_back(atof(joint_red));
173 joint_offset = j->Attribute(
"offset");
176 joint_offset_[1] = 0.0;
180 if (!convertDouble(joint_offset, joint_offset_[1]))
182 ROS_WARN(
"WristTransmission's joint \"%s\", cannot convert jointOffset attribute \"%s\" to floating point.",
183 joint_name, joint_offset);
188 ROS_WARN(
"Joint offset of %f for joint %s.", joint_offset_[1], joint_name);
198 const char *name = elt->Attribute(
"name");
199 name_ = name ? name :
"";
202 TiXmlElement *ael = elt->FirstChildElement(
"rightActuator");
203 const char *actuator_name = ael ? ael->Attribute(
"name") : NULL;
206 ROS_WARN(
"WristTransmission could not find actuator named \"%s\"", actuator_name);
209 actuator_names_.push_back(actuator_name);
210 const char *act_red = ael->Attribute(
"mechanicalReduction");
213 ROS_WARN(
"WristTransmission's actuator \"%s\" was not given a reduction.", actuator_name);
216 actuator_reduction_.push_back(atof(act_red));
218 ael = elt->FirstChildElement(
"leftActuator");
219 actuator_name = ael ? ael->Attribute(
"name") : NULL;
222 ROS_WARN(
"WristTransmission could not find actuator named \"%s\"", actuator_name);
225 actuator_names_.push_back(actuator_name);
226 act_red = ael->Attribute(
"mechanicalReduction");
229 ROS_WARN(
"WristTransmission's actuator \"%s\" was not given a reduction.", actuator_name);
232 actuator_reduction_.push_back(atof(act_red));
235 TiXmlElement *j = elt->FirstChildElement(
"flexJoint");
236 const char *joint_name = j->Attribute(
"name");
239 ROS_ERROR(
"WristTransmission did not specify joint name");
242 joint_names_.push_back(joint_name);
243 const char *joint_red = j->Attribute(
"mechanicalReduction");
246 ROS_WARN(
"WristTransmission's joint \"%s\" was not given a reduction.", joint_name);
249 joint_reduction_.push_back(atof(joint_red));
250 const char *joint_offset = j->Attribute(
"offset");
253 joint_offset_[0] = 0.0;
258 if (!convertDouble(joint_offset, joint_offset_[0]))
260 ROS_WARN(
"WristTransmission's joint \"%s\", cannot convert jointOffset attribute \"%s\" to floating point.",
261 joint_name, joint_offset);
266 ROS_WARN(
"Joint offset of %f for joint %s.", joint_offset_[0], joint_name);
270 j = elt->FirstChildElement(
"rollJoint");
271 joint_name = j->Attribute(
"name");
274 ROS_ERROR(
"WristTransmission did not specify joint name");
277 joint_names_.push_back(joint_name);
278 joint_red = j->Attribute(
"mechanicalReduction");
281 ROS_WARN(
"WristTransmission's joint \"%s\" was not given a reduction.", joint_name);
284 joint_reduction_.push_back(atof(joint_red));
287 joint_offset_[1] = 0.0;
291 if (!convertDouble(joint_offset, joint_offset_[1]))
293 ROS_WARN(
"WristTransmission's joint \"%s\", cannot convert jointOffset attribute \"%s\" to floating point.",
294 joint_name, joint_offset);
299 ROS_WARN(
"Joint offset of %f for joint %s.", joint_offset_[1], joint_name);
307 std::vector<Actuator*>& as, std::vector<JointState*>& js)
309 assert(as.size() == 2);
310 assert(js.size() == 2);
312 js[0]->position_ = ((as[0]->state_.position_ / actuator_reduction_[0] - as[1]->state_.position_ / actuator_reduction_[1])/
313 (2*joint_reduction_[0])) + js[0]->reference_position_+joint_offset_[0];
314 js[0]->velocity_ = (as[0]->state_.velocity_ / actuator_reduction_[0] - as[1]->state_.velocity_ / actuator_reduction_[1])/(2*joint_reduction_[0]);
315 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]);
317 js[1]->position_ = ((-as[0]->state_.position_ / actuator_reduction_[0] - as[1]->state_.position_ / actuator_reduction_[1])/
318 (2*joint_reduction_[1]))+js[1]->reference_position_+joint_offset_[1];
319 js[1]->velocity_ = (-as[0]->state_.velocity_ / actuator_reduction_[0] - as[1]->state_.velocity_ / actuator_reduction_[1])/(2*joint_reduction_[1]);
320 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]);
324 std::vector<JointState*>& js, std::vector<Actuator*>& as)
326 assert(as.size() == 2);
327 assert(js.size() == 2);
329 as[0]->state_.position_ = (((js[0]->position_-js[0]->reference_position_-joint_offset_[0])*joint_reduction_[0] -
330 (js[1]->position_-js[1]->reference_position_-joint_offset_[1])*joint_reduction_[1]) * actuator_reduction_[0]);
331 as[0]->state_.velocity_ = ((js[0]->velocity_*joint_reduction_[0] - js[1]->velocity_*joint_reduction_[1]) * actuator_reduction_[0]);
332 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]);
334 as[1]->state_.position_ = ((-(js[0]->position_-js[0]->reference_position_-joint_offset_[0])*joint_reduction_[0] -
335 (js[1]->position_-js[1]->reference_position_-joint_offset_[1])*joint_reduction_[1]) * actuator_reduction_[1]);
336 as[1]->state_.velocity_ = ((-js[0]->velocity_*joint_reduction_[0] - js[1]->velocity_*joint_reduction_[1]) * actuator_reduction_[1]);
337 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]);
340 if (! simulated_actuator_timestamp_initialized_)
350 simulated_actuator_timestamp_initialized_ =
true;
356 as[0]->state_.sample_timestamp_ =
ros::Time::now() - simulated_actuator_start_time_;
357 as[1]->state_.sample_timestamp_ =
ros::Time::now() - simulated_actuator_start_time_;
360 as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.
toSec();
361 as[1]->state_.timestamp_ = as[1]->state_.sample_timestamp_.toSec();
365 this->joint_calibration_simulator_[0].simulateJointCalibration(js[0],as[1]);
366 this->joint_calibration_simulator_[1].simulateJointCalibration(js[1],as[0]);
370 std::vector<JointState*>& js, std::vector<Actuator*>& as)
372 assert(as.size() == 2);
373 assert(js.size() == 2);
375 as[0]->command_.enable_ =
true;
376 as[1]->command_.enable_ =
true;
378 as[0]->command_.effort_ = (js[0]->commanded_effort_/joint_reduction_[0] - js[1]->commanded_effort_/joint_reduction_[1]) /(2.0*actuator_reduction_[0]);
379 as[1]->command_.effort_ = (-js[0]->commanded_effort_/joint_reduction_[0] - js[1]->commanded_effort_/joint_reduction_[1]) /(2.0*actuator_reduction_[1]);
383 std::vector<Actuator*>& as, std::vector<JointState*>& js)
385 assert(as.size() == 2);
386 assert(js.size() == 2);
388 js[0]->commanded_effort_ = joint_reduction_[0]*(as[0]->command_.effort_ * actuator_reduction_[0] - as[1]->command_.effort_ * actuator_reduction_[1]);
389 js[1]->commanded_effort_ = joint_reduction_[1]*(-as[0]->command_.effort_ * actuator_reduction_[0] - as[1]->command_.effort_ * actuator_reduction_[1]);
395 actuator_reduction_ = ar;
396 joint_reduction_ = jr;
void propagateEffort(std::vector< pr2_mechanism_model::JointState * > &, std::vector< pr2_hardware_interface::Actuator * > &)
Uses commanded joint efforts to fill out commanded motor currents.
void propagateEffortBackwards(std::vector< pr2_hardware_interface::Actuator * > &, std::vector< pr2_mechanism_model::JointState * > &)
Uses the actuator's commanded effort to fill out the torque on the joint.
void propagatePosition(std::vector< pr2_hardware_interface::Actuator * > &, std::vector< pr2_mechanism_model::JointState * > &)
Uses encoder data to fill out joint position and velocities.
bool initXml(TiXmlElement *config, Robot *robot)
Initializes the transmission from XML data.
ROSCPP_DECL bool isStarted()
void propagatePositionBackwards(std::vector< pr2_mechanism_model::JointState * > &, std::vector< pr2_hardware_interface::Actuator * > &)
Uses the joint position to fill out the actuator's encoder.
pr2_hardware_interface::Actuator * getActuator(const std::string &name) const
get an actuator pointer based on the actuator name. Returns NULL on failure
void setReductions(std::vector< double > &ar, std::vector< double > &jr)
This class provides the controllers with an interface to the robot model.
urdf::Model robot_model_
The kinematic/dynamic model of the robot.
PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::WristTransmission, pr2_mechanism_model::Transmission) static bool convertDouble(const char *val_str