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  * 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 #ifndef EFFORT_CONTROLLERS__JOINT_POSITION_CONTROLLER_H
37 #define EFFORT_CONTROLLERS__JOINT_POSITION_CONTROLLER_H
38 
62 #include <ros/node_handle.h>
63 #include <urdf/model.h>
64 #include <control_toolbox/pid.h>
65 #include <boost/scoped_ptr.hpp>
66 #include <boost/thread/condition.hpp>
70 #include <control_msgs/JointControllerState.h>
71 #include <std_msgs/Float64.h>
72 #include <control_msgs/JointControllerState.h>
74 
75 namespace effort_controllers
76 {
77 
78 class JointPositionController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
79 {
80 public:
81 
85  struct Commands
86  {
87  double position_; // Last commanded position
88  double velocity_; // Last commanded velocity
89  bool has_velocity_; // false if no velocity command has been specified
90  };
91 
94 
109 
115  void setCommand(double pos_target);
116 
124  void setCommand(double pos_target, double vel_target);
125 
131  void starting(const ros::Time& time);
132 
136  void update(const ros::Time& time, const ros::Duration& period);
137 
141  void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
142 
146  void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup);
147 
151  void printDebug();
152 
156  void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup = false);
157 
161  std::string getJointName();
162 
167  double getPosition();
168 
170  urdf::JointConstSharedPtr joint_urdf_;
172  Commands command_struct_; // pre-allocated memory that is re-used to set the realtime buffer
173 
174 private:
178  boost::scoped_ptr<
180  control_msgs::JointControllerState> > controller_state_publisher_ ;
181 
183 
187  void setCommandCB(const std_msgs::Float64ConstPtr& msg);
188 
194  void enforceJointLimits(double &command);
195 
196 };
197 
198 } // namespace
199 
200 #endif
void starting(const ros::Time &time)
This is called from within the realtime thread just before the first call to update.
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...
Store position and velocity command in struct to allow easier realtime buffer usage.
void update(const ros::Time &time, const ros::Duration &period)
Issues commands to the joint. Should be called at regular intervals.
realtime_tools::RealtimeBuffer< Commands > command_
void setCommandCB(const std_msgs::Float64ConstPtr &msg)
Callback from /command subscriber for setpoint.
double getPosition()
Get the current position of the joint.
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.
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.


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