pr2_calibration_controllers: pr2_calibration_controllers: joint_calibration

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_calibration_controllers/joint_calibration_controller.h"
00036 #include "ros/time.h"
00037 #include "pluginlib/class_list_macros.h"
00038 
00039 PLUGINLIB_DECLARE_CLASS(pr2_calibration_controllers, JointCalibrationController,
00040                         controller::JointCalibrationController, pr2_controller_interface::Controller)
00041 
00042 using namespace std;
00043 
00044 namespace controller {
00045 
00046 JointCalibrationController::JointCalibrationController()
00047 : robot_(NULL), last_publish_time_(0),
00048   actuator_(NULL), joint_(NULL), transmission_(NULL)
00049 {
00050 }
00051 
00052 JointCalibrationController::~JointCalibrationController()
00053 {
00054 }
00055 
00056 bool JointCalibrationController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00057 {
00058   robot_ = robot;
00059   node_ = n;
00060 
00061   // Joint
00062   std::string joint_name;
00063   if (!node_.getParam("joint", joint_name))
00064   {
00065     ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00066     return false;
00067   }
00068   if (!(joint_ = robot->getJointState(joint_name)))
00069   {
00070     ROS_ERROR("Could not find joint %s (namespace: %s)",
00071               joint_name.c_str(), node_.getNamespace().c_str());
00072     return false;
00073   }
00074   if (!joint_->joint_->calibration)
00075   {
00076     ROS_ERROR("Joint %s has no calibration reference position specified (namespace: %s)",
00077               joint_name.c_str(), node_.getNamespace().c_str());
00078     return false;
00079   }
00080 
00081   // Actuator
00082   std::string actuator_name;
00083   if (!node_.getParam("actuator", actuator_name))
00084   {
00085     ROS_ERROR("No actuator given (namespace: %s)", node_.getNamespace().c_str());
00086     return false;
00087   }
00088   if (!(actuator_ = robot->model_->getActuator(actuator_name)))
00089   {
00090     ROS_ERROR("Could not find actuator %s (namespace: %s)",
00091               actuator_name.c_str(), node_.getNamespace().c_str());
00092     return false;
00093   }
00094   if (actuator_->state_.zero_offset_ != 0){
00095     ROS_INFO("Joint %s is already calibrated at offset %f", joint_name.c_str(), actuator_->state_.zero_offset_);
00096     state_ = CALIBRATED;
00097     joint_->calibrated_ = true;
00098   }
00099   else{
00100     ROS_INFO("Joint %s is not yet calibrated", joint_name.c_str());
00101     state_ = INITIALIZED;
00102     joint_->calibrated_ = false;
00103   }
00104 
00105   // Transmission
00106   std::string transmission_name;
00107   if (!node_.getParam("transmission", transmission_name))
00108   {
00109     ROS_ERROR("No transmission given (namespace: %s)", node_.getNamespace().c_str());
00110     return false;
00111   }
00112   if (!(transmission_ = robot->model_->getTransmission(transmission_name)))
00113   {
00114     ROS_ERROR("Could not find transmission %s (namespace: %s)",
00115               transmission_name.c_str(), node_.getNamespace().c_str());
00116     return false;
00117   }
00118 
00119   if (!node_.getParam("velocity", search_velocity_))
00120   {
00121     ROS_ERROR("Velocity value was not specified (namespace: %s)", node_.getNamespace().c_str());
00122     return false;
00123   }
00124 
00125   // check if calibration fields are supported by this controller
00126   if (!joint_->joint_->calibration->falling && !joint_->joint_->calibration->rising){
00127     ROS_ERROR("No rising or falling edge is specified for calibration of joint %s. Note that the reference_position is not used any more", joint_name.c_str());
00128     return false;
00129   }
00130   if (joint_->joint_->calibration->falling && joint_->joint_->calibration->rising && joint_->joint_->type != urdf::Joint::CONTINUOUS){
00131     ROS_ERROR("Both rising and falling edge are specified for non-continuous joint %s. This is not supported.", joint_name.c_str());
00132     return false;
00133   }
00134   if (search_velocity_ < 0){
00135     search_velocity_ *= -1;
00136     ROS_ERROR("Negative search velocity is not supported for joint %s. Making the search velocity positive.", joint_name.c_str());
00137   }
00138 
00139   // finds search velocity based on rising or falling edge
00140   if (joint_->joint_->calibration->falling && joint_->joint_->calibration->rising){
00141     joint_->reference_position_ = *(joint_->joint_->calibration->rising);
00142     ROS_DEBUG("Using positive search velocity for joint %s", joint_name.c_str());
00143   }
00144   else if (joint_->joint_->calibration->falling){
00145     joint_->reference_position_ = *(joint_->joint_->calibration->falling);
00146     search_velocity_ *= -1.0;
00147     ROS_DEBUG("Using negative search velocity for joint %s", joint_name.c_str());
00148   }
00149   else if (joint_->joint_->calibration->rising){
00150     joint_->reference_position_ = *(joint_->joint_->calibration->rising);
00151     ROS_DEBUG("Using positive search velocity for joint %s", joint_name.c_str());
00152   }
00153 
00154 
00155   // Contained velocity controller
00156   if (!vc_.init(robot, node_))
00157     return false;
00158 
00159   // advertise service to check calibration
00160   is_calibrated_srv_ = node_.advertiseService("is_calibrated", &JointCalibrationController::isCalibrated, this);
00161 
00162   // "Calibrated" topic
00163   pub_calibrated_.reset(new realtime_tools::RealtimePublisher<std_msgs::Empty>(node_, "calibrated", 1));
00164 
00165 
00166   return true;
00167 }
00168 
00169 
00170 void JointCalibrationController::starting()
00171 {
00172   state_ = INITIALIZED;
00173   joint_->calibrated_ = false;
00174   actuator_->state_.zero_offset_ = 0.0;
00175 }
00176 
00177 
00178 bool JointCalibrationController::isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req,
00179                                               pr2_controllers_msgs::QueryCalibrationState::Response& resp)
00180 {
00181   ROS_DEBUG("Is calibrated service %d", state_ == CALIBRATED);
00182   resp.is_calibrated = (state_ == CALIBRATED);
00183   return true;
00184 }
00185 
00186 
00187 void JointCalibrationController::update()
00188 {
00189   assert(joint_);
00190   assert(actuator_);
00191 
00192   switch(state_)
00193   {
00194   case INITIALIZED:
00195     vc_.setCommand(0.0);
00196     state_ = BEGINNING;
00197     break;
00198   case BEGINNING:
00199     if (actuator_->state_.calibration_reading_ & 1)
00200       state_ = MOVING_TO_LOW;
00201     else{
00202       state_ = MOVING_TO_HIGH;
00203       original_position_ = joint_->position_;
00204     }
00205     break;
00206   case MOVING_TO_LOW:
00207     vc_.setCommand(-search_velocity_);
00208     if (!(actuator_->state_.calibration_reading_ & 1))
00209     {
00210       if (--countdown_ <= 0){
00211         state_ = MOVING_TO_HIGH;
00212         original_position_ = joint_->position_;
00213       }
00214     }
00215     else
00216       countdown_ = 200;
00217     break;
00218   case MOVING_TO_HIGH: {
00219     vc_.setCommand(search_velocity_);
00220 
00221     if (actuator_->state_.calibration_reading_ & 1)
00222     {
00223       // detect when we hit the wrong transition because someone pushed the joint during calibration
00224       if ((search_velocity_ > 0.0 && (joint_->position_ - original_position_) < 0) ||
00225           (search_velocity_ < 0.0 && (joint_->position_ - original_position_) > 0))
00226         {
00227           state_ = BEGINNING;
00228           ROS_ERROR("Joint hit the falling edge instead of the rising edge. Calibrating again...");
00229           ros::Duration(1.0).sleep();  // give joint some time to move away from transition
00230           break;
00231         }
00232 
00233       pr2_hardware_interface::Actuator a;
00234       pr2_mechanism_model::JointState j;
00235 
00236       // store position of flag in actuator 
00237       actuator_->state_.zero_offset_ = actuator_->state_.last_calibration_rising_edge_;
00238 
00239       // mark the joint as calibrated
00240       joint_->calibrated_ = true;
00241 
00242       state_ = CALIBRATED;
00243       vc_.setCommand(0.0);
00244     }
00245     break;
00246   }
00247   case CALIBRATED:
00248     if (pub_calibrated_) {
00249       if (last_publish_time_ + ros::Duration(0.5) < robot_->getTime()){
00250         assert(pub_calibrated_);
00251         if (pub_calibrated_->trylock()) {
00252           last_publish_time_ = robot_->getTime();
00253           pub_calibrated_->unlockAndPublish();
00254         }
00255       }
00256     }
00257     break;
00258   }
00259 
00260   if (state_ != CALIBRATED)
00261     vc_.update();
00262 }
00263 } // namespace
joint_calibration_controller.cpp
