joint_position_controller.h
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 
43 #pragma once
44 
45 
69 #include <control_msgs/JointControllerState.h>
70 #include <control_msgs/JointControllerState.h>
71 #include <control_toolbox/pid.h>
74 #include <memory>
77 #include <ros/node_handle.h>
78 #include <std_msgs/Float64.h>
79 #include <urdf/model.h>
80 
81 namespace velocity_controllers
82 {
83 
84 class JointPositionController: public controller_interface::Controller<hardware_interface::VelocityJointInterface>
85 {
86 public:
87 
91  struct Commands
92  {
93  double position_; // Last commanded position
94  double velocity_; // Last commanded velocity
95  bool has_velocity_; // false if no velocity command has been specified
96  };
97 
100 
115 
121  void setCommand(double pos_target);
122 
130  void setCommand(double pos_target, double vel_target);
131 
137  void starting(const ros::Time& time);
138 
142  void update(const ros::Time& time, const ros::Duration& period);
143 
147  void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
148 
152  void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup);
153 
157  void printDebug();
158 
162  void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup = false);
163 
167  std::string getJointName();
168 
173  double getPosition();
174 
176  urdf::JointConstSharedPtr joint_urdf_;
178  Commands command_struct_; // pre-allocated memory that is re-used to set the realtime buffer
179 
180 private:
184  std::unique_ptr<
186  control_msgs::JointControllerState> > controller_state_publisher_ ;
187 
189 
193  void setCommandCB(const std_msgs::Float64ConstPtr& msg);
194 
200  void enforceJointLimits(double &command);
201 
202 };
203 
204 } // namespace
realtime_tools::RealtimeBuffer
velocity_controllers::JointPositionController::Commands::has_velocity_
bool has_velocity_
Definition: joint_position_controller.h:95
realtime_publisher.h
velocity_controllers::JointPositionController::printDebug
void printDebug()
Print debug info to console.
Definition: joint_position_controller.cpp:149
node_handle.h
velocity_controllers::JointPositionController::Commands::position_
double position_
Definition: joint_position_controller.h:93
velocity_controllers::JointPositionController::setCommandCB
void setCommandCB(const std_msgs::Float64ConstPtr &msg)
Callback from /command subscriber for setpoint.
Definition: joint_position_controller.cpp:282
realtime_buffer.h
velocity_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:186
velocity_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:201
velocity_controllers::JointPositionController::Commands::velocity_
double velocity_
Definition: joint_position_controller.h:94
velocity_controllers::JointPositionController::command_
realtime_tools::RealtimeBuffer< Commands > command_
Definition: joint_position_controller.h:177
velocity_controllers::JointPositionController::sub_command_
ros::Subscriber sub_command_
Definition: joint_position_controller.h:188
controller_interface::Controller
realtime_tools::RealtimePublisher
hardware_interface::VelocityJointInterface
controller.h
joint_command_interface.h
velocity_controllers::JointPositionController::joint_urdf_
urdf::JointConstSharedPtr joint_urdf_
Definition: joint_position_controller.h:176
velocity_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:288
model.h
velocity_controllers::JointPositionController::pid_controller_
control_toolbox::Pid pid_controller_
Definition: joint_position_controller.h:182
velocity_controllers::JointPositionController::getJointName
std::string getJointName()
Get the name of the joint this controller uses.
Definition: joint_position_controller.cpp:154
velocity_controllers::JointPositionController::Commands
Store position and velocity command in struct to allow easier realtime buffer usage.
Definition: joint_position_controller.h:91
velocity_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:138
velocity_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:165
hardware_interface::JointHandle
ros::Time
velocity_controllers::JointPositionController::loop_count_
int loop_count_
Definition: joint_position_controller.h:181
velocity_controllers::JointPositionController::JointPositionController
JointPositionController()
Definition: joint_position_controller.cpp:83
velocity_controllers::JointPositionController::controller_state_publisher_
std::unique_ptr< realtime_tools::RealtimePublisher< control_msgs::JointControllerState > > controller_state_publisher_
Definition: joint_position_controller.h:186
control_toolbox::Pid
velocity_controllers::JointPositionController::joint_
hardware_interface::JointHandle joint_
Definition: joint_position_controller.h:175
pid.h
velocity_controllers
Definition: joint_group_velocity_controller.h:44
velocity_controllers::JointPositionController
Joint Position Controller.
Definition: joint_position_controller.h:84
velocity_controllers::JointPositionController::~JointPositionController
~JointPositionController()
Definition: joint_position_controller.cpp:87
ros::Duration
velocity_controllers::JointPositionController::init
bool init(hardware_interface::VelocityJointInterface *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:92
velocity_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)
Get the PID parameters.
Definition: joint_position_controller.cpp:133
velocity_controllers::JointPositionController::getPosition
double getPosition()
Get the current position of the joint.
Definition: joint_position_controller.cpp:159
velocity_controllers::JointPositionController::command_struct_
Commands command_struct_
Definition: joint_position_controller.h:178
ros::NodeHandle
ros::Subscriber


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