00001 00030 #include "sr_mechanism_model/joint_0_transmission.hpp" 00031 00032 using namespace ros_ethercat_model; 00033 using namespace std; 00034 using namespace sr_actuator; 00035 00036 PLUGINLIB_EXPORT_CLASS(sr_mechanism_model::J0Transmission, Transmission) 00037 00038 namespace sr_mechanism_model 00039 { 00040 00041 bool J0Transmission::initXml(TiXmlElement *elt, RobotState *robot) 00042 { 00043 if (!SimpleTransmission::initXml(elt, robot)) 00044 return false; 00045 00046 string joint2_name = joint_->joint_->name; 00047 joint2_name[joint2_name.size()-1] = '2'; 00048 joint2_ = robot->getJointState(joint2_name); 00049 00050 return true; 00051 } 00052 00053 void J0Transmission::propagatePosition() 00054 { 00055 //the size is either 2 or 0 when the joint hasn't been updated yet 00056 // (joint 0 is composed of the 2 calibrated values: joint 1 and joint 2) 00057 00058 SrMotorActuator *act = static_cast<SrMotorActuator*>(actuator_); 00059 size_t size = act->motor_state_.calibrated_sensor_values_.size(); 00060 if (size == 2) 00061 { 00062 ROS_DEBUG_STREAM("READING pos " << act->state_.position_ 00063 << " J1 " << act->motor_state_.calibrated_sensor_values_[0] 00064 << " J2 " << act->motor_state_.calibrated_sensor_values_[1]); 00065 00066 joint_->position_ = act->motor_state_.calibrated_sensor_values_[0]; 00067 joint2_->position_ = act->motor_state_.calibrated_sensor_values_[1]; 00068 00069 joint_->velocity_ = act->state_.velocity_ / 2.0; 00070 joint2_->velocity_ = act->state_.velocity_ / 2.0; 00071 00072 joint_->effort_ = act->state_.last_measured_effort_; 00073 joint2_->effort_ = act->state_.last_measured_effort_; 00074 } 00075 else if (size == 0) 00076 { 00077 ROS_DEBUG_STREAM("READING pos from Gazebo " << act->state_.position_ 00078 << " J1 " << act->state_.position_ / 2.0 00079 << " J2 " << act->state_.position_ / 2.0); 00080 00081 //TODO: use a real formula for the coupling?? 00082 //GAZEBO 00083 joint_->position_ = act->state_.position_ / 2.0; 00084 joint2_->position_ = act->state_.position_ / 2.0; 00085 00086 joint_->velocity_ = act->state_.velocity_ / 2.0; 00087 joint2_->velocity_ = act->state_.velocity_ / 2.0; 00088 00089 joint_->effort_ = act->state_.last_measured_effort_ / 2.0; 00090 joint2_->effort_ = act->state_.last_measured_effort_ / 2.0; 00091 } 00092 } 00093 00094 } //end namespace sr_mechanism_model 00095 00096 /* For the emacs weenies in the crowd. 00097 Local Variables: 00098 c-basic-offset: 2 00099 End: 00100 */