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