51 const char *name = elt->Attribute(
"name");
52 name_ = name ? name :
"";
54 TiXmlElement *jel = elt->FirstChildElement(
"joint");
55 const char *joint_name = jel ? jel->Attribute(
"name") : NULL;
58 ROS_ERROR(
"SimpleTransmission did not specify joint name");
65 ROS_ERROR(
"SimpleTransmission could not find joint named \"%s\"", joint_name);
68 joint_names_.push_back(joint_name);
70 TiXmlElement *ael = elt->FirstChildElement(
"actuator");
71 const char *actuator_name = ael ? ael->Attribute(
"name") : NULL;
73 if (!actuator_name || (a = robot->getActuator(actuator_name)) == NULL )
75 ROS_ERROR(
"SimpleTransmission could not find actuator named \"%s\"", actuator_name);
79 actuator_names_.push_back(actuator_name);
81 mechanical_reduction_ = atof(elt->FirstChildElement(
"mechanicalReduction")->GetText());
84 for (TiXmlElement *j = elt->FirstChildElement(
"simulated_actuated_joint"); j; j = j->NextSiblingElement(
"simulated_actuated_joint"))
86 const char *joint_name = j->Attribute(
"name");
89 ROS_ERROR(
"SimpleTransmission did not specify screw joint name");
90 use_simulated_actuated_joint_=
false;
97 ROS_ERROR(
"SimpleTransmission could not find screw joint named \"%s\"", joint_name);
98 use_simulated_actuated_joint_=
false;
102 use_simulated_actuated_joint_=
true;
103 joint_names_.push_back(joint_name);
106 const char *simulated_reduction = j->Attribute(
"simulated_reduction");
107 if (!simulated_reduction)
109 ROS_ERROR(
"SimpleTransmission's joint \"%s\" has no coefficient: simulated_reduction.", joint_name);
114 simulated_reduction_ = boost::lexical_cast<
double>(simulated_reduction);
116 catch (boost::bad_lexical_cast &e)
118 ROS_ERROR(
"simulated_reduction (%s) is not a float",simulated_reduction);
129 const char *name = elt->Attribute(
"name");
130 name_ = name ? name :
"";
132 TiXmlElement *jel = elt->FirstChildElement(
"joint");
133 const char *joint_name = jel ? jel->Attribute(
"name") : NULL;
136 ROS_ERROR(
"SimpleTransmission did not specify joint name");
139 joint_names_.push_back(joint_name);
141 TiXmlElement *ael = elt->FirstChildElement(
"actuator");
142 const char *actuator_name = ael ? ael->Attribute(
"name") : NULL;
145 ROS_ERROR(
"SimpleTransmission could not find actuator named \"%s\"", actuator_name);
148 actuator_names_.push_back(actuator_name);
150 mechanical_reduction_ = atof(elt->FirstChildElement(
"mechanicalReduction")->GetText());
153 for (TiXmlElement *j = elt->FirstChildElement(
"simulated_actuated_joint"); j; j = j->NextSiblingElement(
"simulated_actuated_joint"))
155 const char *joint_name = j->Attribute(
"name");
158 ROS_ERROR(
"SimpleTransmission screw joint did not specify joint name");
159 use_simulated_actuated_joint_=
false;
163 use_simulated_actuated_joint_=
true;
164 joint_names_.push_back(joint_name);
167 const char *simulated_reduction = j->Attribute(
"simulated_reduction");
168 if (!simulated_reduction)
170 ROS_ERROR(
"SimpleTransmission's joint \"%s\" has no coefficient: simulated_reduction.", joint_name);
175 simulated_reduction_ = boost::lexical_cast<
double>(simulated_reduction);
177 catch (boost::bad_lexical_cast &e)
179 ROS_ERROR(
"simulated_reduction (%s) is not a float",simulated_reduction);
188 std::vector<Actuator*>& as, std::vector<JointState*>& js)
190 assert(as.size() == 1);
191 if (use_simulated_actuated_joint_) {assert(js.size() == 2);}
192 else {assert(js.size() == 1);}
193 js[0]->position_ = (as[0]->state_.position_ / mechanical_reduction_) + js[0]->reference_position_;
194 js[0]->velocity_ = as[0]->state_.velocity_ / mechanical_reduction_;
195 js[0]->measured_effort_ = as[0]->state_.last_measured_effort_ * mechanical_reduction_;
197 if (use_simulated_actuated_joint_)
200 js[1]->position_ = 0;
201 js[1]->velocity_ = 0;
202 js[1]->measured_effort_ = 0;
203 js[1]->reference_position_ = 0;
204 js[1]->calibrated_ =
true;
209 std::vector<JointState*>& js, std::vector<Actuator*>& as)
211 assert(as.size() == 1);
212 if (use_simulated_actuated_joint_) {assert(js.size() == 2);}
213 else {assert(js.size() == 1);}
214 as[0]->state_.position_ = (js[0]->position_ - js[0]->reference_position_) * mechanical_reduction_;
215 as[0]->state_.velocity_ = js[0]->velocity_ * mechanical_reduction_;
216 as[0]->state_.last_measured_effort_ = js[0]->measured_effort_ / mechanical_reduction_;
219 if (! simulated_actuator_timestamp_initialized_)
228 simulated_actuator_timestamp_initialized_ =
true;
234 as[0]->state_.sample_timestamp_ =
ros::Time::now() - simulated_actuator_start_time_;
237 as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.
toSec();
240 this->joint_calibration_simulator_.simulateJointCalibration(js[0],as[0]);
244 std::vector<JointState*>& js, std::vector<Actuator*>& as)
246 assert(as.size() == 1);
247 if (use_simulated_actuated_joint_) {assert(js.size() == 2);}
248 else {assert(js.size() == 1);}
249 as[0]->command_.enable_ =
true;
250 as[0]->command_.effort_ = js[0]->commanded_effort_ / mechanical_reduction_;
254 std::vector<Actuator*>& as, std::vector<JointState*>& js)
256 assert(as.size() == 1);
257 if (use_simulated_actuated_joint_) {assert(js.size() == 2);}
258 else {assert(js.size() == 1);}
259 if (use_simulated_actuated_joint_)
262 js[1]->commanded_effort_ = as[0]->command_.effort_ * mechanical_reduction_/simulated_reduction_;
265 js[0]->commanded_effort_ = as[0]->command_.effort_ * mechanical_reduction_;
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.
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 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.
PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::SimpleTransmission, pr2_mechanism_model::Transmission) bool SimpleTransmission
ROSCPP_DECL bool isStarted()
void propagatePosition(std::vector< pr2_hardware_interface::Actuator * > &, std::vector< pr2_mechanism_model::JointState * > &)
Uses encoder data to fill out joint position and velocities.
This class provides the controllers with an interface to the robot model.