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


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