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");
117 #if URDFDOM_1_0_0_API 118 urdf::JointConstSharedPtr joint = robot->
robot_model_.getJoint(joint_name);
125 ROS_ERROR(
"WristTransmission could not find joint named \"%s\"", joint_name);
128 joint_names_.push_back(joint_name);
129 const char *joint_red = j->Attribute(
"mechanicalReduction");
132 ROS_WARN(
"WristTransmission's joint \"%s\" was not given a reduction.", joint_name);
135 joint_reduction_.push_back(atof(joint_red));
136 const char *joint_offset = j->Attribute(
"offset");
139 joint_offset_[0] = 0.0;
143 if (!convertDouble(joint_offset, joint_offset_[0]))
145 ROS_WARN(
"WristTransmission's joint \"%s\", cannot convert jointOffset attribute \"%s\" to floating point.",
146 joint_name, joint_offset);
151 ROS_WARN(
"Joint offset of %f for joint %s.", joint_offset_[0], joint_name);
155 j = elt->FirstChildElement(
"rollJoint");
156 joint_name = j->Attribute(
"name");
159 ROS_ERROR(
"WristTransmission did not specify joint name");
162 #if URDFDOM_1_0_0_API 163 urdf::JointConstSharedPtr joint2 = robot->
robot_model_.getJoint(joint_name);
170 ROS_ERROR(
"WristTransmission could not find joint named \"%s\"", joint_name);
173 joint_names_.push_back(joint_name);
174 joint_red = j->Attribute(
"mechanicalReduction");
177 ROS_WARN(
"WristTransmission's joint \"%s\" was not given a reduction.", joint_name);
180 joint_reduction_.push_back(atof(joint_red));
181 joint_offset = j->Attribute(
"offset");
184 joint_offset_[1] = 0.0;
188 if (!convertDouble(joint_offset, joint_offset_[1]))
190 ROS_WARN(
"WristTransmission's joint \"%s\", cannot convert jointOffset attribute \"%s\" to floating point.",
191 joint_name, joint_offset);
196 ROS_WARN(
"Joint offset of %f for joint %s.", joint_offset_[1], joint_name);
206 const char *name = elt->Attribute(
"name");
207 name_ = name ? name :
"";
210 TiXmlElement *ael = elt->FirstChildElement(
"rightActuator");
211 const char *actuator_name = ael ? ael->Attribute(
"name") : NULL;
214 ROS_WARN(
"WristTransmission could not find actuator named \"%s\"", actuator_name);
217 actuator_names_.push_back(actuator_name);
218 const char *act_red = ael->Attribute(
"mechanicalReduction");
221 ROS_WARN(
"WristTransmission's actuator \"%s\" was not given a reduction.", actuator_name);
224 actuator_reduction_.push_back(atof(act_red));
226 ael = elt->FirstChildElement(
"leftActuator");
227 actuator_name = ael ? ael->Attribute(
"name") : NULL;
230 ROS_WARN(
"WristTransmission could not find actuator named \"%s\"", actuator_name);
233 actuator_names_.push_back(actuator_name);
234 act_red = ael->Attribute(
"mechanicalReduction");
237 ROS_WARN(
"WristTransmission's actuator \"%s\" was not given a reduction.", actuator_name);
240 actuator_reduction_.push_back(atof(act_red));
243 TiXmlElement *j = elt->FirstChildElement(
"flexJoint");
244 const char *joint_name = j->Attribute(
"name");
247 ROS_ERROR(
"WristTransmission did not specify joint name");
250 joint_names_.push_back(joint_name);
251 const char *joint_red = j->Attribute(
"mechanicalReduction");
254 ROS_WARN(
"WristTransmission's joint \"%s\" was not given a reduction.", joint_name);
257 joint_reduction_.push_back(atof(joint_red));
258 const char *joint_offset = j->Attribute(
"offset");
261 joint_offset_[0] = 0.0;
266 if (!convertDouble(joint_offset, joint_offset_[0]))
268 ROS_WARN(
"WristTransmission's joint \"%s\", cannot convert jointOffset attribute \"%s\" to floating point.",
269 joint_name, joint_offset);
274 ROS_WARN(
"Joint offset of %f for joint %s.", joint_offset_[0], joint_name);
278 j = elt->FirstChildElement(
"rollJoint");
279 joint_name = j->Attribute(
"name");
282 ROS_ERROR(
"WristTransmission did not specify joint name");
285 joint_names_.push_back(joint_name);
286 joint_red = j->Attribute(
"mechanicalReduction");
289 ROS_WARN(
"WristTransmission's joint \"%s\" was not given a reduction.", joint_name);
292 joint_reduction_.push_back(atof(joint_red));
295 joint_offset_[1] = 0.0;
299 if (!convertDouble(joint_offset, joint_offset_[1]))
301 ROS_WARN(
"WristTransmission's joint \"%s\", cannot convert jointOffset attribute \"%s\" to floating point.",
302 joint_name, joint_offset);
307 ROS_WARN(
"Joint offset of %f for joint %s.", joint_offset_[1], joint_name);
315 std::vector<Actuator*>& as, std::vector<JointState*>& js)
317 assert(as.size() == 2);
318 assert(js.size() == 2);
320 js[0]->position_ = ((as[0]->state_.position_ / actuator_reduction_[0] - as[1]->state_.position_ / actuator_reduction_[1])/
321 (2*joint_reduction_[0])) + js[0]->reference_position_+joint_offset_[0];
322 js[0]->velocity_ = (as[0]->state_.velocity_ / actuator_reduction_[0] - as[1]->state_.velocity_ / actuator_reduction_[1])/(2*joint_reduction_[0]);
323 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]);
325 js[1]->position_ = ((-as[0]->state_.position_ / actuator_reduction_[0] - as[1]->state_.position_ / actuator_reduction_[1])/
326 (2*joint_reduction_[1]))+js[1]->reference_position_+joint_offset_[1];
327 js[1]->velocity_ = (-as[0]->state_.velocity_ / actuator_reduction_[0] - as[1]->state_.velocity_ / actuator_reduction_[1])/(2*joint_reduction_[1]);
328 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]);
332 std::vector<JointState*>& js, std::vector<Actuator*>& as)
334 assert(as.size() == 2);
335 assert(js.size() == 2);
337 as[0]->state_.position_ = (((js[0]->position_-js[0]->reference_position_-joint_offset_[0])*joint_reduction_[0] -
338 (js[1]->position_-js[1]->reference_position_-joint_offset_[1])*joint_reduction_[1]) * actuator_reduction_[0]);
339 as[0]->state_.velocity_ = ((js[0]->velocity_*joint_reduction_[0] - js[1]->velocity_*joint_reduction_[1]) * actuator_reduction_[0]);
340 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]);
342 as[1]->state_.position_ = ((-(js[0]->position_-js[0]->reference_position_-joint_offset_[0])*joint_reduction_[0] -
343 (js[1]->position_-js[1]->reference_position_-joint_offset_[1])*joint_reduction_[1]) * actuator_reduction_[1]);
344 as[1]->state_.velocity_ = ((-js[0]->velocity_*joint_reduction_[0] - js[1]->velocity_*joint_reduction_[1]) * actuator_reduction_[1]);
345 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]);
348 if (! simulated_actuator_timestamp_initialized_)
358 simulated_actuator_timestamp_initialized_ =
true;
364 as[0]->state_.sample_timestamp_ =
ros::Time::now() - simulated_actuator_start_time_;
365 as[1]->state_.sample_timestamp_ =
ros::Time::now() - simulated_actuator_start_time_;
368 as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.
toSec();
369 as[1]->state_.timestamp_ = as[1]->state_.sample_timestamp_.toSec();
373 this->joint_calibration_simulator_[0].simulateJointCalibration(js[0],as[1]);
374 this->joint_calibration_simulator_[1].simulateJointCalibration(js[1],as[0]);
378 std::vector<JointState*>& js, std::vector<Actuator*>& as)
380 assert(as.size() == 2);
381 assert(js.size() == 2);
383 as[0]->command_.enable_ =
true;
384 as[1]->command_.enable_ =
true;
386 as[0]->command_.effort_ = (js[0]->commanded_effort_/joint_reduction_[0] - js[1]->commanded_effort_/joint_reduction_[1]) /(2.0*actuator_reduction_[0]);
387 as[1]->command_.effort_ = (-js[0]->commanded_effort_/joint_reduction_[0] - js[1]->commanded_effort_/joint_reduction_[1]) /(2.0*actuator_reduction_[1]);
391 std::vector<Actuator*>& as, std::vector<JointState*>& js)
393 assert(as.size() == 2);
394 assert(js.size() == 2);
396 js[0]->commanded_effort_ = joint_reduction_[0]*(as[0]->command_.effort_ * actuator_reduction_[0] - as[1]->command_.effort_ * actuator_reduction_[1]);
397 js[1]->commanded_effort_ = joint_reduction_[1]*(-as[0]->command_.effort_ * actuator_reduction_[0] - as[1]->command_.effort_ * actuator_reduction_[1]);
403 actuator_reduction_ = ar;
404 joint_reduction_ = jr;
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 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.
void propagateEffort(std::vector< pr2_mechanism_model::JointState *> &, std::vector< pr2_hardware_interface::Actuator *> &)
Uses commanded joint efforts to fill out commanded motor currents.
bool initXml(TiXmlElement *config, Robot *robot)
Initializes the transmission from XML data.
ROSCPP_DECL bool isStarted()
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