00001 00028 #include "sr_mechanism_model/joint_0_transmission_for_muscle.hpp" 00029 00030 using namespace ros_ethercat_model; 00031 using namespace std; 00032 using namespace sr_actuator; 00033 00034 PLUGINLIB_EXPORT_CLASS(sr_mechanism_model::J0TransmissionForMuscle, Transmission) 00035 00036 namespace sr_mechanism_model 00037 { 00038 00039 bool J0TransmissionForMuscle::initXml(TiXmlElement *elt, RobotState *robot) 00040 { 00041 if (!SimpleTransmissionForMuscle::initXml(elt, robot)) 00042 return false; 00043 00044 string joint2_name = joint_->joint_->name; 00045 joint2_name[joint2_name.size()-1] = '2'; 00046 joint2_ = robot->getJointState(joint2_name); 00047 00048 return true; 00049 } 00050 00051 void J0TransmissionForMuscle::propagatePosition() 00052 { 00053 //the size is either 2 or 0 when the joint hasn't been updated yet 00054 // (joint 0 is composed of the 2 calibrated values: joint 1 and joint 2) 00055 SrMuscleActuator *act = static_cast<SrMuscleActuator*>(actuator_); 00056 size_t size = act->muscle_state_.calibrated_sensor_values_.size(); 00057 if (size == 0) 00058 { 00059 ROS_DEBUG_STREAM("READING pos " << act->state_.position_ 00060 << " J1 " << act->muscle_state_.calibrated_sensor_values_[0] 00061 << " J2 " << act->muscle_state_.calibrated_sensor_values_[1]); 00062 00063 joint_->position_ = act->muscle_state_.calibrated_sensor_values_[0]; 00064 joint2_->position_ = act->muscle_state_.calibrated_sensor_values_[1]; 00065 00066 joint_->velocity_ = act->state_.velocity_ / 2.0; 00067 joint2_->velocity_ = act->state_.velocity_ / 2.0; 00068 00069 // We don't want to define a modified version of JointState, as that would imply using a modified version 00070 // of robot_state.hpp, controller manager, ethercat_hardware and ros_etherCAT main loop 00071 // So we will encode the two uint16_t that contain the data from the muscle pressure sensors 00072 // into the double effort_. (We don't have any measured effort in the muscle hand anyway). 00073 // Then in the joint controller we will decode that back into uint16_t. 00074 joint_->effort_ = ((double) (act->muscle_state_.pressure_[1]) * 0x10000) 00075 + (double) (act->muscle_state_.pressure_[0]); 00076 joint2_->effort_ = ((double) (act->muscle_state_.pressure_[1]) * 0x10000) 00077 + (double) (act->muscle_state_.pressure_[0]); 00078 } 00079 else if (size == 0) 00080 { 00081 ROS_DEBUG_STREAM("READING pos from Gazebo " << act->state_.position_ 00082 << " J1 " << act->state_.position_ / 2.0 00083 << " J2 " << act->state_.position_ / 2.0); 00084 00085 //TODO: use a real formula for the coupling?? 00086 //GAZEBO 00087 joint_->position_ = act->state_.position_ / 2.0; 00088 joint2_->position_ = act->state_.position_ / 2.0; 00089 00090 joint_->velocity_ = act->state_.velocity_ / 2.0; 00091 joint2_->velocity_ = act->state_.velocity_ / 2.0; 00092 00093 joint_->effort_ = act->state_.last_measured_effort_ / 2.0; 00094 joint2_->effort_ = act->state_.last_measured_effort_ / 2.0; 00095 } 00096 } 00097 00098 } //end namespace sr_mechanism_model 00099 00100 /* For the emacs weenies in the crowd. 00101 Local Variables: 00102 c-basic-offset: 2 00103 End: 00104 */