sr_mechanism_model: joint_0_transmission_for

Go to the documentation of this file.
00001 
00028 #include "sr_mechanism_model/joint_0_transmission_for_muscle.hpp"
00029 
00030 #include <math.h>
00031 #include <pluginlib/class_list_macros.h>
00032 #include "pr2_mechanism_model/robot.h"
00033 #include "pr2_mechanism_model/simple_transmission.h"
00034 
00035 #include <sr_hardware_interface/sr_actuator.hpp>
00036 
00037 using namespace pr2_hardware_interface;
00038 
00039 PLUGINLIB_DECLARE_CLASS(sr_mechanism_model, J0TransmissionForMuscle,
00040                         sr_mechanism_model::J0TransmissionForMuscle,
00041                         pr2_mechanism_model::Transmission)
00042 
00043 
00044 namespace sr_mechanism_model
00045 {
00046   bool J0TransmissionForMuscle::initXml(TiXmlElement *elt, pr2_mechanism_model::Robot *robot)
00047   {
00048     const char *name = elt->Attribute("name");
00049     name_ = name ? name : "";
00050 
00051     TiXmlElement *jel = elt->FirstChildElement("joint1");
00052     init_joint(jel, robot);
00053     TiXmlElement *jel2 = elt->FirstChildElement("joint2");
00054     init_joint(jel2, robot);
00055 
00056     TiXmlElement *ael = elt->FirstChildElement("actuator");
00057     const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00058     Actuator *a;
00059     if (!actuator_name || (a = robot->getActuator(actuator_name)) == NULL )
00060     {
00061       ROS_ERROR("J0TransmissionForMuscle could not find actuator named \"%s\"", actuator_name);
00062       return false;
00063     }
00064     a->command_.enable_ = true;
00065     actuator_names_.push_back(actuator_name);
00066 
00067     mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
00068 
00069     return true;
00070   }
00071 
00072   bool J0TransmissionForMuscle::initXml(TiXmlElement *elt)
00073   {
00074     const char *name = elt->Attribute("name");
00075     name_ = name ? name : "";
00076 
00077     TiXmlElement *jel = elt->FirstChildElement("joint1");
00078     init_joint(jel, NULL);
00079     TiXmlElement *jel2 = elt->FirstChildElement("joint2");
00080     init_joint(jel2, NULL);
00081 
00082 
00083     TiXmlElement *ael = elt->FirstChildElement("actuator");
00084     const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00085     if (!actuator_name)
00086     {
00087       ROS_ERROR("J0TransmissionForMuscle could not find actuator named \"%s\"", actuator_name);
00088       return false;
00089     }
00090     actuator_names_.push_back(actuator_name);
00091 
00092     mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
00093 
00094     return true;
00095   }
00096 
00097   bool J0TransmissionForMuscle::init_joint(TiXmlElement *jel, pr2_mechanism_model::Robot *robot)
00098   {
00099     const char *joint_name = jel ? jel->Attribute("name") : NULL;
00100     if (!joint_name)
00101     {
00102       ROS_ERROR("J0TransmissionForMuscle did not specify joint name");
00103       return false;
00104     }
00105 
00106     if( robot != NULL )
00107     {
00108       const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
00109       if (!joint)
00110       {
00111         ROS_ERROR("J0TransmissionForMuscle could not find joint named \"%s\"", joint_name);
00112         return false;
00113       }
00114     }
00115     joint_names_.push_back(joint_name);
00116 
00117     return true;
00118   }
00119 
00120   void J0TransmissionForMuscle::propagatePosition(
00121     std::vector<pr2_hardware_interface::Actuator*>& as, std::vector<pr2_mechanism_model::JointState*>& js)
00122   {
00123     ROS_DEBUG(" propagate position for j0");
00124 
00125     assert(as.size() == 1);
00126     assert(js.size() == 2);
00127 
00128     //the size is either 0 (when the joint hasn't been updated yet), either 2
00129     // (joint 0 is composed of the 2 calibrated values: joint 1 and joint 2)
00130     int size = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.calibrated_sensor_values_.size();
00131     if( size != 0)
00132     {
00133       if( size == 2 )
00134       {
00135         ROS_DEBUG_STREAM( "READING pos " << static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.position_
00136                           << " J1 " << static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.calibrated_sensor_values_[0]
00137                           << " J2 " << static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.calibrated_sensor_values_[1] );
00138 
00139         js[0]->position_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.calibrated_sensor_values_[0];
00140         js[1]->position_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.calibrated_sensor_values_[1];
00141 
00142         js[0]->velocity_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.velocity_ / 2.0;
00143         js[1]->velocity_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.velocity_ / 2.0;
00144 
00145         //We don't want to define a modified version of JointState, as that would imply using a modified version of robot.h, controller manager,
00146         //ethercat_hardware and pr2_etherCAT main loop
00147         // So we will encode the two uint16 that contain the data from the muscle pressure sensors into the double measured_effort_. (We don't
00148         // have any measured effort in the muscle hand anyway).
00149         // Then in the joint controller we will decode that back into uint16.
00150         js[0]->measured_effort_ = (static_cast<double>(static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.pressure_[1]) * 0x10000) +
00151                                   static_cast<double>(static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.pressure_[0]);
00152         js[1]->measured_effort_ = (static_cast<double>(static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.pressure_[1]) * 0x10000) +
00153                                   static_cast<double>(static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.pressure_[0]);
00154       }
00155     }
00156     else
00157     {
00158       ROS_DEBUG_STREAM( "READING pos from Gazebo " << static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.position_
00159                         << " J1 " << static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.position_ / 2.0
00160                         << " J2 " << static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.position_ / 2.0 );
00161 
00162       //TODO: use a real formula for the coupling??
00163       //GAZEBO
00164       js[0]->position_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.position_ / 2.0;
00165       js[1]->position_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.position_ / 2.0;
00166 
00167       js[0]->velocity_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.velocity_ / 2.0;
00168       js[1]->velocity_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.velocity_ / 2.0;
00169 
00170       js[0]->measured_effort_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.last_measured_effort_ / 2.0;
00171       js[1]->measured_effort_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.last_measured_effort_ / 2.0;
00172     }
00173 
00174     ROS_DEBUG("end propagate position for j0");
00175   }
00176 
00177   void J0TransmissionForMuscle::propagatePositionBackwards(
00178     std::vector<pr2_mechanism_model::JointState*>& js, std::vector<pr2_hardware_interface::Actuator*>& as)
00179   {
00180     ROS_DEBUG("propagate pos backward for j0");
00181 
00182     assert(as.size() == 1);
00183     assert(js.size() == 2);
00184 
00185     ROS_DEBUG_STREAM("  pos = " << js[0]->position_ << " + " << js[1]->position_ << " = " << static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.position_);
00186     static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.position_ = js[0]->position_ + js[1]->position_;
00187     static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.velocity_ = js[0]->velocity_ + js[1]->velocity_;
00188     static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.last_measured_effort_ = js[0]->measured_effort_ + js[1]->measured_effort_;
00189 
00190     // Update the timing (making sure it's initialized).
00191     if (! simulated_actuator_timestamp_initialized_)
00192     {
00193       // Set the time stamp to zero (it is measured relative to the start time).
00194       static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.sample_timestamp_ = ros::Duration(0);
00195 
00196       // Try to set the start time.  Only then do we claim initialized.
00197       if (ros::isStarted())
00198       {
00199         simulated_actuator_start_time_ = ros::Time::now();
00200         simulated_actuator_timestamp_initialized_ = true;
00201       }
00202     }
00203     else
00204     {
00205       // Measure the time stamp relative to the start time.
00206       static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
00207     }
00208     // Set the historical (double) timestamp accordingly.
00209     static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.timestamp_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->state_.sample_timestamp_.toSec();
00210 
00211     // simulate calibration sensors by filling out actuator states
00212     this->joint_calibration_simulator_.simulateJointCalibration(js[0],static_cast<sr_actuator::SrMuscleActuator*>(as[0]));
00213 
00214     ROS_DEBUG(" end propagate pos backward for j0");
00215   }
00216 
00217   void J0TransmissionForMuscle::propagateEffort(
00218     std::vector<pr2_mechanism_model::JointState*>& js, std::vector<pr2_hardware_interface::Actuator*>& as)
00219   {
00220     ROS_DEBUG(" propagate effort for j0");
00221 
00222     assert(as.size() == 1);
00223     assert(js.size() == 2);
00224     static_cast<sr_actuator::SrMuscleActuator*>(as[0])->command_.enable_ = true;
00225     //We don't want to define a modified version of JointState, as that would imply using a modified version of robot.h, controller manager,
00226     //ethercat_hardware and pr2_etherCAT main loop
00227     // So the controller encodes the two int16 that contain the valve commands into the double effort_. (We don't
00228     // have any real commanded_effort_ in the muscle hand anyway).
00229     // Here we decode them back into two int16.
00230     double valve_0 = fmod(js[0]->commanded_effort_, 0x10);
00231     int8_t valve_0_tmp = static_cast<int8_t>(valve_0 + 0.5);
00232     if (valve_0_tmp >= 8)
00233     {
00234       valve_0_tmp -= 8;
00235       valve_0_tmp *= (-1);
00236     }
00237 
00238     int8_t valve_1_tmp = static_cast<int8_t>(((fmod(js[0]->commanded_effort_, 0x100) - valve_0) / 0x10) + 0.5);
00239     if (valve_1_tmp >= 8)
00240     {
00241       valve_1_tmp -= 8;
00242       valve_1_tmp *= (-1);
00243     }
00244 
00245     static_cast<sr_actuator::SrMuscleActuator*>(as[0])->command_.valve_[0] = valve_0_tmp;
00246     static_cast<sr_actuator::SrMuscleActuator*>(as[0])->command_.valve_[1] = valve_1_tmp;
00247 
00248     ROS_DEBUG("end propagate effort for j0");
00249   }
00250 
00251   void J0TransmissionForMuscle::propagateEffortBackwards(
00252     std::vector<pr2_hardware_interface::Actuator*>& as, std::vector<pr2_mechanism_model::JointState*>& js)
00253   {
00254     ROS_DEBUG("propagate effort backward for j0");
00255 
00256     assert(as.size() == 1);
00257     assert(js.size() == 2);
00258     js[0]->commanded_effort_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->command_.effort_;
00259     js[1]->commanded_effort_ = static_cast<sr_actuator::SrMuscleActuator*>(as[0])->command_.effort_;
00260 
00261     ROS_DEBUG("end propagate effort backward for j0");
00262   }
00263 
00264 } //end namespace sr_mechanism_model
00265 
00266 /* For the emacs weenies in the crowd.
00267 Local Variables:
00268    c-basic-offset: 2
00269 End:
00270 */