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");
63 urdf::JointConstSharedPtr joint = robot->
robot_model_.getJoint(joint_name);
69 ROS_ERROR(
"SimpleTransmission could not find joint named \"%s\"", joint_name);
72 joint_names_.push_back(joint_name);
74 TiXmlElement *ael = elt->FirstChildElement(
"actuator");
75 const char *actuator_name = ael ? ael->Attribute(
"name") : NULL;
77 if (!actuator_name || (a = robot->
getActuator(actuator_name)) == NULL )
79 ROS_ERROR(
"SimpleTransmission could not find actuator named \"%s\"", actuator_name);
83 actuator_names_.push_back(actuator_name);
86 if(ael->FirstChildElement(
"mechanicalReduction"))
87 mechanical_reduction_ = atof(ael->FirstChildElement(
"mechanicalReduction")->GetText());
89 mechanical_reduction_ = atof(elt->FirstChildElement(
"mechanicalReduction")->GetText());
92 for (TiXmlElement *j = elt->FirstChildElement(
"simulated_actuated_joint"); j; j = j->NextSiblingElement(
"simulated_actuated_joint"))
94 const char *joint_name = j->Attribute(
"name");
97 ROS_ERROR(
"SimpleTransmission did not specify screw joint name");
98 use_simulated_actuated_joint_=
false;
102 #if URDFDOM_1_0_0_API 103 urdf::JointConstSharedPtr joint = robot->
robot_model_.getJoint(joint_name);
109 ROS_ERROR(
"SimpleTransmission could not find screw joint named \"%s\"", joint_name);
110 use_simulated_actuated_joint_=
false;
114 use_simulated_actuated_joint_=
true;
115 joint_names_.push_back(joint_name);
118 const char *simulated_reduction = j->Attribute(
"simulated_reduction");
119 if (!simulated_reduction)
121 ROS_ERROR(
"SimpleTransmission's joint \"%s\" has no coefficient: simulated_reduction.", joint_name);
126 simulated_reduction_ = boost::lexical_cast<
double>(simulated_reduction);
128 catch (boost::bad_lexical_cast &e)
130 ROS_ERROR(
"simulated_reduction (%s) is not a float",simulated_reduction);
141 const char *name = elt->Attribute(
"name");
142 name_ = name ? name :
"";
144 TiXmlElement *jel = elt->FirstChildElement(
"joint");
145 const char *joint_name = jel ? jel->Attribute(
"name") : NULL;
148 ROS_ERROR(
"SimpleTransmission did not specify joint name");
151 joint_names_.push_back(joint_name);
153 TiXmlElement *ael = elt->FirstChildElement(
"actuator");
154 const char *actuator_name = ael ? ael->Attribute(
"name") : NULL;
157 ROS_ERROR(
"SimpleTransmission could not find actuator named \"%s\"", actuator_name);
160 actuator_names_.push_back(actuator_name);
162 mechanical_reduction_ = atof(elt->FirstChildElement(
"mechanicalReduction")->GetText());
165 for (TiXmlElement *j = elt->FirstChildElement(
"simulated_actuated_joint"); j; j = j->NextSiblingElement(
"simulated_actuated_joint"))
167 const char *joint_name = j->Attribute(
"name");
170 ROS_ERROR(
"SimpleTransmission screw joint did not specify joint name");
171 use_simulated_actuated_joint_=
false;
175 use_simulated_actuated_joint_=
true;
176 joint_names_.push_back(joint_name);
179 const char *simulated_reduction = j->Attribute(
"simulated_reduction");
180 if (!simulated_reduction)
182 ROS_ERROR(
"SimpleTransmission's joint \"%s\" has no coefficient: simulated_reduction.", joint_name);
187 simulated_reduction_ = boost::lexical_cast<
double>(simulated_reduction);
189 catch (boost::bad_lexical_cast &e)
191 ROS_ERROR(
"simulated_reduction (%s) is not a float",simulated_reduction);
200 std::vector<Actuator*>& as, std::vector<JointState*>& js)
202 assert(as.size() == 1);
203 if (use_simulated_actuated_joint_) {assert(js.size() == 2);}
204 else {assert(js.size() == 1);}
205 js[0]->position_ = (as[0]->state_.position_ / mechanical_reduction_) + js[0]->reference_position_;
206 js[0]->velocity_ = as[0]->state_.velocity_ / mechanical_reduction_;
207 js[0]->measured_effort_ = as[0]->state_.last_measured_effort_ * mechanical_reduction_;
209 if (use_simulated_actuated_joint_)
212 js[1]->position_ = 0;
213 js[1]->velocity_ = 0;
214 js[1]->measured_effort_ = 0;
215 js[1]->reference_position_ = 0;
216 js[1]->calibrated_ =
true;
221 std::vector<JointState*>& js, std::vector<Actuator*>& as)
223 assert(as.size() == 1);
224 if (use_simulated_actuated_joint_) {assert(js.size() == 2);}
225 else {assert(js.size() == 1);}
226 as[0]->state_.position_ = (js[0]->position_ - js[0]->reference_position_) * mechanical_reduction_;
227 as[0]->state_.velocity_ = js[0]->velocity_ * mechanical_reduction_;
228 as[0]->state_.last_measured_effort_ = js[0]->measured_effort_ / mechanical_reduction_;
231 if (! simulated_actuator_timestamp_initialized_)
240 simulated_actuator_timestamp_initialized_ =
true;
246 as[0]->state_.sample_timestamp_ =
ros::Time::now() - simulated_actuator_start_time_;
249 as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.
toSec();
252 this->joint_calibration_simulator_.simulateJointCalibration(js[0],as[0]);
256 std::vector<JointState*>& js, std::vector<Actuator*>& as)
258 assert(as.size() == 1);
259 if (use_simulated_actuated_joint_) {assert(js.size() == 2);}
260 else {assert(js.size() == 1);}
261 as[0]->command_.enable_ =
true;
262 as[0]->command_.effort_ = js[0]->commanded_effort_ / mechanical_reduction_;
266 std::vector<Actuator*>& as, std::vector<JointState*>& js)
268 assert(as.size() == 1);
269 if (use_simulated_actuated_joint_) {assert(js.size() == 2);}
270 else {assert(js.size() == 1);}
271 if (use_simulated_actuated_joint_)
274 js[1]->commanded_effort_ = as[0]->command_.effort_ * mechanical_reduction_/simulated_reduction_;
277 js[0]->commanded_effort_ = as[0]->command_.effort_ * mechanical_reduction_;
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 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 propagateEffort(std::vector< pr2_mechanism_model::JointState *> &, std::vector< pr2_hardware_interface::Actuator *> &)
Uses commanded joint efforts to fill out commanded motor currents.
pr2_hardware_interface::Actuator * getActuator(const std::string &name) const
get an actuator pointer based on the actuator name. Returns NULL on failure
bool initXml(TiXmlElement *config, Robot *robot)
Initializes the transmission from XML data.
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.
PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::SimpleTransmission, pr2_mechanism_model::Transmission) bool SimpleTransmission
ROSCPP_DECL bool isStarted()
This class provides the controllers with an interface to the robot model.
urdf::Model robot_model_
The kinematic/dynamic model of the robot.