pr2_calibration_controllers: wrist_calibration

00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Author: Stuart Glaser
00032  */
00033 
00034 #include "pr2_calibration_controllers/wrist_calibration_controller.h"
00035 #include "pluginlib/class_list_macros.h"
00036 
00037 PLUGINLIB_DECLARE_CLASS(pr2_calibration_controllers, WristCalibrationController,
00038                         controller::WristCalibrationController, pr2_controller_interface::Controller)
00039 
00040 namespace controller {
00041 
00042 
00043 WristCalibrationController::WristCalibrationController()
00044 : robot_(NULL), last_publish_time_(0)
00045 {
00046 }
00047 
00048 WristCalibrationController::~WristCalibrationController()
00049 {
00050   for (size_t i = 0; i < fake_as.size(); ++i)
00051     delete fake_as[i];
00052   for (size_t i = 0; i < fake_js.size(); ++i)
00053     delete fake_js[i];
00054 }
00055 
00056 bool WristCalibrationController::init(pr2_mechanism_model::RobotState *robot,
00057                                       ros::NodeHandle &n)
00058 {
00059   assert(robot);
00060   node_ = n;
00061   robot_ = robot;
00062 
00063   // Joints
00064 
00065   std::string roll_joint_name;
00066   if (!node_.getParam("roll_joint", roll_joint_name))
00067   {
00068     ROS_ERROR("No roll joint given (namespace: %s)", node_.getNamespace().c_str());
00069     return false;
00070   }
00071   if (!(roll_joint_ = robot->getJointState(roll_joint_name)))
00072   {
00073     ROS_ERROR("Could not find roll joint \"%s\" (namespace: %s)",
00074               roll_joint_name.c_str(), node_.getNamespace().c_str());
00075     return false;
00076   }
00077   if (!roll_joint_->joint_->calibration)
00078   {
00079     ROS_ERROR("Joint \"%s\" has no calibration reference position specified (namespace: %s)",
00080               roll_joint_name.c_str(), node_.getNamespace().c_str());
00081     return false;
00082   }
00083 
00084 
00085   std::string flex_joint_name;
00086   if (!node_.getParam("flex_joint", flex_joint_name))
00087   {
00088     ROS_ERROR("No flex joint given (namespace: %s)", node_.getNamespace().c_str());
00089     return false;
00090   }
00091   if (!(flex_joint_ = robot->getJointState(flex_joint_name)))
00092   {
00093     ROS_ERROR("Could not find flex joint \"%s\" (namespace: %s)",
00094               flex_joint_name.c_str(), node_.getNamespace().c_str());
00095     return false;
00096   }
00097   if (!flex_joint_->joint_->calibration)
00098   {
00099     ROS_ERROR("Joint \"%s\" has no calibration reference position specified (namespace: %s)",
00100               flex_joint_name.c_str(), node_.getNamespace().c_str());
00101     return false;
00102   }
00103 
00104 
00105 
00106 
00107   if (!node_.getParam("roll_velocity", roll_search_velocity_))
00108   {
00109     ROS_ERROR("No roll_velocity given (namespace: %s)", node_.getNamespace().c_str());
00110     return false;
00111   }
00112   // check if calibration fields are supported by this controller
00113   if (!roll_joint_->joint_->calibration->falling && !roll_joint_->joint_->calibration->rising){
00114     ROS_ERROR("No rising or falling edge is specified for calibration of joint %s. Note that the reference_position is not used any more", roll_joint_name.c_str());
00115     return false;
00116   }
00117   if (roll_joint_->joint_->calibration->falling && roll_joint_->joint_->calibration->rising && roll_joint_->joint_->type != urdf::Joint::CONTINUOUS){
00118     ROS_ERROR("Both rising and falling edge are specified for non-continuous joint %s. This is not supported.", roll_joint_name.c_str());
00119     return false;
00120   }
00121   if (roll_search_velocity_ < 0){
00122     roll_search_velocity_ *= -1;
00123     ROS_ERROR("Negative search velocity is not supported for joint %s. Making the search velocity positve.", roll_joint_name.c_str());
00124   }
00125 
00126   // sets reference position
00127   if (roll_joint_->joint_->calibration->falling && roll_joint_->joint_->calibration->rising){
00128     roll_joint_->reference_position_ = *(roll_joint_->joint_->calibration->rising);
00129   }
00130   else if (roll_joint_->joint_->calibration->falling){
00131     roll_joint_->reference_position_ = *(roll_joint_->joint_->calibration->falling);
00132   }
00133   else if (roll_joint_->joint_->calibration->rising){
00134     roll_joint_->reference_position_ = *(roll_joint_->joint_->calibration->rising);
00135   }
00136 
00137 
00138   if (!node_.getParam("flex_velocity", flex_search_velocity_))
00139   {
00140     ROS_ERROR("No flex_velocity given (namespace: %s)", node_.getNamespace().c_str());
00141     return false;
00142   }
00143   // check if calibration fields are supported by this controller
00144   if (!flex_joint_->joint_->calibration->falling && !flex_joint_->joint_->calibration->rising){
00145     ROS_ERROR("No rising or falling edge is specified for calibration of joint %s. Note that the reference_position is not used any more", flex_joint_name.c_str());
00146     return false;
00147   }
00148   if (flex_joint_->joint_->calibration->falling && flex_joint_->joint_->calibration->rising && flex_joint_->joint_->type != urdf::Joint::CONTINUOUS){
00149     ROS_ERROR("Both rising and falling edge are specified for non-continuous joint %s. This is not supported.", flex_joint_name.c_str());
00150     return false;
00151   }
00152   if (flex_search_velocity_ < 0){
00153     flex_search_velocity_ *= -1;
00154     ROS_ERROR("Negative search velocity is not supported for joint %s. Making the search velocity positve.", flex_joint_name.c_str());
00155   }
00156 
00157   // sets reference position
00158   if (flex_joint_->joint_->calibration->falling && flex_joint_->joint_->calibration->rising){
00159     flex_joint_->reference_position_ = *(flex_joint_->joint_->calibration->rising);
00160   }
00161   else if (flex_joint_->joint_->calibration->falling){
00162     flex_joint_->reference_position_ = *(flex_joint_->joint_->calibration->falling);
00163   }
00164   else if (flex_joint_->joint_->calibration->rising){
00165     flex_joint_->reference_position_ = *(flex_joint_->joint_->calibration->rising);
00166   }
00167 
00168   // Actuators
00169   std::string actuator_l_name;
00170   if (!node_.getParam("actuator_l", actuator_l_name))
00171   {
00172     ROS_ERROR("No actuator_l given (namespace: %s)", node_.getNamespace().c_str());
00173     return false;
00174   }
00175   if (!(actuator_l_ = robot->model_->getActuator(actuator_l_name)))
00176   {
00177     ROS_ERROR("Could not find actuator \"%s\" (namespace: %s)",
00178               actuator_l_name.c_str(), node_.getNamespace().c_str());
00179     return false;
00180   }
00181 
00182   std::string actuator_r_name;
00183   if (!node_.getParam("actuator_r", actuator_r_name))
00184   {
00185     ROS_ERROR("No actuator_r given (namespace: %s)", node_.getNamespace().c_str());
00186     return false;
00187   }
00188   if (!(actuator_r_ = robot->model_->getActuator(actuator_r_name)))
00189   {
00190     ROS_ERROR("Could not find actuator \"%s\" (namespace: %s)",
00191               actuator_r_name.c_str(), node_.getNamespace().c_str());
00192     return false;
00193   }
00194   if (actuator_l_->state_.zero_offset_ != 0 && actuator_l_->state_.zero_offset_ != 0){
00195     ROS_INFO("Wrist joints %s and %s are already calibrated", flex_joint_name.c_str(), roll_joint_name.c_str());
00196     flex_joint_->calibrated_ = true;
00197     roll_joint_->calibrated_ = true;
00198     state_ = CALIBRATED;
00199   }
00200   else{
00201     ROS_INFO("Not both wrist joints %s and %s are are calibrated. Will re-calibrate both of them", flex_joint_name.c_str(), roll_joint_name.c_str());
00202     roll_joint_->calibrated_ = false;
00203     flex_joint_->calibrated_ = false;
00204     state_ = INITIALIZED;
00205   }
00206 
00207 
00208 
00209   // Transmission
00210   std::string transmission_name;
00211   if (!node_.getParam("transmission", transmission_name))
00212   {
00213     ROS_ERROR("No transmission given (namespace: %s)", node_.getNamespace().c_str());
00214     return false;
00215   }
00216   if (!(transmission_ = robot->model_->getTransmission(transmission_name)))
00217   {
00218     ROS_ERROR("Could not find transmission \"%s\" (namespace: %s)",
00219               transmission_name.c_str(), node_.getNamespace().c_str());
00220     return false;
00221   }
00222 
00223   // Prepares the namespaces for the velocity controllers
00224 
00225   XmlRpc::XmlRpcValue pid;
00226   node_.getParam("pid", pid);
00227 
00228   ros::NodeHandle roll_node(node_, "roll_velocity");
00229   roll_node.setParam("type", std::string("JointVelocityController"));
00230   roll_node.setParam("joint", roll_joint_name);
00231   roll_node.setParam("pid", pid);
00232 
00233   ros::NodeHandle flex_node(node_, "flex_velocity");
00234   flex_node.setParam("type", std::string("JointVelocityController"));
00235   flex_node.setParam("joint", flex_joint_name);
00236   flex_node.setParam("pid", pid);
00237 
00238   fake_as.push_back(new pr2_hardware_interface::Actuator);
00239   fake_as.push_back(new pr2_hardware_interface::Actuator);
00240   fake_js.push_back(new pr2_mechanism_model::JointState);
00241   fake_js.push_back(new pr2_mechanism_model::JointState);
00242 
00243   const int LEFT_MOTOR = pr2_mechanism_model::WristTransmission::LEFT_MOTOR;
00244   const int RIGHT_MOTOR = pr2_mechanism_model::WristTransmission::RIGHT_MOTOR;
00245   const int FLEX_JOINT = pr2_mechanism_model::WristTransmission::FLEX_JOINT;
00246   const int ROLL_JOINT = pr2_mechanism_model::WristTransmission::ROLL_JOINT;
00247   fake_js[FLEX_JOINT]->joint_ = flex_joint_->joint_;
00248   fake_js[ROLL_JOINT]->joint_ = roll_joint_->joint_;
00249 
00250   if (!vc_roll_.init(robot_, roll_node)) return false;
00251   if (!vc_flex_.init(robot_, flex_node)) return false;
00252 
00253   // advertise service to check calibration
00254   is_calibrated_srv_ = node_.advertiseService("is_calibrated", &WristCalibrationController::isCalibrated, this);
00255 
00256   // "Calibrated" topic
00257   pub_calibrated_.reset(new realtime_tools::RealtimePublisher<std_msgs::Empty>(node_, "calibrated", 1));
00258 
00259   return true;
00260 }
00261 
00262 
00263 void WristCalibrationController::starting()
00264 {
00265   state_ = INITIALIZED;
00266   actuator_r_->state_.zero_offset_ = 0.0;
00267   actuator_l_->state_.zero_offset_ = 0.0;
00268   flex_joint_->calibrated_ = false;
00269   roll_joint_->calibrated_ = false;
00270 }
00271 
00272 
00273 bool WristCalibrationController::isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req,
00274                                               pr2_controllers_msgs::QueryCalibrationState::Response& resp)
00275 {
00276   ROS_DEBUG("Is calibrated service %d", state_ == CALIBRATED);
00277   resp.is_calibrated = (state_ == CALIBRATED);
00278   return true;
00279 }
00280 
00281 
00282 void WristCalibrationController::update()
00283 {
00284   // Flex optical switch is connected to actuator_l
00285   // Roll optical switch is connected to actuator_r
00286 
00287   switch(state_)
00288   {
00289   case INITIALIZED:
00290     actuator_l_->state_.zero_offset_ = 0;
00291     actuator_r_->state_.zero_offset_ = 0;
00292     flex_joint_->calibrated_ = false;
00293     roll_joint_->calibrated_ = false;
00294     vc_flex_.setCommand(0);
00295     vc_roll_.setCommand(0);
00296     state_ = BEGINNING;
00297     break;
00298   case BEGINNING:
00299     vc_roll_.setCommand(0);
00300     // Because of the huge hysteresis on the wrist flex calibration sensor, we want to move towards the
00301     // high side for a long period of time, regardless of which side we start on
00302     countdown_ = 1000;
00303     if (actuator_l_->state_.calibration_reading_)
00304       state_ = MOVING_FLEX_TO_HIGH;
00305     else
00306       state_ = MOVING_FLEX_TO_HIGH;
00307     break;
00308   case MOVING_FLEX_TO_HIGH:
00309     vc_flex_.setCommand(-flex_search_velocity_);
00310 
00311     if (actuator_l_->state_.calibration_reading_)
00312     {
00313       if (--countdown_ <= 0)
00314         state_ = MOVING_FLEX;
00315     }
00316     else
00317       countdown_ = 1000;
00318     break;
00319   case MOVING_FLEX: {
00320     // Calibrates across the falling edge in the positive joint direction.
00321     vc_flex_.setCommand(flex_search_velocity_);
00322     if (actuator_l_->state_.calibration_reading_ == false)
00323     {
00324       flex_switch_l_ = actuator_l_->state_.last_calibration_falling_edge_;
00325 
00326       // But where was actuator_r at the transition?  Unfortunately,
00327       // actuator_r is not connected to the flex joint's optical
00328       // switch, so we don't know directly.  Instead, we estimate
00329       // actuator_r's position based on the switch position of
00330       // actuator_l.
00331       double dl = actuator_l_->state_.position_ - prev_actuator_l_position_;
00332       double dr = actuator_r_->state_.position_ - prev_actuator_r_position_;
00333       double k = (flex_switch_l_ - prev_actuator_l_position_) / dl;
00334       if (dl == 0)
00335       {
00336         // This might be a serious hardware failure, so we're going to break realtime.
00337         ROS_WARN("Left actuator (flex joint) didn't move even though the calibration flag tripped.  "
00338                  "This may indicate an encoder problem. (namespace: %s",
00339                  node_.getNamespace().c_str());
00340         k = 0.5;
00341       }
00342       else if ( !(0 <= k && k <= 1) )
00343       {
00344         // This is really serious, so we're going to break realtime to report it.
00345         ROS_ERROR("k = %.4lf is outside of [0,1].  This probably indicates a hardware failure "
00346                   "on the left actuator or the flex joint.  dl = %.4lf, dr = %.4lf, prev = %.4lf.  "
00347                   "Broke realtime to report (namespace: %s)",
00348                   k, dl, dr, prev_actuator_l_position_, node_.getNamespace().c_str());
00349         state_ = INITIALIZED;
00350         break;
00351       }
00352 
00353       flex_switch_r_ = k * dr + prev_actuator_r_position_;
00354 
00355       //original_switch_state_ = actuator_r_->state_.calibration_reading_;
00356 
00357       // Now we calibrate the roll joint
00358       vc_flex_.setCommand(0);
00359       if (actuator_r_->state_.calibration_reading_)
00360         state_ = MOVING_ROLL_TO_LOW;
00361       else
00362         state_ = MOVING_ROLL;
00363     }
00364     break;
00365   }
00366   case MOVING_ROLL_TO_LOW:
00367     vc_roll_.setCommand(roll_search_velocity_);
00368     if (actuator_r_->state_.calibration_reading_ == false)
00369       state_ = MOVING_ROLL;
00370     break;
00371   case MOVING_ROLL: {
00372     // Calibrates across the rising edge in the positive joint direction.
00373     vc_roll_.setCommand(roll_search_velocity_);
00374     if (actuator_r_->state_.calibration_reading_)
00375     {
00376       roll_switch_r_ = actuator_r_->state_.last_calibration_rising_edge_;
00377 
00378       // See corresponding comment above.
00379       double dl = actuator_l_->state_.position_ - prev_actuator_l_position_;
00380       double dr = actuator_r_->state_.position_ - prev_actuator_r_position_;
00381       double k = (roll_switch_r_ - prev_actuator_r_position_) / dr;
00382       if (dr == 0)
00383       {
00384         // This might be a serious hardware failure, so we're going to break realtime.
00385         ROS_WARN("Right actuator (roll joint) didn't move even though the calibration flag tripped.  "
00386                  "This may indicate an encoder problem. (namespace: %s",
00387                  node_.getNamespace().c_str());
00388         k = 0.5;
00389       }
00390       else if ( !(0 <= k && k <= 1) )
00391       {
00392         // This is really serious, so we're going to break realtime to report it.
00393         ROS_ERROR("k = %.4lf is outside of [0,1].  This probably indicates a hardware failure "
00394                   "on the right actuator or the roll joint.  dl = %.4lf, dr = %.4lf, prev = %.4lf.  "
00395                   "Broke realtime to report (namespace: %s)",
00396                   k, dl, dr, prev_actuator_r_position_, node_.getNamespace().c_str());
00397         state_ = INITIALIZED;
00398         break;
00399       }
00400       roll_switch_l_ =  k * dl + prev_actuator_l_position_;
00401 
00402 
00403       //----------------------------------------------------------------------
00404       //       Calibration computation
00405       //----------------------------------------------------------------------
00406 
00407       // At this point, we know the actuator positions when the
00408       // optical switches were hit.  Now we compute the actuator
00409       // positions when the joints should be at 0.
00410 
00411       const int LEFT_MOTOR = pr2_mechanism_model::WristTransmission::LEFT_MOTOR;
00412       const int RIGHT_MOTOR = pr2_mechanism_model::WristTransmission::RIGHT_MOTOR;
00413       const int FLEX_JOINT = pr2_mechanism_model::WristTransmission::FLEX_JOINT;
00414       const int ROLL_JOINT = pr2_mechanism_model::WristTransmission::ROLL_JOINT;
00415 
00416       // Finds the (uncalibrated) joint position where the flex optical switch triggers
00417       fake_as[LEFT_MOTOR]->state_.position_ = flex_switch_l_;
00418       fake_as[RIGHT_MOTOR]->state_.position_ = flex_switch_r_;
00419       transmission_->propagatePosition(fake_as, fake_js);
00420       double flex_joint_switch = fake_js[FLEX_JOINT]->position_;
00421 
00422       // Finds the (uncalibrated) joint position where the roll optical switch triggers
00423       fake_as[LEFT_MOTOR]->state_.position_ = roll_switch_l_;
00424       fake_as[RIGHT_MOTOR]->state_.position_ = roll_switch_r_;
00425       transmission_->propagatePosition(fake_as, fake_js);
00426       double roll_joint_switch = fake_js[ROLL_JOINT]->position_;
00427 
00428       // Finds the (uncalibrated) joint position at the desired zero
00429       fake_js[FLEX_JOINT]->position_ = flex_joint_switch;
00430       fake_js[ROLL_JOINT]->position_ = roll_joint_switch;
00431 
00432       // Determines the actuator zero position from the desired joint zero positions
00433       transmission_->propagatePositionBackwards(fake_js, fake_as);
00434       if (isnan(fake_as[LEFT_MOTOR]->state_.position_) ||
00435           isnan(fake_as[RIGHT_MOTOR]->state_.position_))
00436       {
00437         ROS_ERROR("Restarting calibration because a computed offset was NaN. If this happens "
00438                   "repeatedly it may indicate a hardware failure.  (namespace: %s)",
00439                   node_.getNamespace().c_str());
00440         state_ = INITIALIZED;
00441         break;
00442       }
00443       actuator_l_->state_.zero_offset_ = fake_as[LEFT_MOTOR]->state_.position_;
00444       actuator_r_->state_.zero_offset_ = fake_as[RIGHT_MOTOR]->state_.position_;
00445 
00446       flex_joint_->calibrated_ = true;
00447       roll_joint_->calibrated_ = true;
00448       state_ = CALIBRATED;
00449 
00450       vc_flex_.setCommand(0);
00451       vc_roll_.setCommand(0);
00452     }
00453 
00454     break;
00455   }
00456   case CALIBRATED:
00457     if (pub_calibrated_) {
00458       if (last_publish_time_ + ros::Duration(0.5) < robot_->getTime()) {
00459         assert(pub_calibrated_);
00460         if (pub_calibrated_->trylock()) {
00461           last_publish_time_ = robot_->getTime();
00462           pub_calibrated_->unlockAndPublish();
00463         }
00464       }
00465       }
00466     break;
00467   }
00468 
00469   if (state_ != CALIBRATED)
00470   {
00471     vc_flex_.update();
00472     vc_roll_.update();
00473   }
00474 
00475   prev_actuator_l_position_ = actuator_l_->state_.position_;
00476   prev_actuator_r_position_ = actuator_r_->state_.position_;
00477 }
00478 }
00479
wrist_calibration_controller.cpp
