joint_velocity_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 
38 
39 
40 namespace effort_controllers {
41 
43 : command_(0), loop_count_(0)
44 {}
45 
47 {
49 }
50 
52  const std::string &joint_name, const control_toolbox::Pid &pid)
53 {
54  pid_controller_ = pid;
55 
56  // Get joint handle from hardware interface
57  joint_ = robot->getHandle(joint_name);
58 
59  return true;
60 }
61 
63 {
64  // Get joint name from parameter server
65  std::string joint_name;
66  if (!n.getParam("joint", joint_name)) {
67  ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
68  return false;
69  }
70 
71  // Get joint handle from hardware interface
72  joint_ = robot->getHandle(joint_name);
73 
74  // Load PID Controller using gains set on parameter server
76  return false;
77 
78  // Start realtime state publisher
81  (n, "state", 1));
82 
83  // Start command subscriber
84  sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &JointVelocityController::setCommandCB, this);
85 
86  return true;
87 }
88 
89 void JointVelocityController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup)
90 {
91  pid_controller_.setGains(p,i,d,i_max,i_min,antiwindup);
92 }
93 
94 void JointVelocityController::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
95 {
96  pid_controller_.getGains(p,i,d,i_max,i_min,antiwindup);
97 }
98 
99 void JointVelocityController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
100 {
101  bool dummy;
102  pid_controller_.getGains(p,i,d,i_max,i_min,dummy);
103 }
104 
106 {
108 }
109 
111 {
112  return joint_.getName();
113 }
114 
115 // Set the joint velocity command
117 {
118  command_ = cmd;
119 }
120 
121 // Return the current velocity command
123 {
124  cmd = command_;
125 }
126 
128 {
129  command_ = 0.0;
131 }
132 
133 void JointVelocityController::update(const ros::Time& time, const ros::Duration& period)
134 {
135  double error = command_ - joint_.getVelocity();
136 
137  // Set the PID error and compute the PID command with nonuniform time
138  // step size. The derivative error is computed from the change in the error
139  // and the timestep dt.
140  double commanded_effort = pid_controller_.computeCommand(error, period);
141 
142  joint_.setCommand(commanded_effort);
143 
144  if(loop_count_ % 10 == 0)
145  {
147  {
148  controller_state_publisher_->msg_.header.stamp = time;
150  controller_state_publisher_->msg_.process_value = joint_.getVelocity();
151  controller_state_publisher_->msg_.error = error;
152  controller_state_publisher_->msg_.time_step = period.toSec();
153  controller_state_publisher_->msg_.command = commanded_effort;
154 
155  double dummy;
156  bool antiwindup;
161  dummy,
162  antiwindup);
163  controller_state_publisher_->msg_.antiwindup = static_cast<char>(antiwindup);
164  controller_state_publisher_->unlockAndPublish();
165  }
166  }
167  loop_count_++;
168 }
169 
170 void JointVelocityController::setCommandCB(const std_msgs::Float64ConstPtr& msg)
171 {
172  command_ = msg->data;
173 }
174 
175 } // namespace
176 
effort_controllers::JointVelocityController::command_
double command_
Definition: joint_velocity_controller.h:148
effort_controllers::JointVelocityController::starting
void starting(const ros::Time &time)
This is called from within the realtime thread just before the first call to update.
Definition: joint_velocity_controller.cpp:160
effort_controllers::JointVelocityController::JointVelocityController
JointVelocityController()
Definition: joint_velocity_controller.cpp:75
effort_controllers::JointVelocityController::setCommandCB
void setCommandCB(const std_msgs::Float64ConstPtr &msg)
Callback from /command subscriber for setpoint.
Definition: joint_velocity_controller.cpp:203
effort_controllers::JointVelocityController::loop_count_
int loop_count_
Definition: joint_velocity_controller.h:151
control_toolbox::Pid::setGains
void setGains(const Gains &gains)
effort_controllers::JointVelocityController::update
void update(const ros::Time &time, const ros::Duration &period)
Issues commands to the joint. Should be called at regular intervals.
Definition: joint_velocity_controller.cpp:166
HardwareResourceManager< JointHandle, ClaimResources >::getHandle
JointHandle getHandle(const std::string &name)
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
effort_controllers::JointVelocityController::~JointVelocityController
~JointVelocityController()
Definition: joint_velocity_controller.cpp:79
effort_controllers
Definition: joint_effort_controller.h:42
effort_controllers::JointVelocityController::getJointName
std::string getJointName()
Get the name of the joint this controller uses.
Definition: joint_velocity_controller.cpp:143
ros::Subscriber::shutdown
void shutdown()
hardware_interface::JointHandle::setCommand
void setCommand(double command)
effort_controllers::JointVelocityController::controller_state_publisher_
std::unique_ptr< realtime_tools::RealtimePublisher< control_msgs::JointControllerState > > controller_state_publisher_
Definition: joint_velocity_controller.h:156
effort_controllers::JointVelocityController::printDebug
void printDebug()
Print debug info to console.
Definition: joint_velocity_controller.cpp:138
effort_controllers::JointVelocityController::getCommand
void getCommand(double &cmd)
Get latest velocity command to the joint: revolute (angle) and prismatic (velocity).
Definition: joint_velocity_controller.cpp:155
realtime_tools::RealtimePublisher
effort_controllers::JointVelocityController
Joint Velocity Controller.
Definition: joint_velocity_controller.h:74
controller_interface::ControllerBase
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::JointVelocityController::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_velocity_controller.cpp:122
hardware_interface::EffortJointInterface
d
d
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())
effort_controllers::JointVelocityController::setCommand
void setCommand(double cmd)
Give set velocity of the joint for next update: revolute (angle) and prismatic (velocity)
Definition: joint_velocity_controller.cpp:149
hardware_interface::JointStateHandle::getName
std::string getName() const
effort_controllers::JointVelocityController::pid_controller_
control_toolbox::Pid pid_controller_
Definition: joint_velocity_controller.h:152
effort_controllers::JointVelocityController::init
bool init(hardware_interface::EffortJointInterface *robot, const std::string &joint_name, const control_toolbox::Pid &pid)
Definition: joint_velocity_controller.cpp:84
control_toolbox::Pid::printValues
void printValues()
effort_controllers::JointVelocityController::getGains
void getGains(double &p, double &i, double &d, double &i_max, double &i_min)
Get the PID parameters.
Definition: joint_velocity_controller.cpp:132
hardware_interface::JointStateHandle::getVelocity
double getVelocity() const
ros::Time
ROS_ERROR
#define ROS_ERROR(...)
class_list_macros.hpp
control_toolbox::Pid
effort_controllers::JointVelocityController::sub_command_
ros::Subscriber sub_command_
Definition: joint_velocity_controller.h:158
control_toolbox::Pid::getGains
Gains getGains()
joint_velocity_controller.h
ros::NodeHandle::getNamespace
const std::string & getNamespace() const
DurationBase< Duration >::toSec
double toSec() const
cmd
string cmd
control_toolbox::Pid::computeCommand
double computeCommand(double error, double error_dot, ros::Duration dt)
control_toolbox::Pid::reset
void reset()
ros::Duration
effort_controllers::JointVelocityController::joint_
hardware_interface::JointHandle joint_
Definition: joint_velocity_controller.h:147
ros::NodeHandle


effort_controllers
Author(s): Vijay Pradeep
autogenerated on Sun Mar 3 2024 03:19:24