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