joint_qualification_controllers: joint_qualification_controllers: hysteresis

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 "joint_qualification_controllers/hysteresis_controller.h"
00036 #include "pluginlib/class_list_macros.h"
00037 #include "urdf/joint.h"
00038 
00039 PLUGINLIB_DECLARE_CLASS(joint_qualification_controllers, HysteresisController, 
00040                         joint_qualification_controllers::HysteresisController, 
00041                         pr2_controller_interface::Controller)
00042 
00043 #define MAX_DATA_POINTS 120000
00044 
00045 using namespace std;
00046 using namespace joint_qualification_controllers;
00047 
00048 HysteresisController::HysteresisController() : 
00049   joint_(NULL),
00050   robot_(NULL),
00051   data_sent_(false),
00052   hyst_pub_(NULL)
00053 {
00054   test_data_.joint_name = "default joint";
00055   test_data_.time_up.resize(MAX_DATA_POINTS);
00056   test_data_.effort_up.resize(MAX_DATA_POINTS);
00057   test_data_.position_up.resize(MAX_DATA_POINTS);
00058   test_data_.velocity_up.resize(MAX_DATA_POINTS);
00059 
00060   test_data_.time_down.resize(MAX_DATA_POINTS);
00061   test_data_.effort_down.resize(MAX_DATA_POINTS);
00062   test_data_.position_down.resize(MAX_DATA_POINTS);
00063   test_data_.velocity_down.resize(MAX_DATA_POINTS);
00064 
00065   test_data_.arg_name.resize(14);
00066   test_data_.arg_value.resize(14);
00067   test_data_.arg_name[0] = "Min. Expected Effort";
00068   test_data_.arg_name[1] = "Max. Expected Effort";
00069   test_data_.arg_name[2] = "Minimum Position";
00070   test_data_.arg_name[3] = "Maximum Position";
00071   test_data_.arg_name[4] = "Velocity";
00072   test_data_.arg_name[5] = "Timeout";
00073   test_data_.arg_name[6] = "Max. Allowed Effort";
00074   test_data_.arg_name[7] = "Tolerance";
00075   test_data_.arg_name[8] = "SD Max";
00076   test_data_.arg_name[9] = "Slope";
00077   test_data_.arg_name[10] = "P Gain";
00078   test_data_.arg_name[11] = "I Gain";
00079   test_data_.arg_name[12] = "D Gain";
00080   test_data_.arg_name[13] = "I-Clamp";
00081 
00082   state_         = STOPPED;
00083   starting_count_ = 0;
00084   velocity_      = 0;
00085   initial_time_  = ros::Time(0);
00086   max_effort_    = 0;
00087   complete       = false;
00088   up_count_      = 0;
00089   down_count_    = 0;
00090 }
00091 
00092 HysteresisController::~HysteresisController()
00093 {
00094   if (velocity_controller_) delete velocity_controller_;
00095 }
00096 
00097 bool HysteresisController::init( pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00098 {
00099   assert(robot);
00100   robot_ = robot;
00101 
00102   std::string name;
00103   if (!n.getParam("velocity_controller/joint", name)){
00104     ROS_ERROR("Hysteresis Controller: No joint name found on parameter namespace: %s)",
00105               n.getNamespace().c_str());
00106     return false;
00107   }
00108   if (!(joint_ = robot->getJointState(name)))
00109   {
00110     ROS_ERROR("HysteresisController could not find joint named \"%s\"\n", name.c_str());
00111     return false;
00112   }
00113 
00114   if (!n.getParam("velocity", velocity_)){
00115     ROS_ERROR("Hysteresis Controller: No velocity found on parameter namespace: %s)",
00116               n.getNamespace().c_str());
00117     return false;
00118   }
00119   velocity_ = velocity_ > 0 ? velocity_ : -1.0 * velocity_;
00120 
00121   if (!n.getParam("max_effort", max_effort_)){
00122     ROS_ERROR("Hysteresis Controller: No max effort found on parameter namespace: %s)",
00123               n.getNamespace().c_str());
00124     return false;
00125   }
00126 
00127   double min_expected, max_expected, max_pos, min_pos;
00128 
00129     if (!n.getParam("min_expected", min_expected)){
00130     ROS_ERROR("Hysteresis Controller: No min expected effort found on parameter namespace: %s)",
00131               n.getNamespace().c_str());
00132     return false;
00133   }
00134 
00135   if (!n.getParam("max_expected", max_expected)){
00136     ROS_ERROR("Hysteresis Controller: No max expected effort found on parameter namespace: %s)",
00137               n.getNamespace().c_str());
00138     return false;
00139   }
00140 
00141   if (!n.getParam("max_position", max_pos)){
00142     ROS_ERROR("Hysteresis Controller: No max position found on parameter namespace: %s)",
00143               n.getNamespace().c_str());
00144     return false;
00145   }
00146 
00147   if (!n.getParam("min_position", min_pos)){
00148     ROS_ERROR("Hysteresis Controller: No min position found on parameter namespace: %s)",
00149               n.getNamespace().c_str());
00150     return false;
00151   }
00152 
00153   if (!n.getParam("timeout", timeout_)){
00154     ROS_ERROR("Hysteresis Controller: No timeout found on parameter namespace: %s)",
00155               n.getNamespace().c_str());
00156     return false;
00157   }
00158 
00159   double tolerance, sd_max;
00160   if (!n.getParam("tolerance", tolerance)){
00161     ROS_WARN("Parameter 'tolerance' is not set on namespace: %s. Default is 0.20.", 
00162              n.getNamespace().c_str());
00163     // This is deprecated, so eventually "return false" will be here.
00164     tolerance = 0.20;
00165   }
00166 
00167   if (!n.getParam("sd_max", sd_max)) {
00168     ROS_WARN("Parameter 'sd_max' is not set on namespace: %s. Default is 0.20.", 
00169              n.getNamespace().c_str());
00170     // This is deprecated, so eventually "return false" will be here.
00171     sd_max = 0.20;
00172   }
00173 
00174   double slope;
00175   if (!n.getParam("slope", slope))
00176     slope = 0;
00177 
00178   initial_time_ = robot_->getTime();
00179   initial_position_ = joint_->position_;
00180 
00181   // Set values in test data output
00182   test_data_.joint_name = name;
00183   test_data_.arg_value[0] = min_expected;
00184   test_data_.arg_value[1] = max_expected;
00185   test_data_.arg_value[2] = min_pos;
00186   test_data_.arg_value[3] = max_pos;
00187   test_data_.arg_value[4] = velocity_;
00188   test_data_.arg_value[5] = timeout_;
00189   test_data_.arg_value[6] = max_effort_;
00190   test_data_.arg_value[7] = tolerance;
00191   test_data_.arg_value[8] = sd_max;
00192   test_data_.arg_value[9] = slope;
00193 
00194   velocity_controller_ = new controller::JointVelocityController();
00195   ros::NodeHandle n_vel(n, "velocity_controller");
00196   if (!velocity_controller_->init(robot, n_vel)) return false;
00197 
00198   // Get the gains, add them to test data
00199   double p, i, d, iClamp, imin;
00200   velocity_controller_->getGains(p, i, d, iClamp, imin);
00201 
00202   test_data_.arg_value[10] = p;
00203   test_data_.arg_value[11] = i;
00204   test_data_.arg_value[12] = d;
00205   test_data_.arg_value[13] = iClamp;
00206 
00207   hyst_pub_.reset(new realtime_tools::RealtimePublisher<joint_qualification_controllers::HysteresisData>(n, "/test_data", 1, true));
00208 
00209   return true;
00210 }
00211 
00212 void HysteresisController::starting()
00213 {
00214   velocity_controller_->starting();
00215 
00216   initial_time_ = robot_->getTime();
00217   initial_position_ = joint_->position_;
00218 }
00219 
00220 void HysteresisController::update()
00221 {
00222   if (!joint_->calibrated_)
00223     return;
00224 
00225   ros::Time time = robot_->getTime();
00226   velocity_controller_->update();
00227 
00228   // Timeout check.
00229   if ((time - initial_time_).toSec() > timeout_ && state_ != ANALYZING && state_ != DONE) 
00230   {
00231     state_ = ANALYZING;
00232     test_data_.arg_value[5] = -1;
00233     velocity_controller_->setCommand(0.0);
00234   }
00235 
00236   switch (state_)
00237   {
00238   case STOPPED:
00239     velocity_controller_->setCommand(-1.0 * velocity_);
00240     starting_count_ = 0;
00241     state_ = MOVING_HOME;
00242     break;
00243   case MOVING_HOME:
00244     starting_count_++;
00245     if (turn() and starting_count_ > 100)
00246     {
00247       velocity_controller_->setCommand(velocity_);
00248       state_ = MOVING_UP;
00249       starting_count_ = 0;
00250     }
00251     break;
00252   case MOVING_UP:
00253     starting_count_++;
00254     if (up_count_ < MAX_DATA_POINTS)
00255     {
00256       test_data_.time_up[up_count_] = time.toSec();
00257       test_data_.effort_up[up_count_] = joint_->measured_effort_;
00258       test_data_.position_up[up_count_] = joint_->position_;
00259       test_data_.velocity_up[up_count_] = joint_->velocity_;
00260       up_count_++;
00261     }
00262     
00263     if ((turn() and starting_count_ > 100) or up_count_ >= MAX_DATA_POINTS)
00264     {
00265       velocity_controller_->setCommand(-1.0 * velocity_);
00266       state_ = MOVING_DOWN;
00267       starting_count_ = 0;
00268     }
00269     break;
00270   case MOVING_DOWN:
00271     starting_count_++;
00272     if (down_count_ < MAX_DATA_POINTS)
00273     {
00274       test_data_.time_down[down_count_] = time.toSec();
00275       test_data_.effort_down[down_count_] = joint_->measured_effort_;
00276       test_data_.position_down[down_count_] = joint_->position_;
00277       test_data_.velocity_down[down_count_] = joint_->velocity_;
00278       down_count_++;
00279     }
00280     if ((turn() and starting_count_ > 100) or down_count_ >= MAX_DATA_POINTS)
00281     {
00282       velocity_controller_->setCommand(0.0);
00283       state_ = ANALYZING;
00284       starting_count_ = 0;
00285     }
00286     break;
00287   case ANALYZING:
00288     velocity_controller_->setCommand(0.0);
00289     analysis();
00290     state_ = DONE;
00291     break;
00292   case DONE:
00293     velocity_controller_->setCommand(0.0);
00294     if (!data_sent_)
00295       data_sent_ = sendData();
00296     break;
00297   }
00298 
00299 }
00300 
00301 bool HysteresisController::turn()
00302 {
00303   if (joint_->joint_->type!=urdf::Joint::CONTINUOUS)
00304   {
00305     return (fabs(joint_->velocity_) < 0.001 && fabs(joint_->commanded_effort_) > max_effort_);
00306   }
00307   else
00308   {
00309     if (fabs(joint_->position_-initial_position_) > 6.28)
00310     {
00311       initial_position_ = joint_->position_;
00312       return true;
00313     }
00314     return false;
00315   }
00316 }
00317 
00318 void HysteresisController::analysis()
00319 {
00320   // Resize if no points
00321   if (up_count_ == 0)
00322     up_count_ = 1; 
00323   if (down_count_ == 0)
00324     down_count_ = 1;
00325 
00326   test_data_.time_up.resize(up_count_);
00327   test_data_.effort_up.resize(up_count_);
00328   test_data_.position_up.resize(up_count_);
00329   test_data_.velocity_up.resize(up_count_);
00330 
00331   test_data_.time_down.resize(down_count_);
00332   test_data_.effort_down.resize(down_count_);
00333   test_data_.position_down.resize(down_count_);
00334   test_data_.velocity_down.resize(down_count_);
00335 
00336   return;
00337 }
00338 
00339 bool HysteresisController::sendData()
00340 {
00341   if (hyst_pub_->trylock())
00342   {
00343     joint_qualification_controllers::HysteresisData *out = &hyst_pub_->msg_;
00344     out->joint_name = test_data_.joint_name;
00345 
00346     out->time_up = test_data_.time_up;
00347     out->effort_up = test_data_.effort_up;
00348     out->position_up = test_data_.position_up;
00349     out->velocity_up = test_data_.velocity_up;
00350 
00351     out->time_down = test_data_.time_down;
00352     out->effort_down = test_data_.effort_down;
00353     out->position_down = test_data_.position_down;
00354     out->velocity_down = test_data_.velocity_down;
00355 
00356 
00357     out->arg_name = test_data_.arg_name;
00358     out->arg_value = test_data_.arg_value;
00359 
00360     hyst_pub_->unlockAndPublish();
00361     return true;
00362   }
00363   return false;
00364 }
00365 
00366 
00367
hysteresis_controller.cpp
