wrist_calibration_controller.cpp
Go to the documentation of this file.
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_EXPORT_CLASS(controller::WristCalibrationController, pr2_controller_interface::Controller)
00038 
00039 namespace controller {
00040 
00041 
00042 WristCalibrationController::WristCalibrationController()
00043 : robot_(NULL), last_publish_time_(0)
00044 {
00045 }
00046 
00047 WristCalibrationController::~WristCalibrationController()
00048 {
00049   for (size_t i = 0; i < fake_as.size(); ++i)
00050     delete fake_as[i];
00051   for (size_t i = 0; i < fake_js.size(); ++i)
00052     delete fake_js[i];
00053 }
00054 
00055 bool WristCalibrationController::init(pr2_mechanism_model::RobotState *robot,
00056                                       ros::NodeHandle &n)
00057 {
00058   assert(robot);
00059   node_ = n;
00060   robot_ = robot;
00061 
00062   // Joints
00063 
00064   std::string roll_joint_name;
00065   if (!node_.getParam("roll_joint", roll_joint_name))
00066   {
00067     ROS_ERROR("No roll joint given (namespace: %s)", node_.getNamespace().c_str());
00068     return false;
00069   }
00070   if (!(roll_joint_ = robot->getJointState(roll_joint_name)))
00071   {
00072     ROS_ERROR("Could not find roll joint \"%s\" (namespace: %s)",
00073               roll_joint_name.c_str(), node_.getNamespace().c_str());
00074     return false;
00075   }
00076   if (!roll_joint_->joint_->calibration)
00077   {
00078     ROS_ERROR("Joint \"%s\" has no calibration reference position specified (namespace: %s)",
00079               roll_joint_name.c_str(), node_.getNamespace().c_str());
00080     return false;
00081   }
00082 
00083 
00084   std::string flex_joint_name;
00085   if (!node_.getParam("flex_joint", flex_joint_name))
00086   {
00087     ROS_ERROR("No flex joint given (namespace: %s)", node_.getNamespace().c_str());
00088     return false;
00089   }
00090   if (!(flex_joint_ = robot->getJointState(flex_joint_name)))
00091   {
00092     ROS_ERROR("Could not find flex joint \"%s\" (namespace: %s)",
00093               flex_joint_name.c_str(), node_.getNamespace().c_str());
00094     return false;
00095   }
00096   if (!flex_joint_->joint_->calibration)
00097   {
00098     ROS_ERROR("Joint \"%s\" has no calibration reference position specified (namespace: %s)",
00099               flex_joint_name.c_str(), node_.getNamespace().c_str());
00100     return false;
00101   }
00102 
00103 
00104 
00105 
00106   if (!node_.getParam("roll_velocity", roll_search_velocity_))
00107   {
00108     ROS_ERROR("No roll_velocity given (namespace: %s)", node_.getNamespace().c_str());
00109     return false;
00110   }
00111   // check if calibration fields are supported by this controller
00112   if (!roll_joint_->joint_->calibration->falling && !roll_joint_->joint_->calibration->rising){
00113     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());
00114     return false;
00115   }
00116   if (roll_joint_->joint_->calibration->falling && roll_joint_->joint_->calibration->rising && roll_joint_->joint_->type != urdf::Joint::CONTINUOUS){
00117     ROS_ERROR("Both rising and falling edge are specified for non-continuous joint %s. This is not supported.", roll_joint_name.c_str());
00118     return false;
00119   }
00120   if (roll_search_velocity_ < 0){
00121     roll_search_velocity_ *= -1;
00122     ROS_ERROR("Negative search velocity is not supported for joint %s. Making the search velocity positve.", roll_joint_name.c_str());
00123   }
00124 
00125   // sets reference position
00126   if (roll_joint_->joint_->calibration->falling && roll_joint_->joint_->calibration->rising){
00127     roll_joint_->reference_position_ = *(roll_joint_->joint_->calibration->rising);
00128   }
00129   else if (roll_joint_->joint_->calibration->falling){
00130     roll_joint_->reference_position_ = *(roll_joint_->joint_->calibration->falling);
00131   }
00132   else if (roll_joint_->joint_->calibration->rising){
00133     roll_joint_->reference_position_ = *(roll_joint_->joint_->calibration->rising);
00134   }
00135 
00136 
00137   if (!node_.getParam("flex_velocity", flex_search_velocity_))
00138   {
00139     ROS_ERROR("No flex_velocity given (namespace: %s)", node_.getNamespace().c_str());
00140     return false;
00141   }
00142   // check if calibration fields are supported by this controller
00143   if (!flex_joint_->joint_->calibration->falling && !flex_joint_->joint_->calibration->rising){
00144     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());
00145     return false;
00146   }
00147   if (flex_joint_->joint_->calibration->falling && flex_joint_->joint_->calibration->rising && flex_joint_->joint_->type != urdf::Joint::CONTINUOUS){
00148     ROS_ERROR("Both rising and falling edge are specified for non-continuous joint %s. This is not supported.", flex_joint_name.c_str());
00149     return false;
00150   }
00151   if (flex_search_velocity_ < 0){
00152     flex_search_velocity_ *= -1;
00153     ROS_ERROR("Negative search velocity is not supported for joint %s. Making the search velocity positve.", flex_joint_name.c_str());
00154   }
00155 
00156   // sets reference position
00157   if (flex_joint_->joint_->calibration->falling && flex_joint_->joint_->calibration->rising){
00158     flex_joint_->reference_position_ = *(flex_joint_->joint_->calibration->rising);
00159   }
00160   else if (flex_joint_->joint_->calibration->falling){
00161     flex_joint_->reference_position_ = *(flex_joint_->joint_->calibration->falling);
00162   }
00163   else if (flex_joint_->joint_->calibration->rising){
00164     flex_joint_->reference_position_ = *(flex_joint_->joint_->calibration->rising);
00165   }
00166 
00167   // Actuators
00168   std::string actuator_l_name;
00169   if (!node_.getParam("actuator_l", actuator_l_name))
00170   {
00171     ROS_ERROR("No actuator_l given (namespace: %s)", node_.getNamespace().c_str());
00172     return false;
00173   }
00174   if (!(actuator_l_ = robot->model_->getActuator(actuator_l_name)))
00175   {
00176     ROS_ERROR("Could not find actuator \"%s\" (namespace: %s)",
00177               actuator_l_name.c_str(), node_.getNamespace().c_str());
00178     return false;
00179   }
00180 
00181   std::string actuator_r_name;
00182   if (!node_.getParam("actuator_r", actuator_r_name))
00183   {
00184     ROS_ERROR("No actuator_r given (namespace: %s)", node_.getNamespace().c_str());
00185     return false;
00186   }
00187   if (!(actuator_r_ = robot->model_->getActuator(actuator_r_name)))
00188   {
00189     ROS_ERROR("Could not find actuator \"%s\" (namespace: %s)",
00190               actuator_r_name.c_str(), node_.getNamespace().c_str());
00191     return false;
00192   }
00193 
00194   bool force_calibration = false;
00195   node_.getParam("force_calibration", force_calibration);
00196 
00197   roll_joint_->calibrated_ = false;
00198   flex_joint_->calibrated_ = false;
00199   state_ = INITIALIZED;
00200   if (actuator_l_->state_.zero_offset_ != 0 && actuator_l_->state_.zero_offset_ != 0){
00201     if (force_calibration)
00202     {
00203       ROS_INFO("Joints %s and %s are already calibrated but will be recalibrated. "
00204                "Actuator %s was zeroed at %f and %s was zeroed at %f.", 
00205                flex_joint_name.c_str(), roll_joint_name.c_str(),
00206                actuator_r_->name_.c_str(), actuator_r_->state_.zero_offset_,
00207                actuator_l_->name_.c_str(), actuator_l_->state_.zero_offset_
00208                );
00209     }
00210     else 
00211     {
00212       ROS_INFO("Wrist joints %s and %s are already calibrated", flex_joint_name.c_str(), roll_joint_name.c_str());
00213       flex_joint_->calibrated_ = true;
00214       roll_joint_->calibrated_ = true;
00215       state_ = CALIBRATED;
00216     }
00217   }
00218   else{
00219     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());
00220   }
00221 
00222 
00223 
00224   // Transmission
00225   std::string transmission_name;
00226   if (!node_.getParam("transmission", transmission_name))
00227   {
00228     ROS_ERROR("No transmission given (namespace: %s)", node_.getNamespace().c_str());
00229     return false;
00230   }
00231   if (!(transmission_ = robot->model_->getTransmission(transmission_name)))
00232   {
00233     ROS_ERROR("Could not find transmission \"%s\" (namespace: %s)",
00234               transmission_name.c_str(), node_.getNamespace().c_str());
00235     return false;
00236   }
00237 
00238   // Prepares the namespaces for the velocity controllers
00239 
00240   XmlRpc::XmlRpcValue pid;
00241   node_.getParam("pid", pid);
00242 
00243   ros::NodeHandle roll_node(node_, "roll_velocity");
00244   roll_node.setParam("type", std::string("JointVelocityController"));
00245   roll_node.setParam("joint", roll_joint_name);
00246   roll_node.setParam("pid", pid);
00247 
00248   ros::NodeHandle flex_node(node_, "flex_velocity");
00249   flex_node.setParam("type", std::string("JointVelocityController"));
00250   flex_node.setParam("joint", flex_joint_name);
00251   flex_node.setParam("pid", pid);
00252 
00253   fake_as.push_back(new pr2_hardware_interface::Actuator);
00254   fake_as.push_back(new pr2_hardware_interface::Actuator);
00255   fake_js.push_back(new pr2_mechanism_model::JointState);
00256   fake_js.push_back(new pr2_mechanism_model::JointState);
00257 
00258   const int LEFT_MOTOR = pr2_mechanism_model::WristTransmission::LEFT_MOTOR;
00259   const int RIGHT_MOTOR = pr2_mechanism_model::WristTransmission::RIGHT_MOTOR;
00260   const int FLEX_JOINT = pr2_mechanism_model::WristTransmission::FLEX_JOINT;
00261   const int ROLL_JOINT = pr2_mechanism_model::WristTransmission::ROLL_JOINT;
00262   fake_js[FLEX_JOINT]->joint_ = flex_joint_->joint_;
00263   fake_js[ROLL_JOINT]->joint_ = roll_joint_->joint_;
00264 
00265   if (!vc_roll_.init(robot_, roll_node)) return false;
00266   if (!vc_flex_.init(robot_, flex_node)) return false;
00267 
00268   // advertise service to check calibration
00269   is_calibrated_srv_ = node_.advertiseService("is_calibrated", &WristCalibrationController::isCalibrated, this);
00270 
00271   // "Calibrated" topic
00272   pub_calibrated_.reset(new realtime_tools::RealtimePublisher<std_msgs::Empty>(node_, "calibrated", 1));
00273 
00274   return true;
00275 }
00276 
00277 
00278 void WristCalibrationController::starting()
00279 {
00280   state_ = INITIALIZED;
00281   actuator_r_->state_.zero_offset_ = 0.0;
00282   actuator_l_->state_.zero_offset_ = 0.0;
00283   flex_joint_->calibrated_ = false;
00284   roll_joint_->calibrated_ = false;
00285 }
00286 
00287 
00288 bool WristCalibrationController::isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req,
00289                                               pr2_controllers_msgs::QueryCalibrationState::Response& resp)
00290 {
00291   ROS_DEBUG("Is calibrated service %d", state_ == CALIBRATED);
00292   resp.is_calibrated = (state_ == CALIBRATED);
00293   return true;
00294 }
00295 
00296 
00297 void WristCalibrationController::update()
00298 {
00299   // Flex optical switch is connected to actuator_l
00300   // Roll optical switch is connected to actuator_r
00301 
00302   switch(state_)
00303   {
00304   case INITIALIZED:
00305     actuator_l_->state_.zero_offset_ = 0;
00306     actuator_r_->state_.zero_offset_ = 0;
00307     flex_joint_->calibrated_ = false;
00308     roll_joint_->calibrated_ = false;
00309     vc_flex_.setCommand(0);
00310     vc_roll_.setCommand(0);
00311     state_ = BEGINNING;
00312     break;
00313   case BEGINNING:
00314     vc_roll_.setCommand(0);
00315     // Because of the huge hysteresis on the wrist flex calibration sensor, we want to move towards the
00316     // high side for a long period of time, regardless of which side we start on
00317     countdown_ = 1000;
00318     if (actuator_l_->state_.calibration_reading_)
00319       state_ = MOVING_FLEX_TO_HIGH;
00320     else
00321       state_ = MOVING_FLEX_TO_HIGH;
00322     break;
00323   case MOVING_FLEX_TO_HIGH:
00324     vc_flex_.setCommand(-flex_search_velocity_);
00325 
00326     if (actuator_l_->state_.calibration_reading_)
00327     {
00328       if (--countdown_ <= 0)
00329         state_ = MOVING_FLEX;
00330     }
00331     else
00332       countdown_ = 1000;
00333     break;
00334   case MOVING_FLEX: {
00335     // Calibrates across the falling edge in the positive joint direction.
00336     vc_flex_.setCommand(flex_search_velocity_);
00337     if (actuator_l_->state_.calibration_reading_ == false)
00338     {
00339       flex_switch_l_ = actuator_l_->state_.last_calibration_falling_edge_;
00340 
00341       // But where was actuator_r at the transition?  Unfortunately,
00342       // actuator_r is not connected to the flex joint's optical
00343       // switch, so we don't know directly.  Instead, we estimate
00344       // actuator_r's position based on the switch position of
00345       // actuator_l.
00346       double dl = actuator_l_->state_.position_ - prev_actuator_l_position_;
00347       double dr = actuator_r_->state_.position_ - prev_actuator_r_position_;
00348       double k = (flex_switch_l_ - prev_actuator_l_position_) / dl;
00349       if (dl == 0)
00350       {
00351         // This might be a serious hardware failure, so we're going to break realtime.
00352         ROS_WARN("Left actuator (flex joint) didn't move even though the calibration flag tripped.  "
00353                  "This may indicate an encoder problem. (namespace: %s",
00354                  node_.getNamespace().c_str());
00355         k = 0.5;
00356       }
00357       else if ( !(0 <= k && k <= 1) )
00358       {
00359         // This is really serious, so we're going to break realtime to report it.
00360         ROS_ERROR("k = %.4lf is outside of [0,1].  This probably indicates a hardware failure "
00361                   "on the left actuator or the flex joint.  dl = %.4lf, dr = %.4lf, prev = %.4lf.  "
00362                   "Broke realtime to report (namespace: %s)",
00363                   k, dl, dr, prev_actuator_l_position_, node_.getNamespace().c_str());
00364         state_ = INITIALIZED;
00365         break;
00366       }
00367 
00368       flex_switch_r_ = k * dr + prev_actuator_r_position_;
00369 
00370       //original_switch_state_ = actuator_r_->state_.calibration_reading_;
00371 
00372       // Now we calibrate the roll joint
00373       vc_flex_.setCommand(0);
00374       if (actuator_r_->state_.calibration_reading_)
00375         state_ = MOVING_ROLL_TO_LOW;
00376       else
00377         state_ = MOVING_ROLL;
00378     }
00379     break;
00380   }
00381   case MOVING_ROLL_TO_LOW:
00382     vc_roll_.setCommand(roll_search_velocity_);
00383     if (actuator_r_->state_.calibration_reading_ == false)
00384       state_ = MOVING_ROLL;
00385     break;
00386   case MOVING_ROLL: {
00387     // Calibrates across the rising edge in the positive joint direction.
00388     vc_roll_.setCommand(roll_search_velocity_);
00389     if (actuator_r_->state_.calibration_reading_)
00390     {
00391       roll_switch_r_ = actuator_r_->state_.last_calibration_rising_edge_;
00392 
00393       // See corresponding comment above.
00394       double dl = actuator_l_->state_.position_ - prev_actuator_l_position_;
00395       double dr = actuator_r_->state_.position_ - prev_actuator_r_position_;
00396       double k = (roll_switch_r_ - prev_actuator_r_position_) / dr;
00397       if (dr == 0)
00398       {
00399         // This might be a serious hardware failure, so we're going to break realtime.
00400         ROS_WARN("Right actuator (roll joint) didn't move even though the calibration flag tripped.  "
00401                  "This may indicate an encoder problem. (namespace: %s",
00402                  node_.getNamespace().c_str());
00403         k = 0.5;
00404       }
00405       else if ( !(0 <= k && k <= 1) )
00406       {
00407         // This is really serious, so we're going to break realtime to report it.
00408         ROS_ERROR("k = %.4lf is outside of [0,1].  This probably indicates a hardware failure "
00409                   "on the right actuator or the roll joint.  dl = %.4lf, dr = %.4lf, prev = %.4lf.  "
00410                   "Broke realtime to report (namespace: %s)",
00411                   k, dl, dr, prev_actuator_r_position_, node_.getNamespace().c_str());
00412         state_ = INITIALIZED;
00413         break;
00414       }
00415       roll_switch_l_ =  k * dl + prev_actuator_l_position_;
00416 
00417 
00418       //----------------------------------------------------------------------
00419       //       Calibration computation
00420       //----------------------------------------------------------------------
00421 
00422       // At this point, we know the actuator positions when the
00423       // optical switches were hit.  Now we compute the actuator
00424       // positions when the joints should be at 0.
00425 
00426       const int LEFT_MOTOR = pr2_mechanism_model::WristTransmission::LEFT_MOTOR;
00427       const int RIGHT_MOTOR = pr2_mechanism_model::WristTransmission::RIGHT_MOTOR;
00428       const int FLEX_JOINT = pr2_mechanism_model::WristTransmission::FLEX_JOINT;
00429       const int ROLL_JOINT = pr2_mechanism_model::WristTransmission::ROLL_JOINT;
00430 
00431       // Finds the (uncalibrated) joint position where the flex optical switch triggers
00432       fake_as[LEFT_MOTOR]->state_.position_ = flex_switch_l_;
00433       fake_as[RIGHT_MOTOR]->state_.position_ = flex_switch_r_;
00434       transmission_->propagatePosition(fake_as, fake_js);
00435       double flex_joint_switch = fake_js[FLEX_JOINT]->position_;
00436 
00437       // Finds the (uncalibrated) joint position where the roll optical switch triggers
00438       fake_as[LEFT_MOTOR]->state_.position_ = roll_switch_l_;
00439       fake_as[RIGHT_MOTOR]->state_.position_ = roll_switch_r_;
00440       transmission_->propagatePosition(fake_as, fake_js);
00441       double roll_joint_switch = fake_js[ROLL_JOINT]->position_;
00442 
00443       // Finds the (uncalibrated) joint position at the desired zero
00444       fake_js[FLEX_JOINT]->position_ = flex_joint_switch;
00445       fake_js[ROLL_JOINT]->position_ = roll_joint_switch;
00446 
00447       // Determines the actuator zero position from the desired joint zero positions
00448       transmission_->propagatePositionBackwards(fake_js, fake_as);
00449       if (isnan(fake_as[LEFT_MOTOR]->state_.position_) ||
00450           isnan(fake_as[RIGHT_MOTOR]->state_.position_))
00451       {
00452         ROS_ERROR("Restarting calibration because a computed offset was NaN. If this happens "
00453                   "repeatedly it may indicate a hardware failure.  (namespace: %s)",
00454                   node_.getNamespace().c_str());
00455         state_ = INITIALIZED;
00456         break;
00457       }
00458       actuator_l_->state_.zero_offset_ = fake_as[LEFT_MOTOR]->state_.position_;
00459       actuator_r_->state_.zero_offset_ = fake_as[RIGHT_MOTOR]->state_.position_;
00460 
00461       flex_joint_->calibrated_ = true;
00462       roll_joint_->calibrated_ = true;
00463       state_ = CALIBRATED;
00464 
00465       vc_flex_.setCommand(0);
00466       vc_roll_.setCommand(0);
00467     }
00468 
00469     break;
00470   }
00471   case CALIBRATED:
00472     if (pub_calibrated_) {
00473       if (last_publish_time_ + ros::Duration(0.5) < robot_->getTime()) {
00474         assert(pub_calibrated_);
00475         if (pub_calibrated_->trylock()) {
00476           last_publish_time_ = robot_->getTime();
00477           pub_calibrated_->unlockAndPublish();
00478         }
00479       }
00480       }
00481     break;
00482   }
00483 
00484   if (state_ != CALIBRATED)
00485   {
00486     vc_flex_.update();
00487     vc_roll_.update();
00488   }
00489 
00490   prev_actuator_l_position_ = actuator_l_->state_.position_;
00491   prev_actuator_r_position_ = actuator_r_->state_.position_;
00492 }
00493 }
00494 


pr2_calibration_controllers
Author(s): Stuart Glaser
autogenerated on Thu Aug 27 2015 14:23:03