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