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


joint_qualification_controllers
Author(s): Kevin Watts, Melonee Wise
autogenerated on Sat Apr 27 2019 03:10:47