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


sr_mechanism_model
Author(s): Ugo Cupcic / ugo@shadowrobot.com , contact@shadowrobot.com
autogenerated on Fri Jan 3 2014 12:04:56