joint_position_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Modified BSD License)
00003  *
00004  *  Copyright (c) 2011, PAL Robotics, S.L.
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 PAL Robotics, S.L. 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 
00039 #include <pluginlib/class_list_macros.h>
00040 #include "joint_position_controller/joint_position_controller.h"
00041 
00042 
00043 static const std::string SUB_TOPIC_JOINT_STATES_CMD = "/joint_states_cmd";
00044 
00045 
00046 void JointPositionController::jointStatesCmdCB(const sensor_msgs::JointState::ConstPtr &command)
00047 {
00048   if (command->position.size() == q_desi_.rows())
00049   {
00050     for(unsigned int i = 0; i < q_desi_.rows(); ++i)
00051     {
00052       q_desi_(i)  = command->position[i];
00053     }
00054   }
00055   else
00056     ROS_WARN("Size of the commanded joint state does not match the internally used size of the joint array!");
00057     ROS_WARN("Desired joint positions are not updated!");    
00058 }
00059 
00060 
00061 // Controller initialization in non-realtime
00062 bool JointPositionController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &node)
00063 {
00064   // Store the robot handle for later use
00065   robot_state_ = robot;
00066 
00067   // Initialize tree from robot model
00068   tree_.init(robot_state_);
00069 
00070   // Resize (pre-allocate) the variables in non-realtime.
00071   nrOfJnts_ = tree_.size();
00072   q_.resize(nrOfJnts_);
00073   q_desi_.resize(nrOfJnts_);
00074   q_err_.resize(nrOfJnts_);
00075   q_err_old_.resize(nrOfJnts_);
00076   q_err_dot_.resize(nrOfJnts_);
00077   tau_.resize(nrOfJnts_);
00078   tau_max_.resize(nrOfJnts_);
00079   Kp_.resize(nrOfJnts_);
00080   Kd_.resize(nrOfJnts_);
00081 
00082   // flag for whether or not to limit error
00083   node.param("limit_error", limit_error_, false);
00084   ROS_INFO("limit_error = %s", (limit_error_)?"true":"false");
00085   
00086   // Maximum error
00087   double factor;
00088   node.param("q_err_max_factor", factor, 0.0);
00089   node.param("q_err_max", q_err_max_, 0.0);
00090   ROS_INFO("q_err_max_ = %f", q_err_max_);
00091 
00092   // Maximum error velocity
00093   node.param("q_err_dot_max_factor", factor, 0.0);
00094   q_err_dot_max_ = factor * q_err_max_;
00095   ROS_INFO("q_err_dot_max_ = %f", q_err_dot_max_);
00096 
00098   double PGain, DGain, DGainFactor;
00099   node.param("PGain", PGain, 0.0);
00100   ROS_INFO("PGain = %f", PGain);
00101   node.param("DGainFactor", DGainFactor, 0.0);
00102   node.param("DGain", DGain, 0.0);
00103   ROS_INFO("DGain = %f", DGain);
00104   for (unsigned int i = 0 ; i < nrOfJnts_ ; ++i)
00105   {
00106     Kp_(i) = 0.5 * PGain;
00107     Kd_(i) = 0.5 * DGain;
00108   }
00109   for (unsigned int i = 0 ; i < nrOfJnts_; ++i)
00110   {
00111     std::string name_str;
00112     std::stringstream ss;
00113     ss << "Kp_" << i;
00114     ss >> name_str;
00115     double factor;
00116     node.getParam(name_str, factor);
00117     Kp_(i) = factor * PGain;
00118     ROS_INFO("Kp_(%d) = %f", i, Kp_(i));
00119     ss << "Kd_" << i;
00120     ss >> name_str;
00121     node.getParam(name_str, factor);
00122     Kd_(i) = factor * DGain;
00123     ROS_INFO("Kd_(%d) = %f", i, Kd_(i));
00124   }
00125   
00126   double tau_max_value;
00127   node.param("tau_max_value", tau_max_value, 100.0);
00128   for (unsigned int i = 0 ; i < nrOfJnts_; ++i)
00129   {
00130     std::string name_str;
00131     std::stringstream ss;
00132     ss << "tau_max_factor_" << i;
00133     ss >> name_str;
00134     double factor;
00135     node.param(name_str, factor, 0.0);
00136     tau_max_(i) = factor * tau_max_value;
00137     ROS_INFO("tau_max_(%d) = %f", i, tau_max_(i));
00138   }
00139   
00140   // Subscribe to the topic for joint states command
00141   sub_joint_states_cmd = node.subscribe(SUB_TOPIC_JOINT_STATES_CMD, 20, &JointPositionController::jointStatesCmdCB, this);
00142                                
00143   return true;
00144 }
00145 
00146 
00147 // Controller startup in realtime
00148 void JointPositionController::starting()
00149 {
00150   loop_count_ = 0;
00151   last_time_  = robot_state_->getTime();
00152   dt_         = 0.0;
00153   
00154   for (unsigned int i = 0; i < nrOfJnts_; ++i)
00155   {
00156     q_(i) = q_desi_(i) = q_err_(i) = q_err_old_(i) = q_err_dot_(i) = tau_(i) = 0.0;
00157   }
00158 }
00159 
00160 
00161 // Controller update loop in realtime
00162 void JointPositionController::update()
00163 {
00164   if (loop_count_ % 1000 == 0)
00165   {
00166     ROS_DEBUG("-----------------------------------UPDATING_START--------------------------------------");
00167     ROS_DEBUG("loop_count_ = %d", loop_count_);
00168   }
00169 
00170   // Current joint positions and velocities.
00171   tree_.getPositions(q_);
00172 
00173   // Calculate the dt between servo cycles.
00174   dt_ = (robot_state_->getTime() - last_time_).toSec();
00175   last_time_ = robot_state_->getTime();
00176   if (loop_count_ % 1000 == 0)
00177     ROS_DEBUG("dt_ = %f", dt_);
00178   
00179   // Current error
00180   KDL::Subtract(q_desi_, q_, q_err_);
00181 
00182   /*
00183   // Workaround to enforce no movement of the base joints (like caster wheels)
00184   q_err_(0) = 0.0;
00185   q_err_(1) = 0.0;
00186   q_err_(2) = 0.0;
00187   q_err_(3) = 0.0;
00188   */
00189   
00190   // check, if one (or more) joint velocities exceed the maximum value
00191   // and if so, safe the biggest overshoot for scaling q_dot_ properly
00192   // to keep the direction of the velocity vector the same
00193   double rel_os, rel_os_max = 0.0; // relative overshoot and the biggest relative overshoot
00194   bool max_exceeded = false;
00195 
00196   if(limit_error_ == true)
00197   {
00198     ROS_DEBUG_THROTTLE(1.0, "limitting");
00199     for (unsigned int i = 0; i < q_err_.rows(); ++i)
00200     {
00201       if ( q_err_(i) > q_err_max_ )
00202       {
00203         max_exceeded = true;
00204         rel_os = (q_err_(i) - q_err_max_) / q_err_max_;
00205         if ( rel_os > rel_os_max )
00206         {
00207           rel_os_max = rel_os;
00208         }
00209       }
00210       else if ( q_err_(i) < -q_err_max_ )
00211       {
00212         max_exceeded = true;
00213         rel_os = (-q_err_(i) - q_err_max_) / q_err_max_;
00214         if ( rel_os > rel_os_max)
00215         {
00216           rel_os_max = rel_os;
00217         }
00218       }
00219     }
00220     // scales q_out, if one joint exceeds the maximum value
00221     if ( max_exceeded == true )
00222      KDL::Multiply(q_err_, ( 1.0 / ( 1.0 + rel_os_max ) ), q_err_);
00223   }
00224 
00225   // calculating error velocity = the change of the error
00226   KDL::Subtract(q_err_, q_err_old_, q_err_dot_);
00227   q_err_old_ = q_err_;
00228   
00229   // calculate fake efforts
00230   for (unsigned int i = 0; i < nrOfJnts_; ++i)
00231     tau_(i) = Kp_(i) * q_err_(i) + Kd_(i) * q_err_dot_(i);
00232   
00233   // Limit efforts
00234   for (unsigned int i = 0; i < nrOfJnts_; ++i)
00235   {
00236     if (tau_(i) > tau_max_(i))
00237       tau_(i) = tau_max_(i);      
00238     else if (tau_(i) < -tau_max_(i))
00239       tau_(i) = -tau_max_(i);
00240   }
00241 
00242   // And finally send these efforts out
00243   tree_.setEfforts(tau_);
00244 
00245   if (loop_count_ % 1000 == 0)
00246   {
00247     ROS_DEBUG("tree joint controller statistics:");
00248     for (unsigned int i = 0; i < q_.rows(); ++i)
00249     {
00250       ROS_DEBUG("q_[%d]: %f, q_desi_[%d]: %f, q_err_[%d]: %f, q_err_dot_[%d]: %f, tau_[%d]: %f",
00251       i, q_(i), i, q_desi_(i), i, q_err_(i), i, q_err_dot_(i), i, tau_(i));
00252     }
00253     ROS_DEBUG("-----------------------------------UPDATING_END--------------------------------------");
00254   }
00255   
00256   loop_count_++;
00257 }
00258 
00259 
00260 // Controller stopping in realtime
00261 void JointPositionController::stopping()
00262 {}
00263 
00264 
00266 PLUGINLIB_DECLARE_CLASS(joint_position_controller, JointPositionController, JointPositionController, pr2_controller_interface::Controller)
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


joint_position_controller
Author(s): Marcus Liebhardt , Adolfo Rodríguez Tsouroukdissian , Hilario Tome
autogenerated on Thu Jun 27 2013 16:12:18