pr2_belt_transmission.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <pr2_mechanism_model/pr2_belt_transmission.h>
00036 #include <math.h>
00037 #include <pluginlib/class_list_macros.h>
00038 
00039 PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::PR2BeltCompensatorTransmission,
00040                         pr2_mechanism_model::Transmission)
00041 
00042 namespace pr2_mechanism_model {
00043 
00044 bool PR2BeltCompensatorTransmission::initXml(TiXmlElement *elt, Robot *robot)
00045 {
00046   const char *name = elt->Attribute("name");
00047   name_ = name ? name : "";
00048 
00049   TiXmlElement *jel = elt->FirstChildElement("joint");
00050   const char *joint_name = jel ? jel->Attribute("name") : NULL;
00051   if (!joint_name)
00052   {
00053     ROS_ERROR("PR2BeltCompensatorTransmission did not specify joint name");
00054     return false;
00055   }
00056 
00057   const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
00058   if (!joint)
00059   {
00060     ROS_ERROR("PR2BeltCompensatorTransmission could not find joint named \"%s\"", joint_name);
00061     return false;
00062   }
00063   joint_names_.push_back(joint_name);
00064 
00065   TiXmlElement *ael = elt->FirstChildElement("actuator");
00066   const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00067   pr2_hardware_interface::Actuator *a;
00068   if (!actuator_name || (a = robot->getActuator(actuator_name)) == NULL )
00069   {
00070     ROS_ERROR("PR2BeltCompensatorTransmission could not find actuator named \"%s\"", actuator_name);
00071     return false;
00072   }
00073   a->command_.enable_ = true;
00074   actuator_names_.push_back(actuator_name);
00075 
00076   mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
00077 
00078   TiXmlElement *c = elt->FirstChildElement("compensator");
00079   if (!c)
00080   {
00081     ROS_ERROR("No compensator element given for transmission %s", name_.c_str());
00082     return false;
00083   }
00084 
00085   double k_belt;
00086   const char *k_belt_str = c->Attribute("k_belt");
00087   if (!k_belt_str) {
00088     ROS_ERROR("No k_belt given for transmission %s", name_.c_str());
00089     return false;
00090   }
00091   k_belt = atof(k_belt_str);
00092 
00093   double mass_motor;
00094   const char *mass_motor_str = c->Attribute("mass_motor");
00095   if (!mass_motor_str) {
00096     ROS_ERROR("No mass_motor given for transmission %s", name_.c_str());
00097     return false;
00098   }
00099   mass_motor = atof(mass_motor_str);
00100 
00101   trans_compl_ = (k_belt > 0.0 ? 1.0 / k_belt : 0.0);
00102   trans_tau_ = sqrt(mass_motor * trans_compl_);
00103 
00104   const char *kd_motor_str = c->Attribute("kd_motor");
00105   if (!kd_motor_str) {
00106     ROS_ERROR("No kd_motor given for transmission %s", name_.c_str());
00107     return false;
00108   }
00109   Kd_motor_ = atof(kd_motor_str);
00110 
00111   const char *lambda_motor_str = c->Attribute("lambda_motor");
00112   if (!lambda_motor_str) {
00113     ROS_ERROR("No lambda_motor given for transmission %s", name_.c_str());
00114     return false;
00115   }
00116   lambda_motor_ = atof(lambda_motor_str);
00117 
00118   const char *lambda_joint_str = c->Attribute("lambda_joint");
00119   if (!lambda_joint_str) {
00120     ROS_ERROR("No lambda_joint given for transmission %s", name_.c_str());
00121     return false;
00122   }
00123   lambda_joint_ = atof(lambda_joint_str);
00124 
00125   const char *lambda_combined_str = c->Attribute("lambda_combined");
00126   if (!lambda_combined_str) {
00127     ROS_ERROR("No lambda_combined given for transmission %s", name_.c_str());
00128     return false;
00129   }
00130   lambda_combo_ = atof(lambda_combined_str);
00131 
00132   // Initializes the filters
00133   last_motor_pos_ = last_motor_vel_ = 0;
00134   last_jnt1_pos_ = last_jnt1_vel_ = last_jnt1_acc_ = 0;
00135   last_defl_pos_ = last_defl_vel_ = last_defl_acc_ = 0;
00136   last_joint_pos_ = last_joint_vel_ = 0;
00137   delta_motor_vel_ = 0;
00138   last_motor_damping_force_ = 0;
00139 
00140   last_timestamp_ = ros::Duration(0);
00141 
00142   // Initialize the backward transmission state variables.
00143   last_timestamp_backwards_ = last_timestamp_;
00144   halfdt_backwards_ = 0.0;
00145   motor_force_backwards_ = 0.0;
00146   last_motor_pos_backwards_ = 0.0;
00147   last_motor_vel_backwards_ = 0.0;
00148   last_motor_acc_backwards_ = 0.0;
00149   last_joint_pos_backwards_ = 0.0;
00150   last_joint_vel_backwards_ = 0.0;
00151 
00152   return true;
00153 }
00154 
00155 bool PR2BeltCompensatorTransmission::initXml(TiXmlElement *elt)
00156 {
00157   const char *name = elt->Attribute("name");
00158   name_ = name ? name : "";
00159 
00160   TiXmlElement *jel = elt->FirstChildElement("joint");
00161   const char *joint_name = jel ? jel->Attribute("name") : NULL;
00162   if (!joint_name)
00163   {
00164     ROS_ERROR("PR2BeltCompensatorTransmission did not specify joint name");
00165     return false;
00166   }
00167 
00168   joint_names_.push_back(joint_name);
00169 
00170   TiXmlElement *ael = elt->FirstChildElement("actuator");
00171   const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00172   actuator_names_.push_back(actuator_name);
00173 
00174   mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
00175 
00176   TiXmlElement *c = elt->FirstChildElement("compensator");
00177   if (!c)
00178   {
00179     ROS_ERROR("No compensator element given for transmission %s", name_.c_str());
00180     return false;
00181   }
00182 
00183   double k_belt;
00184   const char *k_belt_str = c->Attribute("k_belt");
00185   if (!k_belt_str) {
00186     ROS_ERROR("No k_belt given for transmission %s", name_.c_str());
00187     return false;
00188   }
00189   k_belt = atof(k_belt_str);
00190 
00191   double mass_motor;
00192   const char *mass_motor_str = c->Attribute("mass_motor");
00193   if (!mass_motor_str) {
00194     ROS_ERROR("No mass_motor given for transmission %s", name_.c_str());
00195     return false;
00196   }
00197   mass_motor = atof(mass_motor_str);
00198 
00199   trans_compl_ = (k_belt > 0.0 ? 1.0 / k_belt : 0.0);
00200   trans_tau_ = sqrt(mass_motor * trans_compl_);
00201 
00202   const char *kd_motor_str = c->Attribute("kd_motor");
00203   if (!kd_motor_str) {
00204     ROS_ERROR("No kd_motor given for transmission %s", name_.c_str());
00205     return false;
00206   }
00207   Kd_motor_ = atof(kd_motor_str);
00208 
00209   const char *lambda_motor_str = c->Attribute("lambda_motor");
00210   if (!lambda_motor_str) {
00211     ROS_ERROR("No lambda_motor given for transmission %s", name_.c_str());
00212     return false;
00213   }
00214   lambda_motor_ = atof(lambda_motor_str);
00215 
00216   const char *lambda_joint_str = c->Attribute("lambda_joint");
00217   if (!lambda_joint_str) {
00218     ROS_ERROR("No lambda_joint given for transmission %s", name_.c_str());
00219     return false;
00220   }
00221   lambda_joint_ = atof(lambda_joint_str);
00222 
00223   const char *lambda_combined_str = c->Attribute("lambda_combined");
00224   if (!lambda_combined_str) {
00225     ROS_ERROR("No lambda_combined given for transmission %s", name_.c_str());
00226     return false;
00227   }
00228   lambda_combo_ = atof(lambda_combined_str);
00229 
00230   // Initializes the filters
00231   last_motor_pos_ = last_motor_vel_ = 0;
00232   last_jnt1_pos_ = last_jnt1_vel_ = last_jnt1_acc_ = 0;
00233   last_defl_pos_ = last_defl_vel_ = last_defl_acc_ = 0;
00234   last_joint_pos_ = last_joint_vel_ = 0;
00235   delta_motor_vel_ = 0;
00236   last_motor_damping_force_ = 0;
00237 
00238   last_timestamp_ = ros::Duration(0);
00239 
00240   // Initialize the backward transmission state variables.
00241   last_timestamp_backwards_ = last_timestamp_;
00242   halfdt_backwards_ = 0.0;
00243   motor_force_backwards_ = 0.0;
00244   last_motor_pos_backwards_ = 0.0;
00245   last_motor_vel_backwards_ = 0.0;
00246   last_motor_acc_backwards_ = 0.0;
00247   last_joint_pos_backwards_ = 0.0;
00248   last_joint_vel_backwards_ = 0.0;
00249 
00250   return true;
00251 }
00252 
00253 void PR2BeltCompensatorTransmission::propagatePosition(
00254   std::vector<pr2_hardware_interface::Actuator*>& as, std::vector<JointState*>& js)
00255 {
00256   assert(as.size() == 1);
00257   assert(js.size() == 1);
00258   ros::Duration timestamp = as[0]->state_.sample_timestamp_;
00259   dt = (timestamp - last_timestamp_).toSec();
00260   last_timestamp_ = timestamp;
00261 
00262   // These are not the actual "motor" positions.  They are the
00263   // theoretical joint positions if there were no belt involved.
00264 
00265   // Get the motor position, velocity, measured force.  Note we do not
00266   // filter the motor velocity, because we want the least lag for the
00267   // motor damping feedback.  We will filter the joint velocity.
00268   double motor_pos = as[0]->state_.position_ / mechanical_reduction_;
00269   double motor_vel = ( dt>0.0 ? (motor_pos - last_motor_pos_)/dt : 0.0 );
00270 
00271   double motor_measured_force = as[0]->state_.last_measured_effort_ * mechanical_reduction_;
00272 
00273 
00274   // Estimate the actual joint position and velocity.  We use two
00275   // distinct methods (each with their own pros/cons and combine).
00276 
00277   // Method 1: Twice filter the motor position at the resonance mode
00278   // of the joint against the transmission and locked motor.  This
00279   // duplicates the mechanical filtering occuring in the
00280   // transmission.  Obviously, this is an approximation.  In
00281   // particular the resonance frequency is unknown (we have to assume
00282   // that lambda_joint_ over-estimates it).  Also, this doesn't
00283   // account for steady-state forces, i.e. transmission stretch.  So
00284   // the method is better at higher frequencies.  For numerical
00285   // stability, upper bound the bandwidth by 2/dt.
00286   double jnt1_pos, jnt1_vel, jnt1_acc;
00287   double lam = (dt*lambda_joint_ < 2.0 ? lambda_joint_ : 2.0/dt);
00288 
00289   jnt1_vel = last_jnt1_vel_ + 0.5*dt*(           last_jnt1_acc_);
00290   jnt1_pos = last_jnt1_pos_ + 0.5*dt*(jnt1_vel + last_jnt1_vel_);
00291 
00292   jnt1_acc = (lam*lam * (motor_pos-jnt1_pos) - 2.0*lam * jnt1_vel)
00293            / (1.0 + 0.5*dt*2.0*lam + 0.25*dt*dt*lam*lam);
00294 
00295   jnt1_vel = last_jnt1_vel_ + 0.5*dt*(jnt1_acc + last_jnt1_acc_);
00296   jnt1_pos = last_jnt1_pos_ + 0.5*dt*(jnt1_vel + last_jnt1_vel_);
00297 
00298   // Method 2: Estimate the transmission deflection explicitly.  This
00299   // uses only the transmission stiffness (compliance) and motor mass.
00300   // The later is combined with the compliance to give a transmission
00301   // time constant (tau).  This method assumes the motor mass is much
00302   // smaller than the joint mass and the resonance of the motor
00303   // against the transmission is damped.  It does NOT need to know the
00304   // joint/link mass (i.e. is independent of joint configurations) and
00305   // adds the appropriate DC transmission stretch.  However, it
00306   // assumes zero motor friction to ground and no Coulomb fiction.
00307   // Also, it uses the encoder directly at high frequency, remaining
00308   // noisy.  For numerical stability, if the time constant is zero,
00309   // implement a 0th order system.  Else lower bound the time constant
00310   // by dt/2.
00311   double defl_pos, defl_vel, defl_acc;
00312   if (trans_tau_ == 0)
00313   {
00314     defl_acc = 0.0;
00315     defl_vel = 0.0;
00316     defl_pos = trans_compl_ * motor_measured_force;
00317   }
00318   else
00319   {
00320     double tau = (dt < 2.0*trans_tau_ ? trans_tau_ : dt/2.0);
00321 
00322     defl_vel = last_defl_vel_ + 0.5*dt*(           last_defl_acc_);
00323     defl_pos = last_defl_pos_ + 0.5*dt*(defl_vel + last_defl_vel_);
00324 
00325     defl_acc = (trans_compl_ * motor_measured_force
00326                 - defl_pos - 2.0*tau * defl_vel)
00327              / (tau*tau + 2.0*tau*0.5*dt + 0.25*dt*dt);
00328 
00329     defl_vel = last_defl_vel_ + 0.5*dt*(defl_acc + last_defl_acc_);
00330     defl_pos = last_defl_pos_ + 0.5*dt*(defl_vel + last_defl_vel_);
00331   }
00332 
00333   double jnt2_pos = motor_pos - defl_pos;
00334 
00335   // Combine the two joint position estimates and calculate the
00336   // velocity: High pass method 1, low pass method 2.  In the end, the
00337   // encoder and measured force are both filtered at least once before
00338   // reaching the joint estimate.  I.e. the velocity is smooth.  For
00339   // numerical stability, upper bound the combination bandwidth.  If
00340   // the bandwidth is zero, take just method 1.
00341   double joint_pos, joint_vel;
00342   if (lambda_combo_ == 0.0)
00343   {
00344     joint_pos = jnt1_pos;
00345     joint_vel = jnt1_vel;
00346   }
00347   else
00348   {
00349     lam = (dt*lambda_combo_ < 2.0 ? lambda_combo_ : 2.0/dt);
00350 
00351     joint_pos = last_joint_pos_ + 0.5*dt*(last_joint_vel_);
00352     joint_vel = (jnt1_vel + lam * (jnt2_pos - joint_pos))
00353               / (1.0 + 0.5*dt*lam);
00354     joint_pos = last_joint_pos_ + 0.5*dt*(joint_vel + last_joint_vel_);
00355   }
00356 
00357 
00358   // Push the joint info out.
00359   js[0]->position_ = joint_pos + js[0]->reference_position_;
00360   js[0]->velocity_ = joint_vel;
00361   js[0]->measured_effort_ = as[0]->state_.last_measured_effort_ * mechanical_reduction_;
00362 
00363 
00364   // Stores values used by propogateEffort
00365   delta_motor_vel_ = motor_vel - last_motor_vel_;
00366 
00367   // Saves the current values for use in the next servo cycle.  These
00368   // are used to filter the signals
00369   last_motor_pos_   = motor_pos;
00370   last_motor_vel_   = motor_vel;
00371 
00372   last_jnt1_pos_    = jnt1_pos;
00373   last_jnt1_vel_    = jnt1_vel;
00374   last_jnt1_acc_    = jnt1_acc;
00375 
00376   last_defl_pos_    = defl_pos;
00377   last_defl_vel_    = defl_vel;
00378   last_defl_acc_    = defl_acc;
00379 
00380   last_joint_pos_   = joint_pos;
00381   last_joint_vel_   = joint_vel;
00382 }
00383 
00384 void PR2BeltCompensatorTransmission::propagateEffort(
00385   std::vector<JointState*>& js, std::vector<pr2_hardware_interface::Actuator*>& as)
00386 {
00387   assert(as.size() == 1);
00388   assert(js.size() == 1);
00389 
00390 
00391   // Calculate the damping force for the motor vibrations against the
00392   // transmission.  As we don't have a true vibration measurement,
00393   // dampen the motor simply at high frequency.  For numerical
00394   // stability, upper bound the cutoff bandwidth.  If the bandwidth is
00395   // zero, use damping over all frequencies, i.e. regular damping.
00396   double motor_damping_force;
00397   if (lambda_motor_ == 0.0)
00398   {
00399     motor_damping_force = - Kd_motor_ * last_motor_vel_;
00400   }
00401   else
00402   {
00403     double lam = (dt*lambda_motor_ < 2.0 ? lambda_motor_ : 2.0/dt);
00404 
00405     motor_damping_force = ((1.0-0.5*dt*lam) * last_motor_damping_force_
00406                            - Kd_motor_ * delta_motor_vel_)
00407                         / (1.0+0.5*dt*lam);
00408   }
00409 
00410   // Add to the joint force.
00411   double motor_force = js[0]->commanded_effort_ + motor_damping_force;
00412 
00413   // Send out!
00414   as[0]->command_.effort_ = motor_force / mechanical_reduction_;
00415 
00416   last_motor_damping_force_ = motor_damping_force;
00417 }
00418 
00419 
00420 
00421 // The backward transmission is a entirely separate entity for the
00422 // forward transmission.  Both contain state information, that is NOT
00423 // shared.  In particular, the backward transmission implements a
00424 // 4th-order model of the motor mass, belt stiffness, and joint mass,
00425 // which is what the forward transmission expects.  It should be used
00426 // ONLY in Gazebo, i.e. when simulating the robot.  As such, we assume
00427 // here that a cycle starts with a given actuator effort,
00428 // propagateEffortBackwards() is called first (setting the time step) to
00429 // set the joint effort, Gazebo then calculates a new joint position,
00430 // and propagatePositionBackwards() is called last to provide the new
00431 // actuator position.
00432 void PR2BeltCompensatorTransmission::propagateEffortBackwards(
00433   std::vector<pr2_hardware_interface::Actuator*>& as, std::vector<JointState*>& js)
00434 {
00435   // Check the arguments.  This acts only on a single actuator/joint.
00436   assert(as.size() == 1);
00437   assert(js.size() == 1);
00438 
00439   // We are simulating a motor-mass / belt-spring-damper / joint-mass
00440   // system.  Note all calculations are done if joint-space units, so
00441   // that the motor position/velocity/acceleration/force are scaled
00442   // appropriately by the mechanical reduction.  Also note, the joint
00443   // mass is actually simulated in Gazebo, so for the purposes of this
00444   // belt simulation, we assume an infinite joint mass and hence zero
00445   // joint acceleration.  Finally, we assume the belt-spring/motor-mass
00446   // dynamics are critically damped and set the damping accordingly.
00447   double motor_pos, motor_vel, motor_acc;
00448   double joint_pos, joint_vel;
00449   double motor_force;
00450   double spring_force;
00451   double halfdt;
00452 
00453   // Calculate the time step.  Should be the same as the forward
00454   // transmission, but we don't want/need to assume that.  Furthermore,
00455   // given this is used only to simulate a transmission, the time-steps
00456   // should be perfectly constant and known in advance.  But to keep
00457   // this clean we recalculate.  The question remains who defines the
00458   // time step.  Like the forward transmission, we can only use the time
00459   // step in the actuator state, though this makes little sense here...
00460   ros::Duration timestamp = as[0]->state_.sample_timestamp_;
00461   halfdt = 0.5*(timestamp - last_timestamp_backwards_).toSec();
00462   last_timestamp_backwards_ = timestamp;
00463 
00464   // Get the actuator force acting on the motor mass, multipled by the
00465   // mechanical reduction to be in joint-space units.  Note we are
00466   // assuming the command is perfectly executed, i.e. will completely
00467   // act on the motor.
00468   motor_force = as[0]->command_.effort_ * mechanical_reduction_;
00469 
00470   // If the transmission compliance is zero (infinitely stiff) or the
00471   // motor mass is zero (infinitely light), then the transmission time
00472   // constant is also zero (infinitely fast) and the entire transmission
00473   // collapses to a regular rigid connection.
00474   if ((trans_compl_ == 0.0) || (trans_tau_ == 0.0))
00475     {
00476       // Immediately propagate the motor force to the spring, ignoring
00477       // the (infinitely fast) model dynamics.
00478       spring_force = motor_force;
00479     }
00480   else
00481     {
00482       // Update the model.  Note for numerical stability, the
00483       // transmission dynamics need to be slower than the integration
00484       // time step.  Specifically we need to lower bound the tranmission
00485       // time constant by dt/2.
00486       double tau = (halfdt < trans_tau_ ? trans_tau_ : halfdt);
00487 
00488       // Calculate the new motor position/velocity assuming a new motor
00489       // acceleration of zero (simply integrate the last information).
00490       motor_vel = last_motor_vel_backwards_ + halfdt*(last_motor_acc_backwards_ + 0        );
00491       motor_pos = last_motor_pos_backwards_ + halfdt*(last_motor_vel_backwards_ + motor_vel);
00492 
00493       // Calculate the new joint position/velocity assuming a new joint
00494       // acceleration of zero, equivalent to an extremely large joint
00495       // mass (relatively to the motor).  This is also "fixed" in the
00496       // second half of the backward transmission after the simulator
00497       // has provided a new joint position/velocity.
00498       joint_vel = last_joint_vel_backwards_;
00499       joint_pos = last_joint_pos_backwards_ + halfdt*(last_joint_vel_backwards_ + joint_vel);
00500 
00501       // Calculate the spring force between the two masses.
00502       spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) / trans_compl_;
00503 
00504       // This gives us the new motor acceleration (still assuming no
00505       // joint acceleration).
00506       motor_acc = (motor_force - spring_force) * trans_compl_ / (tau*tau + 2.0*tau*halfdt + halfdt*halfdt);
00507 
00508       // Recalculate the motor position/velocity, using this new acceleration.
00509       motor_vel = last_motor_vel_backwards_ + halfdt*(last_motor_acc_backwards_ + motor_acc);
00510       motor_pos = last_motor_pos_backwards_ + halfdt*(last_motor_vel_backwards_ + motor_vel);
00511 
00512       // Recalculate the spring force.
00513       spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) / trans_compl_;
00514     }
00515   
00516   // The spring force becomes the force seen by the joint.
00517   js[0]->commanded_effort_ = spring_force;
00518 
00519   // Save the information that is to be used in the second half, i.e. in
00520   // propagatePositionBackwards().  This includes the motor force
00521   // (driving this cycle) and the time step (calculated here).
00522   halfdt_backwards_ = halfdt;
00523   motor_force_backwards_ = motor_force;
00524 }
00525 
00526 
00527 void PR2BeltCompensatorTransmission::propagatePositionBackwards(
00528   std::vector<JointState*>& js, std::vector<pr2_hardware_interface::Actuator*>& as)
00529 {
00530   // Check the arguments.  This acts only on a single actuator/joint.
00531   assert(as.size() == 1);
00532   assert(js.size() == 1);
00533 
00534   // Again (as in the first half of the backward transmission) simulate
00535   // the motor-mass / belt-spring-damper / joint-mass system.  And
00536   // again, all variables are in joint-space units.  Only this time use
00537   // the joint position/velocity provided by the simulator.
00538   double motor_pos, motor_vel, motor_acc;
00539   double joint_pos, joint_vel;
00540   double motor_force;
00541   double spring_force;
00542   double halfdt;
00543 
00544   // Get the time step and motor force from the first half of the
00545   // backward transmission.
00546   halfdt = halfdt_backwards_;
00547   motor_force = motor_force_backwards_;
00548 
00549   // Get the new joint position and velocity, as calculated by the
00550   // simulator.
00551   joint_pos = js[0]->position_ - js[0]->reference_position_;
00552   joint_vel = js[0]->velocity_;
00553 
00554   // As in the first half, if the transmission compliance or time
00555   // constant are zero, the transmission collapses to a regular rigid
00556   // transmission.
00557   if ((trans_compl_ == 0.0) || (trans_tau_ == 0.0))
00558     {
00559       // Immediately propagate the joint position/velocity to the motor,
00560       // ignoring the (infinitely fast) model dynamics.
00561       motor_acc = 0.0;
00562       motor_vel = joint_vel;
00563       motor_pos = joint_pos;
00564     }
00565   else
00566     {
00567       // Update the model.  Note for numerical stability, we again lower
00568       // bound the tranmission time constant by dt/2.
00569       double tau = (halfdt < trans_tau_ ? trans_tau_ : halfdt);
00570 
00571       // Calculate the new motor position/velocity assuming a new motor
00572       // acceleration of zero.
00573       motor_vel = last_motor_vel_backwards_ + halfdt*(last_motor_acc_backwards_ + 0        );
00574       motor_pos = last_motor_pos_backwards_ + halfdt*(last_motor_vel_backwards_ + motor_vel);
00575 
00576       // Calculate the spring force between the two masses.
00577       spring_force = (2.0*tau*(motor_vel - joint_vel) + (motor_pos - joint_pos)) / trans_compl_;
00578 
00579       // This gives us the new motor acceleration.
00580       motor_acc = (motor_force - spring_force) * trans_compl_ / (tau*tau + 2.0*tau*halfdt + halfdt*halfdt);
00581 
00582       // Recalculate the motor position/velocity, using this new acceleration.
00583       motor_vel = last_motor_vel_backwards_ + halfdt*(last_motor_acc_backwards_ + motor_acc);
00584       motor_pos = last_motor_pos_backwards_ + halfdt*(last_motor_vel_backwards_ + motor_vel);
00585     }
00586 
00587   // Save the current state for the next cycle.
00588   last_motor_pos_backwards_ = motor_pos;
00589   last_motor_vel_backwards_ = motor_vel;
00590   last_motor_acc_backwards_ = motor_acc;
00591 
00592   last_joint_pos_backwards_ = joint_pos;
00593   last_joint_vel_backwards_ = joint_vel;
00594 
00595   // Push the motor position/velocity to the actuator, accounting for
00596   // the mechanical reduction.
00597   as[0]->state_.position_ = motor_pos * mechanical_reduction_;
00598   as[0]->state_.velocity_ = motor_vel * mechanical_reduction_;
00599 
00600   // Also push the motor force to the actuator.  Note we already assumed
00601   // that the commanded actuator effort was accurately executed/applied,
00602   // so the measured actuator effort is just the motor force.
00603   as[0]->state_.last_measured_effort_ = motor_force / mechanical_reduction_;
00604 
00605 
00606   // By storing the new actuator data, we are advancing to the next
00607   // servo cycle.  Always make sure the timing has been initialized.
00608   if (! simulated_actuator_timestamp_initialized_)
00609     {
00610       // Set the time stamp to zero (it is measured relative to the start time).
00611       as[0]->state_.sample_timestamp_ = ros::Duration(0);
00612 
00613       // Try to set the start time.  Only then do we claim initialized.
00614       if (ros::isStarted())
00615         {
00616           simulated_actuator_start_time_ = ros::Time::now();
00617           simulated_actuator_timestamp_initialized_ = true;
00618         }
00619     }
00620   else
00621     {
00622       // Measure the time stamp relative to the start time.
00623       as[0]->state_.sample_timestamp_ = ros::Time::now() - simulated_actuator_start_time_;
00624     }
00625   // Set the historical (double) timestamp accordingly.
00626   as[0]->state_.timestamp_ = as[0]->state_.sample_timestamp_.toSec();
00627 
00628 
00629   // Simulate calibration sensors by filling out actuator states
00630   this->joint_calibration_simulator_.simulateJointCalibration(js[0],as[0]);
00631 }
00632 
00633 } // namespace


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Dec 2 2013 13:13:02