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  *  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 "robot_mechanism_controllers/joint_velocity_controller.h"
00036 #include "pluginlib/class_list_macros.h"
00037 
00038 PLUGINLIB_EXPORT_CLASS( controller::JointVelocityController, pr2_controller_interface::Controller)
00039 
00040 using namespace std;
00041 
00042 namespace controller {
00043 
00044 JointVelocityController::JointVelocityController()
00045 : joint_state_(NULL), command_(0), robot_(NULL), last_time_(0), loop_count_(0)
00046 {
00047 }
00048 
00049 JointVelocityController::~JointVelocityController()
00050 {
00051   sub_command_.shutdown();
00052 }
00053 
00054 bool JointVelocityController::init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name,
00055                                    const control_toolbox::Pid &pid)
00056 {
00057   assert(robot);
00058   robot_ = robot;
00059   last_time_ = robot->getTime();
00060 
00061   joint_state_ = robot_->getJointState(joint_name);
00062   if (!joint_state_)
00063   {
00064     ROS_ERROR("JointVelocityController could not find joint named \"%s\"\n",
00065               joint_name.c_str());
00066     return false;
00067   }
00068 
00069   pid_controller_ = pid;
00070 
00071   return true;
00072 }
00073 
00074 bool JointVelocityController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00075 {
00076   assert(robot);
00077   node_ = n;
00078   robot_ = robot;
00079 
00080   std::string joint_name;
00081   if (!node_.getParam("joint", joint_name)) {
00082     ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00083     return false;
00084   }
00085   if (!(joint_state_ = robot->getJointState(joint_name)))
00086   {
00087     ROS_ERROR("Could not find joint \"%s\" (namespace: %s)",
00088               joint_name.c_str(), node_.getNamespace().c_str());
00089     return false;
00090   }
00091 
00092   if (!pid_controller_.init(ros::NodeHandle(node_, "pid")))
00093     return false;
00094 
00095   controller_state_publisher_.reset(
00096     new realtime_tools::RealtimePublisher<pr2_controllers_msgs::JointControllerState>
00097     (node_, "state", 1));
00098 
00099   sub_command_ = node_.subscribe<std_msgs::Float64>("command", 1, &JointVelocityController::setCommandCB, this);
00100 
00101   return true;
00102 }
00103 
00104 
00105 void JointVelocityController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min)
00106 {
00107   pid_controller_.setGains(p,i,d,i_max,i_min);
00108 
00109 }
00110 
00111 void JointVelocityController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
00112 {
00113   pid_controller_.getGains(p,i,d,i_max,i_min);
00114 }
00115 
00116 std::string JointVelocityController::getJointName()
00117 {
00118   return joint_state_->joint_->name;
00119 }
00120 
00121 // Set the joint velocity command
00122 void JointVelocityController::setCommand(double cmd)
00123 {
00124   command_ = cmd;
00125 }
00126 
00127 // Return the current velocity command
00128 void JointVelocityController::getCommand(double  & cmd)
00129 {
00130   cmd = command_;
00131 }
00132 
00133 void JointVelocityController::update()
00134 {
00135   assert(robot_ != NULL);
00136   ros::Time time = robot_->getTime();
00137 
00138   double error = joint_state_->velocity_ - command_;
00139   dt_ = time - last_time_;
00140   double command = pid_controller_.updatePid(error, dt_);
00141   joint_state_->commanded_effort_ += command;
00142 
00143   if(loop_count_ % 10 == 0)
00144   {
00145     if(controller_state_publisher_ && controller_state_publisher_->trylock())
00146     {
00147       controller_state_publisher_->msg_.header.stamp = time;
00148       controller_state_publisher_->msg_.set_point = command_;
00149       controller_state_publisher_->msg_.process_value = joint_state_->velocity_;
00150       controller_state_publisher_->msg_.error = error;
00151       controller_state_publisher_->msg_.time_step = dt_.toSec();
00152       controller_state_publisher_->msg_.command = command;
00153 
00154       double dummy;
00155       getGains(controller_state_publisher_->msg_.p,
00156                controller_state_publisher_->msg_.i,
00157                controller_state_publisher_->msg_.d,
00158                controller_state_publisher_->msg_.i_clamp,
00159                dummy);
00160       controller_state_publisher_->unlockAndPublish();
00161     }
00162   }
00163   loop_count_++;
00164 
00165   last_time_ = time;
00166 }
00167 
00168 void JointVelocityController::setCommandCB(const std_msgs::Float64ConstPtr& msg)
00169 {
00170   command_ = msg->data;
00171 }
00172 
00173 } // namespace


robot_mechanism_controllers
Author(s): John Hsu, Melonee Wise, Stuart Glaser
autogenerated on Thu Apr 24 2014 15:44:44