joint_qualification_controllers: joint_qualification_controllers: wrist_difference

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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/wrist_difference_controller.h"
00036 #include "urdf/joint.h"
00037 
00038 PLUGINLIB_DECLARE_CLASS(joint_qualification_controllers, WristDifferenceController, 
00039                         joint_qualification_controllers::WristDifferenceController, 
00040                         pr2_controller_interface::Controller)
00041 
00042 #define MAX_DATA_POINTS 120000
00043 
00044 using namespace std;
00045 using namespace joint_qualification_controllers;
00046 
00047 WristDifferenceController::WristDifferenceController()
00048 : flex_joint_(NULL),
00049   roll_joint_(NULL),
00050   robot_(NULL),
00051   data_sent_(false),
00052   wrist_data_pub_(NULL)
00053 {
00054   wrist_test_data_.left_turn.time.resize(MAX_DATA_POINTS);
00055   wrist_test_data_.left_turn.flex_position.resize(MAX_DATA_POINTS);
00056   wrist_test_data_.left_turn.flex_effort.resize(MAX_DATA_POINTS);
00057   wrist_test_data_.left_turn.flex_cmd.resize(MAX_DATA_POINTS);
00058   wrist_test_data_.left_turn.roll_position.resize(MAX_DATA_POINTS);
00059   wrist_test_data_.left_turn.roll_effort.resize(MAX_DATA_POINTS);
00060   wrist_test_data_.left_turn.roll_cmd.resize(MAX_DATA_POINTS);
00061   wrist_test_data_.left_turn.roll_velocity.resize(MAX_DATA_POINTS);
00062 
00063   wrist_test_data_.right_turn.time.resize(MAX_DATA_POINTS);
00064   wrist_test_data_.right_turn.flex_position.resize(MAX_DATA_POINTS);
00065   wrist_test_data_.right_turn.flex_effort.resize(MAX_DATA_POINTS);
00066   wrist_test_data_.right_turn.flex_cmd.resize(MAX_DATA_POINTS);
00067   wrist_test_data_.right_turn.roll_position.resize(MAX_DATA_POINTS);
00068   wrist_test_data_.right_turn.roll_effort.resize(MAX_DATA_POINTS);
00069   wrist_test_data_.right_turn.roll_cmd.resize(MAX_DATA_POINTS);
00070   wrist_test_data_.right_turn.roll_velocity.resize(MAX_DATA_POINTS);
00071 
00072   wrist_test_data_.flex_pid.resize(4);
00073   wrist_test_data_.roll_pid.resize(4);
00074 
00075   wrist_test_data_.arg_name.resize(10);
00076   wrist_test_data_.arg_value.resize(10);
00077   wrist_test_data_.arg_name[0] = "Flex Position";
00078   wrist_test_data_.arg_name[1] = "Roll Velocity";
00079   wrist_test_data_.arg_name[2] = "Roll Tolerance (%)";
00080   wrist_test_data_.arg_name[3] = "Roll SD Max (%)";
00081   wrist_test_data_.arg_name[4] = "Timeout";
00082   wrist_test_data_.arg_name[5] = "Left Effort";
00083   wrist_test_data_.arg_name[6] = "Right Effort";
00084   wrist_test_data_.arg_name[7] = "Flex Tolerance";
00085   wrist_test_data_.arg_name[8] = "Flex Max Value";
00086   wrist_test_data_.arg_name[9] = "Flex SD";
00087 
00088   wrist_test_data_.timeout = false;
00089   
00090   state_         = STARTING;
00091   starting_count = 0;
00092   roll_velocity_ = 0;
00093   flex_position_ = 0;
00094   initial_time_  = ros::Time(0);
00095   left_count_    = 0;
00096   right_count_   = 0;
00097   start_count_   = 0;
00098   // Assume 1KHz update rate
00099   timeout_ = MAX_DATA_POINTS / 1000;
00100 }
00101 
00102 WristDifferenceController::~WristDifferenceController()
00103 {
00104   delete flex_controller_;
00105   delete roll_controller_;
00106 }
00107 
00108 bool WristDifferenceController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00109 {
00110   ROS_ASSERT(robot);
00111   robot_ = robot;
00112 
00113   std::string roll_name;
00114   if (!n.getParam("roll_velocity_controller/joint", roll_name)){
00115     ROS_ERROR("Hysteresis Controller: No joint name found on parameter namespace: %s)",
00116               n.getNamespace().c_str());
00117     return false;
00118   }
00119   if (!(roll_joint_ = robot->getJointState(roll_name)))
00120   {
00121     ROS_ERROR("WristDifferenceController could not find joint named \"%s\"\n", roll_name.c_str());
00122     return false;
00123   }
00124   ROS_DEBUG("Roll joint: %s", roll_name.c_str());
00125   if (roll_joint_->joint_->type != urdf::Joint::CONTINUOUS)
00126   {
00127     ROS_ERROR("Wrist roll joint must be continuous. Unable to check wrist symmetry. Roll joint: %s", roll_name.c_str());
00128     return false;
00129   }
00130 
00131   if (!n.getParam("roll_velocity", roll_velocity_)){
00132     ROS_ERROR("Hysteresis Controller: No velocity found on parameter namespace: %s)",
00133               n.getNamespace().c_str());
00134     return false;
00135   }
00136   roll_velocity_ = fabs(roll_velocity_);
00137 
00138   std::string flex_name;
00139   if (!n.getParam("flex_position_controller/joint", flex_name)){
00140     ROS_ERROR("Hysteresis Controller: No joint name found on parameter namespace: %s)",
00141               n.getNamespace().c_str());
00142     return false;
00143   }
00144   if (!(flex_joint_ = robot->getJointState(flex_name)))
00145   {
00146     ROS_ERROR("WristDifferenceController could not find joint named \"%s\"\n", flex_name.c_str());
00147     return false;
00148   }
00149 
00150   if (!n.getParam("flex_position", flex_position_)) {
00151     ROS_ERROR("Hysteresis Controller: No velocity found on parameter namespace: %s)",
00152               n.getNamespace().c_str());
00153     return false;
00154   }
00155 
00156   if (!n.getParam("timeout", timeout_)){
00157     ROS_ERROR("Hysteresis Controller: No timeout found on parameter namespace: %s)",
00158               n.getNamespace().c_str());
00159     return false;
00160   }
00161 
00162   if (!n.getParam("tolerance", tolerance_)){
00163     ROS_WARN("Parameter 'tolerance' is not set on namespace: %s.", 
00164              n.getNamespace().c_str());
00165     return false;
00166   }
00167 
00168   if (!n.getParam("sd_max", sd_max_)) {
00169     ROS_WARN("Parameter 'sd_max' is not set on namespace: %s.", 
00170              n.getNamespace().c_str());
00171     return false;
00172   }
00173 
00174   double left_effort, right_effort, flex_tolerance, flex_max, flex_sd;
00175   if (!n.getParam("left_effort", left_effort)) {
00176     ROS_WARN("Parameter 'left_effort' is not set on namespace: %s.", 
00177              n.getNamespace().c_str());
00178     return false;
00179   }
00180 
00181   if (!n.getParam("right_effort", right_effort)) {
00182     ROS_WARN("Parameter 'right_effort' is not set on namespace: %s.", 
00183              n.getNamespace().c_str());
00184     return false;
00185   }
00186 
00187   if (!n.getParam("flex_tolerance", flex_tolerance)) {
00188     ROS_WARN("Parameter 'flex_tolerance' is not set on namespace: %s.", 
00189              n.getNamespace().c_str());
00190     return false;
00191   }
00192 
00193 
00194   if (!n.getParam("flex_max", flex_max)) {
00195     ROS_WARN("Parameter 'flex_max' is not set on namespace: %s.", 
00196              n.getNamespace().c_str());
00197     return false;
00198   }
00199 
00200   if (!n.getParam("flex_sd", flex_sd)) {
00201     ROS_WARN("Parameter 'flex_sd' is not set on namespace: %s.", 
00202              n.getNamespace().c_str());
00203     return false;
00204   }
00205 
00206   initial_time_ = robot_->getTime();
00207 
00208   // Set values in test data output
00209   wrist_test_data_.flex_joint = flex_name;
00210   wrist_test_data_.roll_joint = roll_name;
00211   wrist_test_data_.arg_value[0] = flex_position_;
00212   wrist_test_data_.arg_value[1] = roll_velocity_;
00213   wrist_test_data_.arg_value[2] = tolerance_;
00214   wrist_test_data_.arg_value[3] = sd_max_;
00215   wrist_test_data_.arg_value[4] = timeout_;
00216   wrist_test_data_.arg_value[5] = left_effort;
00217   wrist_test_data_.arg_value[6] = right_effort;
00218   wrist_test_data_.arg_value[7] = flex_tolerance;
00219   wrist_test_data_.arg_value[8] = flex_max;
00220   wrist_test_data_.arg_value[9] = flex_sd;
00221 
00222   flex_controller_ = new controller::JointPositionController();
00223   ros::NodeHandle n_flex(n, "flex_position_controller");
00224   if (!flex_controller_->init(robot, n_flex)) return false;
00225 
00226   roll_controller_ = new controller::JointVelocityController();
00227   ros::NodeHandle n_roll(n, "roll_velocity_controller");
00228   if (!roll_controller_->init(robot, n_roll)) return false;
00229 
00230   // Get the gains, add them to test data
00231   double p, i, d, iClamp, imin;
00232   roll_controller_->getGains(p, i, d, iClamp, imin);
00233   wrist_test_data_.roll_pid[0] = p;
00234   wrist_test_data_.roll_pid[1] = i;
00235   wrist_test_data_.roll_pid[2] = d;
00236   wrist_test_data_.roll_pid[3] = iClamp;
00237 
00238   flex_controller_->getGains(p, i, d, iClamp, imin);
00239   wrist_test_data_.flex_pid[0] = p;
00240   wrist_test_data_.flex_pid[1] = i;
00241   wrist_test_data_.flex_pid[2] = d;
00242   wrist_test_data_.flex_pid[3] = iClamp;
00243 
00244   wrist_data_pub_.reset(new realtime_tools::RealtimePublisher<joint_qualification_controllers::WristDiffData>(n, "/test_data", 1, true));
00245 
00246   return true;
00247 }
00248 
00249 void WristDifferenceController::starting()
00250 {
00251   roll_controller_->starting();
00252   flex_controller_->starting();
00253 
00254   initial_time_ = robot_->getTime();
00255 }
00256 
00257 void WristDifferenceController::update()
00258 {
00259   // wait until the all joints calibrated
00260   if(!flex_joint_->calibrated_ || !roll_joint_->calibrated_)
00261   {
00262     return;
00263   }
00264 
00265   ros::Time time = robot_->getTime();
00266   flex_controller_->update();
00267   roll_controller_->update();
00268   
00269   // Timeout check
00270   if ((time - initial_time_).toSec() > timeout_ && state_ != ANALYZING && state_ != DONE) 
00271   {
00272     state_ = ANALYZING;
00273     wrist_test_data_.timeout = true;
00274     roll_controller_->setCommand(0.0);
00275   }
00276 
00277   switch (state_)
00278   {
00279   case STARTING:
00280     roll_controller_->setCommand(roll_velocity_);
00281     flex_controller_->setCommand(flex_position_);
00282     start_count_++;
00283     // Let it settle for 3sec before recording data
00284     if (start_count_++ > 3000)
00285     {
00286       initial_position_ = roll_joint_->position_;
00287       state_ = LEFT;
00288     }
00289     break;
00290   case LEFT:
00291     if (left_count_ < MAX_DATA_POINTS)
00292     {
00293       wrist_test_data_.left_turn.time         [left_count_] = time.toSec();
00294       wrist_test_data_.left_turn.flex_position[left_count_] = flex_joint_->position_;
00295       wrist_test_data_.left_turn.flex_effort  [left_count_] = flex_joint_->measured_effort_;
00296       wrist_test_data_.left_turn.flex_cmd     [left_count_] = flex_joint_->commanded_effort_;
00297 
00298       wrist_test_data_.left_turn.roll_position[left_count_] = roll_joint_->position_;
00299       wrist_test_data_.left_turn.roll_effort  [left_count_] = roll_joint_->measured_effort_;
00300       wrist_test_data_.left_turn.roll_cmd     [left_count_] = roll_joint_->commanded_effort_;
00301       wrist_test_data_.left_turn.roll_velocity[left_count_] = roll_joint_->velocity_;
00302       left_count_++;
00303     }
00304 
00305     if(fabs(roll_joint_->position_ - initial_position_) > 6.28 || left_count_ >= MAX_DATA_POINTS)
00306     {
00307       double right_vel = -1 * roll_velocity_;
00308       roll_controller_->setCommand(right_vel);
00309       initial_position_ = roll_joint_->position_;
00310       state_ = RIGHT;
00311     }
00312     break;
00313   case RIGHT:
00314     if (right_count_ < MAX_DATA_POINTS)
00315     {
00316       wrist_test_data_.right_turn.time         [right_count_] = time.toSec();
00317       wrist_test_data_.right_turn.flex_position[right_count_] = flex_joint_->position_;
00318       wrist_test_data_.right_turn.flex_effort  [right_count_] = flex_joint_->measured_effort_;
00319       wrist_test_data_.right_turn.flex_cmd     [right_count_] = flex_joint_->commanded_effort_;
00320 
00321       wrist_test_data_.right_turn.roll_position[right_count_] = roll_joint_->position_;
00322       wrist_test_data_.right_turn.roll_effort  [right_count_] = roll_joint_->measured_effort_;
00323       wrist_test_data_.right_turn.roll_cmd     [right_count_] = roll_joint_->commanded_effort_;
00324       wrist_test_data_.right_turn.roll_velocity[right_count_] = roll_joint_->velocity_;
00325       right_count_++;
00326     }
00327 
00328     if(fabs(roll_joint_->position_ - initial_position_) > 6.28 || right_count_ >= MAX_DATA_POINTS)
00329     {
00330       roll_controller_->setCommand(0.0);
00331       state_ = ANALYZING;
00332     }
00333     break;
00334   case ANALYZING:
00335     roll_controller_->setCommand(0.0);
00336     analysis();
00337     state_ = DONE;
00338     break;
00339   case DONE:
00340     roll_controller_->setCommand(0.0);
00341     if (!data_sent_)
00342       data_sent_ = sendData();
00343     break;
00344   }
00345 }
00346 
00347 void WristDifferenceController::analysis()
00348 {
00349   // Resize if no points
00350   if (left_count_ == 0)
00351     left_count_ = 1; 
00352 
00353   if (right_count_ == 0)
00354     right_count_ = 1; 
00355 
00356   wrist_test_data_.left_turn.time.resize(left_count_);
00357   wrist_test_data_.left_turn.flex_position.resize(left_count_);
00358   wrist_test_data_.left_turn.flex_effort.resize(left_count_);
00359   wrist_test_data_.left_turn.roll_cmd.resize(left_count_);
00360   wrist_test_data_.left_turn.roll_position.resize(left_count_);
00361   wrist_test_data_.left_turn.roll_effort.resize(left_count_);
00362   wrist_test_data_.left_turn.roll_cmd.resize(left_count_);
00363   wrist_test_data_.left_turn.roll_velocity.resize(left_count_);
00364 
00365   wrist_test_data_.right_turn.time.resize(right_count_);
00366   wrist_test_data_.right_turn.flex_position.resize(right_count_);
00367   wrist_test_data_.right_turn.flex_effort.resize(right_count_);
00368   wrist_test_data_.right_turn.roll_cmd.resize(right_count_);
00369   wrist_test_data_.right_turn.roll_position.resize(right_count_);
00370   wrist_test_data_.right_turn.roll_effort.resize(right_count_);
00371   wrist_test_data_.right_turn.roll_cmd.resize(right_count_);
00372   wrist_test_data_.right_turn.roll_velocity.resize(right_count_);
00373 
00374   return;
00375 }
00376 
00377 bool WristDifferenceController::sendData()
00378 {
00379   if (wrist_data_pub_->trylock())
00380   {
00381     joint_qualification_controllers::WristDiffData *out = &wrist_data_pub_->msg_;
00382     out->flex_joint = wrist_test_data_.flex_joint;
00383     out->roll_joint = wrist_test_data_.roll_joint;
00384     out->flex_pid   = wrist_test_data_.flex_pid;
00385     out->roll_pid   = wrist_test_data_.roll_pid;
00386     out->arg_name   = wrist_test_data_.arg_name;
00387     out->arg_value  = wrist_test_data_.arg_value;
00388     out->left_turn  = wrist_test_data_.left_turn;
00389     out->right_turn = wrist_test_data_.right_turn;
00390     out->timeout    = wrist_test_data_.timeout;
00391 
00392     wrist_data_pub_->unlockAndPublish();
00393     return true;
00394   }
00395   return false;
00396 }
00397 
00398 
00399
wrist_difference_controller.cpp
