joint_position_controller.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  Copyright (c) 2012, hiDOF, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 /*
00037  Author: Vijay Pradeep
00038  Contributors: Jonathan Bohren, Wim Meeussen, Dave Coleman
00039  Desc: Effort(force)-based position controller using basic PID loop
00040 */
00041 
00042 #include <effort_controllers/joint_position_controller.h>
00043 #include <angles/angles.h>
00044 #include <pluginlib/class_list_macros.h>
00045 
00046 namespace effort_controllers {
00047 
00048 JointPositionController::JointPositionController()
00049   : loop_count_(0)
00050 {}
00051 
00052 JointPositionController::~JointPositionController()
00053 {
00054   sub_command_.shutdown();
00055 }
00056 
00057 bool JointPositionController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
00058 {
00059   // Get joint name from parameter server
00060   std::string joint_name;
00061   if (!n.getParam("joint", joint_name)) 
00062   {
00063     ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
00064     return false;
00065   }
00066 
00067   // Load PID Controller using gains set on parameter server
00068   if (!pid_controller_.init(ros::NodeHandle(n, "pid")))
00069     return false;
00070 
00071   // Start realtime state publisher
00072   controller_state_publisher_.reset(
00073     new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>(n, "state", 1));
00074 
00075   // Start command subscriber
00076   sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &JointPositionController::setCommandCB, this);
00077 
00078   // Get joint handle from hardware interface
00079   joint_ = robot->getHandle(joint_name);
00080 
00081   // Get URDF info about joint
00082   urdf::Model urdf;
00083   if (!urdf.initParam("robot_description"))
00084   {
00085     ROS_ERROR("Failed to parse urdf file");
00086     return false;
00087   }
00088   joint_urdf_ = urdf.getJoint(joint_name);
00089   if (!joint_urdf_)
00090   {
00091     ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
00092     return false;
00093   }
00094 
00095   return true;
00096 }
00097 
00098 void JointPositionController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min)
00099 {
00100   pid_controller_.setGains(p,i,d,i_max,i_min);
00101 }
00102 
00103 void JointPositionController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
00104 {
00105   pid_controller_.getGains(p,i,d,i_max,i_min);
00106 }
00107 
00108 void JointPositionController::printDebug()
00109 {
00110   pid_controller_.printValues();
00111 }
00112 
00113 std::string JointPositionController::getJointName()
00114 {
00115   return joint_.getName();
00116 }
00117 
00118 double JointPositionController::getPosition()
00119 {
00120   return joint_.getPosition();
00121 }
00122 
00123 // Set the joint position command
00124 void JointPositionController::setCommand(double pos_command)
00125 {
00126   command_struct_.position_ = pos_command;
00127   command_struct_.has_velocity_ = false; // Flag to ignore the velocity command since our setCommand method did not include it
00128 
00129   // the writeFromNonRT can be used in RT, if you have the guarantee that
00130   //  * no non-rt thread is calling the same function (we're not subscribing to ros callbacks)
00131   //  * there is only one single rt thread
00132   command_.writeFromNonRT(command_struct_);
00133 }
00134 
00135 // Set the joint position command with a velocity command as well
00136 void JointPositionController::setCommand(double pos_command, double vel_command)
00137 {
00138   command_struct_.position_ = pos_command;
00139   command_struct_.velocity_ = vel_command;
00140   command_struct_.has_velocity_ = true;
00141 
00142   command_.writeFromNonRT(command_struct_);
00143 }
00144 
00145 void JointPositionController::starting(const ros::Time& time)
00146 {
00147   double pos_command = joint_.getPosition();
00148 
00149   // Make sure joint is within limits if applicable
00150   enforceJointLimits(pos_command);
00151 
00152   command_struct_.position_ = pos_command;
00153   command_struct_.has_velocity_ = false;
00154 
00155   command_.initRT(command_struct_);
00156 
00157   pid_controller_.reset();
00158 }
00159 
00160 void JointPositionController::update(const ros::Time& time, const ros::Duration& period)
00161 {
00162   command_struct_ = *(command_.readFromRT());
00163   double command_position = command_struct_.position_;
00164   double command_velocity = command_struct_.velocity_;
00165   bool has_velocity_ =  command_struct_.has_velocity_;
00166 
00167   double error, vel_error;
00168   double commanded_effort;
00169 
00170   double current_position = joint_.getPosition();
00171 
00172   // Make sure joint is within limits if applicable
00173   enforceJointLimits(command_position);
00174 
00175   // Compute position error
00176   if (joint_urdf_->type == urdf::Joint::REVOLUTE)
00177   {
00178    angles::shortest_angular_distance_with_limits(
00179       current_position,
00180       command_position,
00181       joint_urdf_->limits->lower,
00182       joint_urdf_->limits->upper,
00183       error);
00184   }
00185   else if (joint_urdf_->type == urdf::Joint::CONTINUOUS)
00186   {
00187     error = angles::shortest_angular_distance(current_position, command_position);
00188   }
00189   else //prismatic
00190   {
00191     error = command_position - current_position;
00192   }
00193 
00194   // Decide which of the two PID computeCommand() methods to call
00195   if (has_velocity_)
00196   {
00197     // Compute velocity error if a non-zero velocity command was given
00198     vel_error = command_velocity - joint_.getVelocity();
00199 
00200     // Set the PID error and compute the PID command with nonuniform
00201     // time step size. This also allows the user to pass in a precomputed derivative error.
00202     commanded_effort = pid_controller_.computeCommand(error, vel_error, period);
00203   }
00204   else
00205   {
00206     // Set the PID error and compute the PID command with nonuniform
00207     // time step size.
00208     commanded_effort = pid_controller_.computeCommand(error, period);
00209   }
00210 
00211   joint_.setCommand(commanded_effort);
00212 
00213   // publish state
00214   if (loop_count_ % 10 == 0)
00215   {
00216     if(controller_state_publisher_ && controller_state_publisher_->trylock())
00217     {
00218       controller_state_publisher_->msg_.header.stamp = time;
00219       controller_state_publisher_->msg_.set_point = command_position;
00220       controller_state_publisher_->msg_.process_value = current_position;
00221       controller_state_publisher_->msg_.process_value_dot = joint_.getVelocity();
00222       controller_state_publisher_->msg_.error = error;
00223       controller_state_publisher_->msg_.time_step = period.toSec();
00224       controller_state_publisher_->msg_.command = commanded_effort;
00225 
00226       double dummy;
00227       getGains(controller_state_publisher_->msg_.p,
00228         controller_state_publisher_->msg_.i,
00229         controller_state_publisher_->msg_.d,
00230         controller_state_publisher_->msg_.i_clamp,
00231         dummy);
00232       controller_state_publisher_->unlockAndPublish();
00233     }
00234   }
00235   loop_count_++;
00236 }
00237 
00238 void JointPositionController::setCommandCB(const std_msgs::Float64ConstPtr& msg)
00239 {
00240   setCommand(msg->data);
00241 }
00242 
00243 // Note: we may want to remove this function once issue https://github.com/ros/angles/issues/2 is resolved
00244 void JointPositionController::enforceJointLimits(double &command)
00245 {
00246   // Check that this joint has applicable limits
00247   if (joint_urdf_->type == urdf::Joint::REVOLUTE || joint_urdf_->type == urdf::Joint::PRISMATIC)
00248   {
00249     if( command > joint_urdf_->limits->upper ) // above upper limnit
00250     {
00251       command = joint_urdf_->limits->upper;
00252     }
00253     else if( command < joint_urdf_->limits->lower ) // below lower limit
00254     {
00255       command = joint_urdf_->limits->lower;
00256     }
00257   }
00258 }
00259 
00260 } // namespace
00261 
00262 PLUGINLIB_EXPORT_CLASS( effort_controllers::JointPositionController, controller_interface::ControllerBase)


effort_controllers
Author(s): Vijay Pradeep
autogenerated on Fri Aug 28 2015 12:37:06