joint_velocity_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 #include <effort_controllers/joint_velocity_controller.h>
00037 #include <pluginlib/class_list_macros.h>
00038 
00039 
00040 namespace effort_controllers {
00041 
00042 JointVelocityController::JointVelocityController()
00043 : command_(0), loop_count_(0)
00044 {}
00045 
00046 JointVelocityController::~JointVelocityController()
00047 {
00048   sub_command_.shutdown();
00049 }
00050 
00051 bool JointVelocityController::init(hardware_interface::EffortJointInterface *robot, 
00052   const std::string &joint_name, const control_toolbox::Pid &pid)
00053 {
00054   pid_controller_ = pid;
00055 
00056   // Get joint handle from hardware interface
00057   joint_ = robot->getHandle(joint_name);
00058 
00059   return true;
00060 }
00061 
00062 bool JointVelocityController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
00063 {
00064   // Get joint name from parameter server
00065   std::string joint_name;
00066   if (!n.getParam("joint", joint_name)) {
00067     ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
00068     return false;
00069   }
00070 
00071   // Get joint handle from hardware interface
00072   joint_ = robot->getHandle(joint_name);
00073 
00074   // Load PID Controller using gains set on parameter server
00075   if (!pid_controller_.init(ros::NodeHandle(n, "pid")))
00076     return false;
00077 
00078   // Start realtime state publisher
00079   controller_state_publisher_.reset(
00080     new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>
00081     (n, "state", 1));
00082 
00083   // Start command subscriber
00084   sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &JointVelocityController::setCommandCB, this);
00085 
00086   return true;
00087 }
00088 
00089 
00090 void JointVelocityController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min)
00091 {
00092   pid_controller_.setGains(p,i,d,i_max,i_min);
00093 
00094 }
00095 
00096 void JointVelocityController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
00097 {
00098   pid_controller_.getGains(p,i,d,i_max,i_min);
00099 }
00100 
00101 void JointVelocityController::printDebug()
00102 {
00103   pid_controller_.printValues();
00104 }
00105 
00106 std::string JointVelocityController::getJointName()
00107 {
00108   return joint_.getName();
00109 }
00110 
00111 // Set the joint velocity command
00112 void JointVelocityController::setCommand(double cmd)
00113 {
00114   command_ = cmd;
00115 }
00116 
00117 // Return the current velocity command
00118 void JointVelocityController::getCommand(double& cmd)
00119 {
00120   cmd = command_;
00121 }
00122 
00123 void JointVelocityController::starting(const ros::Time& time)
00124 {
00125   command_ = 0.0;
00126   pid_controller_.reset();
00127 }
00128 
00129 void JointVelocityController::update(const ros::Time& time, const ros::Duration& period)
00130 {
00131   double error = command_ - joint_.getVelocity();
00132 
00133   // Set the PID error and compute the PID command with nonuniform time
00134   // step size. The derivative error is computed from the change in the error
00135   // and the timestep dt.
00136   double commanded_effort = pid_controller_.computeCommand(error, period);
00137 
00138   joint_.setCommand(commanded_effort);
00139 
00140   if(loop_count_ % 10 == 0)
00141   {
00142     if(controller_state_publisher_ && controller_state_publisher_->trylock())
00143     {
00144       controller_state_publisher_->msg_.header.stamp = time;
00145       controller_state_publisher_->msg_.set_point = command_;
00146       controller_state_publisher_->msg_.process_value = joint_.getVelocity();
00147       controller_state_publisher_->msg_.error = error;
00148       controller_state_publisher_->msg_.time_step = period.toSec();
00149       controller_state_publisher_->msg_.command = commanded_effort;
00150 
00151       double dummy;
00152       getGains(controller_state_publisher_->msg_.p,
00153                controller_state_publisher_->msg_.i,
00154                controller_state_publisher_->msg_.d,
00155                controller_state_publisher_->msg_.i_clamp,
00156                dummy);
00157       controller_state_publisher_->unlockAndPublish();
00158     }
00159   }
00160   loop_count_++;
00161 }
00162 
00163 void JointVelocityController::setCommandCB(const std_msgs::Float64ConstPtr& msg)
00164 {
00165   command_ = msg->data;
00166 }
00167 
00168 } // namespace
00169 
00170 PLUGINLIB_EXPORT_CLASS( effort_controllers::JointVelocityController, controller_interface::ControllerBase)


effort_controllers
Author(s): Vijay Pradeep
autogenerated on Thu Jun 6 2019 18:58:55