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 
166 void JointPositionController::update(const ros::Time& time, const ros::Duration& period)
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 
effort_controllers::JointPositionController::setGains
void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup=false)
Set the PID parameters.
Definition: joint_position_controller.cpp:131
effort_controllers::JointPositionController::Commands::has_velocity_
bool has_velocity_
Definition: joint_position_controller.h:88
angles::shortest_angular_distance
static double shortest_angular_distance(double from, double to)
effort_controllers::JointPositionController::JointPositionController
JointPositionController()
Definition: joint_position_controller.cpp:81
control_toolbox::Pid::setGains
void setGains(const Gains &gains)
effort_controllers::JointPositionController::getGains
void getGains(double &p, double &i, double &d, double &i_max, double &i_min)
Get the PID parameters.
Definition: joint_position_controller.cpp:141
joint_position_controller.h
effort_controllers::JointPositionController::sub_command_
ros::Subscriber sub_command_
Definition: joint_position_controller.h:181
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)
effort_controllers
Definition: joint_effort_controller.h:42
effort_controllers::JointPositionController::loop_count_
int loop_count_
Definition: joint_position_controller.h:174
effort_controllers::JointPositionController::init
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...
Definition: joint_position_controller.cpp:90
ros::Subscriber::shutdown
void shutdown()
urdf::Model
hardware_interface::JointHandle::setCommand
void setCommand(double command)
effort_controllers::JointPositionController::starting
void starting(const ros::Time &time)
This is called from within the realtime thread just before the first call to update.
Definition: joint_position_controller.cpp:184
effort_controllers::JointPositionController::printDebug
void printDebug()
Print debug info to console.
Definition: joint_position_controller.cpp:147
realtime_tools::RealtimePublisher
controller_interface::ControllerBase
effort_controllers::JointPositionController::Commands::position_
double position_
Definition: joint_position_controller.h:86
effort_controllers::JointPositionController::Commands::velocity_
double velocity_
Definition: joint_position_controller.h:87
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
control_toolbox::Pid::init
bool init(const ros::NodeHandle &n, const bool quiet=false)
effort_controllers::JointPositionController::pid_controller_
control_toolbox::Pid pid_controller_
Definition: joint_position_controller.h:175
effort_controllers::JointPositionController::update
void update(const ros::Time &time, const ros::Duration &period)
Issues commands to the joint. Should be called at regular intervals.
Definition: joint_position_controller.cpp:199
effort_controllers::JointPositionController::command_struct_
Commands command_struct_
Definition: joint_position_controller.h:171
hardware_interface::EffortJointInterface
effort_controllers::JointPositionController::joint_
hardware_interface::JointHandle joint_
Definition: joint_position_controller.h:168
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())
hardware_interface::JointStateHandle::getName
std::string getName() const
effort_controllers::JointPositionController::controller_state_publisher_
std::unique_ptr< realtime_tools::RealtimePublisher< control_msgs::JointControllerState > > controller_state_publisher_
Definition: joint_position_controller.h:179
effort_controllers::JointPositionController::joint_urdf_
urdf::JointConstSharedPtr joint_urdf_
Definition: joint_position_controller.h:169
effort_controllers::JointPositionController::setCommand
void setCommand(double pos_target)
Give set position of the joint for next update: revolute (angle) and prismatic (position)
Definition: joint_position_controller.cpp:163
effort_controllers::JointPositionController::command_
realtime_tools::RealtimeBuffer< Commands > command_
Definition: joint_position_controller.h:170
control_toolbox::Pid::printValues
void printValues()
effort_controllers::JointPositionController::getJointName
std::string getJointName()
Get the name of the joint this controller uses.
Definition: joint_position_controller.cpp:152
hardware_interface::JointStateHandle::getVelocity
double getVelocity() const
ros::Time
effort_controllers::JointPositionController::enforceJointLimits
void enforceJointLimits(double &command)
Check that the command is within the hard limits of the joint. Checks for joint type first....
Definition: joint_position_controller.cpp:286
hardware_interface::JointStateHandle::getPosition
double getPosition() const
urdf
effort_controllers::JointPositionController
Joint Position Controller.
Definition: joint_position_controller.h:77
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)
control_toolbox::Pid::getGains
Gains getGains()
ros::NodeHandle::getNamespace
const std::string & getNamespace() const
DurationBase< Duration >::toSec
double toSec() const
effort_controllers::JointPositionController::~JointPositionController
~JointPositionController()
Definition: joint_position_controller.cpp:85
control_toolbox::Pid::computeCommand
double computeCommand(double error, double error_dot, ros::Duration dt)
control_toolbox::Pid::reset
void reset()
ros::Duration
effort_controllers::JointPositionController::getPosition
double getPosition()
Get the current position of the joint.
Definition: joint_position_controller.cpp:157
effort_controllers::JointPositionController::setCommandCB
void setCommandCB(const std_msgs::Float64ConstPtr &msg)
Callback from /command subscriber for setpoint.
Definition: joint_position_controller.cpp:280
ros::NodeHandle
angles.h


effort_controllers
Author(s): Vijay Pradeep
autogenerated on Fri May 24 2024 02:41:22