joint_position_controller.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * Copyright (c) 2012, hiDOF, Inc.
6  * Copyright (c) 2013, University of Colorado, Boulder
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the Willow Garage nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *********************************************************************/
36 
37 /*
38  Author: Dave Coleman
39  Contributors: Jonathan Bohren, Wim Meeussen, Vijay Pradeep
40  Desc: Velocity-based position controller using basic PID loop
41 */
42 
44 #include <angles/angles.h>
46 
47 namespace velocity_controllers {
48 
50  : loop_count_(0)
51 {}
52 
53 JointPositionController::~JointPositionController()
54 {
55  sub_command_.shutdown();
56 }
57 
58 bool JointPositionController::init(hardware_interface::VelocityJointInterface *robot, ros::NodeHandle &n)
59 {
60  // Get joint name from parameter server
61  std::string joint_name;
62  if (!n.getParam("joint", joint_name))
63  {
64  ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
65  return false;
66  }
67 
68  // Load PID Controller using gains set on parameter server
69  if (!pid_controller_.init(ros::NodeHandle(n, "pid")))
70  return false;
71 
72  // Start realtime state publisher
73  controller_state_publisher_.reset(
75 
76  // Start command subscriber
77  sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &JointPositionController::setCommandCB, this);
78 
79  // Get joint handle from hardware interface
80  joint_ = robot->getHandle(joint_name);
81 
82  // Get URDF info about joint
84  if (!urdf.initParamWithNodeHandle("robot_description", n))
85  {
86  ROS_ERROR("Failed to parse urdf file");
87  return false;
88  }
89  joint_urdf_ = urdf.getJoint(joint_name);
90  if (!joint_urdf_)
91  {
92  ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
93  return false;
94  }
95 
96  return true;
97 }
98 
99 void JointPositionController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup)
100 {
101  pid_controller_.setGains(p,i,d,i_max,i_min,antiwindup);
102 }
103 
104 void JointPositionController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
105 {
106  bool dummy;
107  pid_controller_.getGains(p,i,d,i_max,i_min,dummy);
108 }
109 
110 void JointPositionController::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
111 {
112  pid_controller_.getGains(p,i,d,i_max,i_min,antiwindup);
113 }
114 
115 void JointPositionController::printDebug()
116 {
117  pid_controller_.printValues();
118 }
119 
120 std::string JointPositionController::getJointName()
121 {
122  return joint_.getName();
123 }
124 
125 double JointPositionController::getPosition()
126 {
127  return joint_.getPosition();
128 }
129 
130 // Set the joint position command
131 void JointPositionController::setCommand(double pos_command)
132 {
133  command_struct_.position_ = pos_command;
134  command_struct_.has_velocity_ = false; // Flag to ignore the velocity command since our setCommand method did not include it
135 
136  // the writeFromNonRT can be used in RT, if you have the guarantee that
137  // * no non-rt thread is calling the same function (we're not subscribing to ros callbacks)
138  // * there is only one single rt thread
139  command_.writeFromNonRT(command_struct_);
140 }
141 
142 // Set the joint position command with a velocity command as well
143 void JointPositionController::setCommand(double pos_command, double vel_command)
144 {
145  command_struct_.position_ = pos_command;
146  command_struct_.velocity_ = vel_command;
147  command_struct_.has_velocity_ = true;
148 
149  command_.writeFromNonRT(command_struct_);
150 }
151 
152 void JointPositionController::starting(const ros::Time& time)
153 {
154  double pos_command = joint_.getPosition();
155 
156  // Make sure joint is within limits if applicable
157  enforceJointLimits(pos_command);
158 
159  command_struct_.position_ = pos_command;
160  command_struct_.has_velocity_ = false;
161 
162  command_.initRT(command_struct_);
163 
164  pid_controller_.reset();
165 }
166 
167 void JointPositionController::update(const ros::Time& time, const ros::Duration& period)
168 {
169  command_struct_ = *(command_.readFromRT());
170  double command_position = command_struct_.position_;
171  double command_velocity = command_struct_.velocity_;
172  bool has_velocity_ = command_struct_.has_velocity_;
173 
174  double error, vel_error;
175  double commanded_velocity;
176 
177  double current_position = joint_.getPosition();
178 
179  // Make sure joint is within limits if applicable
180  enforceJointLimits(command_position);
181 
182  // Compute position error
183  if (joint_urdf_->type == urdf::Joint::REVOLUTE)
184  {
186  current_position,
187  command_position,
188  joint_urdf_->limits->lower,
189  joint_urdf_->limits->upper,
190  error);
191  }
192  else if (joint_urdf_->type == urdf::Joint::CONTINUOUS)
193  {
194  error = angles::shortest_angular_distance(current_position, command_position);
195  }
196  else //prismatic
197  {
198  error = command_position - current_position;
199  }
200 
201  // Decide which of the two PID computeCommand() methods to call
202  if (has_velocity_)
203  {
204  // Compute velocity error if a non-zero velocity command was given
205  vel_error = command_velocity - joint_.getVelocity();
206 
207  // Set the PID error and compute the PID command with nonuniform
208  // time step size. This also allows the user to pass in a precomputed derivative error.
209  commanded_velocity = pid_controller_.computeCommand(error, vel_error, period);
210  }
211  else
212  {
213  // Set the PID error and compute the PID command with nonuniform
214  // time step size.
215  commanded_velocity = pid_controller_.computeCommand(error, period);
216  }
217 
218  joint_.setCommand(commanded_velocity);
219 
220  // publish state
221  if (loop_count_ % 10 == 0)
222  {
223  if(controller_state_publisher_ && controller_state_publisher_->trylock())
224  {
225  controller_state_publisher_->msg_.header.stamp = time;
226  controller_state_publisher_->msg_.set_point = command_position;
227  controller_state_publisher_->msg_.process_value = current_position;
228  controller_state_publisher_->msg_.process_value_dot = joint_.getVelocity();
229  controller_state_publisher_->msg_.error = error;
230  controller_state_publisher_->msg_.time_step = period.toSec();
231  controller_state_publisher_->msg_.command = commanded_velocity;
232 
233  double dummy;
234  bool antiwindup;
235  getGains(controller_state_publisher_->msg_.p,
236  controller_state_publisher_->msg_.i,
237  controller_state_publisher_->msg_.d,
238  controller_state_publisher_->msg_.i_clamp,
239  dummy,
240  antiwindup);
241  controller_state_publisher_->msg_.antiwindup = static_cast<char>(antiwindup);
242  controller_state_publisher_->unlockAndPublish();
243  }
244  }
245  loop_count_++;
246 }
247 
248 void JointPositionController::setCommandCB(const std_msgs::Float64ConstPtr& msg)
249 {
250  setCommand(msg->data);
251 }
252 
253 // Note: we may want to remove this function once issue https://github.com/ros/angles/issues/2 is resolved
254 void JointPositionController::enforceJointLimits(double &command)
255 {
256  // Check that this joint has applicable limits
257  if (joint_urdf_->type == urdf::Joint::REVOLUTE || joint_urdf_->type == urdf::Joint::PRISMATIC)
258  {
259  if( command > joint_urdf_->limits->upper ) // above upper limnit
260  {
261  command = joint_urdf_->limits->upper;
262  }
263  else if( command < joint_urdf_->limits->lower ) // below lower limit
264  {
265  command = joint_urdf_->limits->lower;
266  }
267  }
268 }
269 
270 } // namespace
271 
angles::shortest_angular_distance
static double shortest_angular_distance(double from, double to)
joint_position_controller.h
HardwareResourceManager< JointHandle, ClaimResources >::getHandle
JointHandle getHandle(const std::string &name)
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
command
ROSLIB_DECL std::string command(const std::string &cmd)
urdf::Model
realtime_tools::RealtimePublisher
controller_interface::ControllerBase
hardware_interface::VelocityJointInterface
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
ros::Time
urdf
velocity_controllers::JointPositionController::JointPositionController
JointPositionController()
Definition: joint_position_controller.cpp:83
ROS_ERROR
#define ROS_ERROR(...)
class_list_macros.hpp
angles::shortest_angular_distance_with_large_limits
static bool shortest_angular_distance_with_large_limits(double from, double to, double left_limit, double right_limit, double &shortest_angle)
velocity_controllers
Definition: joint_group_velocity_controller.h:44
ros::NodeHandle::getNamespace
const std::string & getNamespace() const
DurationBase< Duration >::toSec
double toSec() const
velocity_controllers::JointPositionController
Joint Position Controller.
Definition: joint_position_controller.h:84
ros::Duration
ros::NodeHandle
angles.h


velocity_controllers
Author(s): Vijay Pradeep
autogenerated on Fri May 24 2024 02:41:29