46 const char *name = elt->Attribute(
"name");
47 name_ = name ? name :
"";
49 TiXmlElement *jel = elt->FirstChildElement(
"joint");
50 const char *joint_name = jel ? jel->Attribute(
"name") : NULL;
53 ROS_ERROR(
"PR2BeltCompensatorTransmission did not specify joint name");
58 urdf::JointConstSharedPtr joint = robot->robot_model_.getJoint(joint_name);
64 ROS_ERROR(
"PR2BeltCompensatorTransmission could not find joint named \"%s\"", joint_name);
69 TiXmlElement *ael = elt->FirstChildElement(
"actuator");
70 const char *actuator_name = ael ? ael->Attribute(
"name") : NULL;
72 if (!actuator_name || (a = robot->getActuator(actuator_name)) == NULL )
74 ROS_ERROR(
"PR2BeltCompensatorTransmission could not find actuator named \"%s\"", actuator_name);
82 TiXmlElement *c = elt->FirstChildElement(
"compensator");
85 ROS_ERROR(
"No compensator element given for transmission %s",
name_.c_str());
90 const char *k_belt_str = c->Attribute(
"k_belt");
95 k_belt = atof(k_belt_str);
98 const char *mass_motor_str = c->Attribute(
"mass_motor");
99 if (!mass_motor_str) {
100 ROS_ERROR(
"No mass_motor given for transmission %s",
name_.c_str());
103 mass_motor = atof(mass_motor_str);
108 const char *kd_motor_str = c->Attribute(
"kd_motor");
110 ROS_ERROR(
"No kd_motor given for transmission %s",
name_.c_str());
115 const char *lambda_motor_str = c->Attribute(
"lambda_motor");
116 if (!lambda_motor_str) {
117 ROS_ERROR(
"No lambda_motor given for transmission %s",
name_.c_str());
122 const char *lambda_joint_str = c->Attribute(
"lambda_joint");
123 if (!lambda_joint_str) {
124 ROS_ERROR(
"No lambda_joint given for transmission %s",
name_.c_str());
129 const char *lambda_combined_str = c->Attribute(
"lambda_combined");
130 if (!lambda_combined_str) {
131 ROS_ERROR(
"No lambda_combined given for transmission %s",
name_.c_str());
161 const char *name = elt->Attribute(
"name");
162 name_ = name ? name :
"";
164 TiXmlElement *jel = elt->FirstChildElement(
"joint");
165 const char *joint_name = jel ? jel->Attribute(
"name") : NULL;
168 ROS_ERROR(
"PR2BeltCompensatorTransmission did not specify joint name");
174 TiXmlElement *ael = elt->FirstChildElement(
"actuator");
175 const char *actuator_name = ael ? ael->Attribute(
"name") : NULL;
180 TiXmlElement *c = elt->FirstChildElement(
"compensator");
183 ROS_ERROR(
"No compensator element given for transmission %s",
name_.c_str());
188 const char *k_belt_str = c->Attribute(
"k_belt");
193 k_belt = atof(k_belt_str);
196 const char *mass_motor_str = c->Attribute(
"mass_motor");
197 if (!mass_motor_str) {
198 ROS_ERROR(
"No mass_motor given for transmission %s",
name_.c_str());
201 mass_motor = atof(mass_motor_str);
206 const char *kd_motor_str = c->Attribute(
"kd_motor");
208 ROS_ERROR(
"No kd_motor given for transmission %s",
name_.c_str());
213 const char *lambda_motor_str = c->Attribute(
"lambda_motor");
214 if (!lambda_motor_str) {
215 ROS_ERROR(
"No lambda_motor given for transmission %s",
name_.c_str());
220 const char *lambda_joint_str = c->Attribute(
"lambda_joint");
221 if (!lambda_joint_str) {
222 ROS_ERROR(
"No lambda_joint given for transmission %s",
name_.c_str());
227 const char *lambda_combined_str = c->Attribute(
"lambda_combined");
228 if (!lambda_combined_str) {
229 ROS_ERROR(
"No lambda_combined given for transmission %s",
name_.c_str());
258 std::vector<pr2_hardware_interface::Actuator*>& as, std::vector<JointState*>& js)
260 assert(as.size() == 1);
261 assert(js.size() == 1);
290 double jnt1_pos, jnt1_vel, jnt1_acc;
296 jnt1_acc = (lam*lam * (motor_pos-jnt1_pos) - 2.0*lam * jnt1_vel)
297 / (1.0 + 0.5*
dt*2.0*lam + 0.25*
dt*
dt*lam*lam);
315 double defl_pos, defl_vel, defl_acc;
330 - defl_pos - 2.0*tau * defl_vel)
331 / (tau*tau + 2.0*tau*0.5*
dt + 0.25*
dt*
dt);
337 double jnt2_pos = motor_pos - defl_pos;
345 double joint_pos, joint_vel;
348 joint_pos = jnt1_pos;
349 joint_vel = jnt1_vel;
356 joint_vel = (jnt1_vel + lam * (jnt2_pos - joint_pos))
357 / (1.0 + 0.5*
dt*lam);
363 js[0]->position_ = joint_pos + js[0]->reference_position_;
364 js[0]->velocity_ = joint_vel;
374 last_motor_vel_ = motor_vel;
389 std::vector<JointState*>& js, std::vector<pr2_hardware_interface::Actuator*>& as)
391 assert(as.size() == 1);
392 assert(js.size() == 1);
400 double motor_damping_force;
415 double motor_force = js[0]->commanded_effort_ + motor_damping_force;
437 std::vector<pr2_hardware_interface::Actuator*>& as, std::vector<JointState*>& js)
440 assert(as.size() == 1);
441 assert(js.size() == 1);
451 double motor_pos, motor_vel, motor_acc;
452 double joint_pos, joint_vel;
482 spring_force = motor_force;
506 spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) /
trans_compl_;
510 motor_acc = (motor_force - spring_force) *
trans_compl_ / (tau*tau + 2.0*tau*halfdt + halfdt*halfdt);
517 spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) /
trans_compl_;
521 js[0]->commanded_effort_ = spring_force;
532 std::vector<JointState*>& js, std::vector<pr2_hardware_interface::Actuator*>& as)
535 assert(as.size() == 1);
536 assert(js.size() == 1);
542 double motor_pos, motor_vel, motor_acc;
543 double joint_pos, joint_vel;
555 joint_pos = js[0]->position_ - js[0]->reference_position_;
556 joint_vel = js[0]->velocity_;
566 motor_vel = joint_vel;
567 motor_pos = joint_pos;
581 spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) /
trans_compl_;
584 motor_acc = (motor_force - spring_force) *
trans_compl_ / (tau*tau + 2.0*tau*halfdt + halfdt*halfdt);
630 as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.
toSec();
ros::Time simulated_actuator_start_time_
void simulateJointCalibration(pr2_mechanism_model::JointState *, pr2_hardware_interface::Actuator *)
void propagatePosition(std::vector< pr2_hardware_interface::Actuator *> &, std::vector< pr2_mechanism_model::JointState *> &)
Uses encoder data to fill out joint position and velocities.
double 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 propagateEffort(std::vector< pr2_mechanism_model::JointState *> &, std::vector< pr2_hardware_interface::Actuator *> &)
Uses commanded joint efforts to fill out commanded motor currents.
ros::Duration last_timestamp_backwards_
bool initXml(TiXmlElement *config, Robot *robot)
Initializes the transmission from XML data.
int simulated_actuator_timestamp_initialized_
std::vector< std::string > actuator_names_
PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::PR2BeltCompensatorTransmission, pr2_mechanism_model::Transmission) namespace pr2_mechanism_model
double last_joint_pos_backwards_
double last_motor_vel_backwards_
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
ROSCPP_DECL bool isStarted()
std::vector< std::string > joint_names_
double last_motor_damping_force_
ros::Duration last_timestamp_
std::string name_
the name of the transmission
double last_motor_pos_backwards_
JointCalibrationSimulator joint_calibration_simulator_
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.
double last_motor_acc_backwards_
double motor_force_backwards_
double last_joint_vel_backwards_