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


effort_controllers
Author(s): Vijay Pradeep
autogenerated on Thu Apr 11 2019 03:08:20