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


velocity_controllers
Author(s): Vijay Pradeep
autogenerated on Thu Jun 6 2019 18:59:06