joint_group_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  * Copyright (c) 2013, PAL Robotics, S.L.
7  * Copyright (c) 2014, Fraunhofer IPA
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the Willow Garage nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *********************************************************************/
37 
40 #include <angles/angles.h>
41 
42 namespace effort_controllers
43 {
44 
60 
62  {
63  // List of controlled joints
64  std::string param_name = "joints";
65  if(!n.getParam(param_name, joint_names_))
66  {
67  ROS_ERROR_STREAM("Failed to getParam '" << param_name << "' (namespace: " << n.getNamespace() << ").");
68  return false;
69  }
70  n_joints_ = joint_names_.size();
71 
72  if(n_joints_ == 0){
73  ROS_ERROR_STREAM("List of joint names is empty.");
74  return false;
75  }
76 
77  // Get URDF
79  if (!urdf.initParamWithNodeHandle("robot_description", n))
80  {
81  ROS_ERROR("Failed to parse urdf file");
82  return false;
83  }
84 
86 
87  for(unsigned int i=0; i<n_joints_; i++)
88  {
89  const auto& joint_name = joint_names_[i];
90 
91  try
92  {
93  joints_.push_back(hw->getHandle(joint_name));
94  }
96  {
97  ROS_ERROR_STREAM("Exception thrown: " << e.what());
98  return false;
99  }
100 
101  urdf::JointConstSharedPtr joint_urdf = urdf.getJoint(joint_name);
102  if (!joint_urdf)
103  {
104  ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
105  return false;
106  }
107  joint_urdfs_.push_back(joint_urdf);
108 
109  // Load PID Controller using gains set on parameter server
110  if (!pid_controllers_[i].init(ros::NodeHandle(n, joint_name + "/pid")))
111  {
112  ROS_ERROR_STREAM("Failed to load PID parameters from " << joint_name + "/pid");
113  return false;
114  }
115  }
116 
117  commands_buffer_.writeFromNonRT(std::vector<double>(n_joints_, 0.0));
118 
119  sub_command_ = n.subscribe<std_msgs::Float64MultiArray>("command", 1, &JointGroupPositionController::commandCB, this);
120  return true;
121  }
122 
124  {
125  std::vector<double> current_positions(n_joints_, 0.0);
126  for (std::size_t i = 0; i < n_joints_; ++i)
127  {
128  current_positions[i] = joints_[i].getPosition();
129  enforceJointLimits(current_positions[i], i);
130  pid_controllers_[i].reset();
131  }
132  commands_buffer_.initRT(current_positions);
133  }
134 
135  void JointGroupPositionController::update(const ros::Time& time, const ros::Duration& period)
136  {
137  std::vector<double> & commands = *commands_buffer_.readFromRT();
138  for(unsigned int i=0; i<n_joints_; i++)
139  {
140  double command_position = commands[i];
141 
142  double error;
143  double commanded_effort;
144 
145  double current_position = joints_[i].getPosition();
146 
147  // Make sure joint is within limits if applicable
148  enforceJointLimits(command_position, i);
149 
150  // Compute position error
151  if (joint_urdfs_[i]->type == urdf::Joint::REVOLUTE)
152  {
154  current_position,
155  command_position,
156  joint_urdfs_[i]->limits->lower,
157  joint_urdfs_[i]->limits->upper,
158  error);
159  }
160  else if (joint_urdfs_[i]->type == urdf::Joint::CONTINUOUS)
161  {
162  error = angles::shortest_angular_distance(current_position, command_position);
163  }
164  else //prismatic
165  {
166  error = command_position - current_position;
167  }
168 
169  // Set the PID error and compute the PID command with nonuniform
170  // time step size.
171  commanded_effort = pid_controllers_[i].computeCommand(error, period);
172 
173  joints_[i].setCommand(commanded_effort);
174  }
175  }
176 
177  void JointGroupPositionController::commandCB(const std_msgs::Float64MultiArrayConstPtr& msg)
178  {
179  if(msg->data.size()!=n_joints_)
180  {
181  ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!");
182  return;
183  }
184  commands_buffer_.writeFromNonRT(msg->data);
185  }
186 
187  void JointGroupPositionController::enforceJointLimits(double &command, unsigned int index)
188  {
189  // Check that this joint has applicable limits
190  if (joint_urdfs_[index]->type == urdf::Joint::REVOLUTE || joint_urdfs_[index]->type == urdf::Joint::PRISMATIC)
191  {
192  if( command > joint_urdfs_[index]->limits->upper ) // above upper limnit
193  {
194  command = joint_urdfs_[index]->limits->upper;
195  }
196  else if( command < joint_urdfs_[index]->limits->lower ) // below lower limit
197  {
198  command = joint_urdfs_[index]->limits->lower;
199  }
200  }
201  }
202 
203 } // namespace
204 
realtime_tools::RealtimeBuffer::writeFromNonRT
void writeFromNonRT(const T &data)
angles::shortest_angular_distance
static double shortest_angular_distance(double from, double to)
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
effort_controllers::JointGroupPositionController::update
void update(const ros::Time &, const ros::Duration &)
Definition: joint_group_position_controller.cpp:170
HardwareResourceManager< JointHandle, ClaimResources >::getHandle
JointHandle getHandle(const std::string &name)
hardware_interface::HardwareInterfaceException::what
const char * what() const noexcept override
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
command
ROSLIB_DECL std::string command(const std::string &cmd)
effort_controllers::JointGroupPositionController::enforceJointLimits
void enforceJointLimits(double &command, unsigned int index)
Definition: joint_group_position_controller.cpp:222
effort_controllers
Definition: joint_effort_controller.h:42
effort_controllers::JointGroupPositionController::~JointGroupPositionController
~JointGroupPositionController()
Definition: joint_group_position_controller.cpp:94
ros::Subscriber::shutdown
void shutdown()
urdf::Model
effort_controllers::JointGroupPositionController::JointGroupPositionController
JointGroupPositionController()
Forward command controller for a set of effort controlled joints (torque or force).
Definition: joint_group_position_controller.cpp:93
controller_interface::ControllerBase
effort_controllers::JointGroupPositionController::joint_names_
std::vector< std::string > joint_names_
Definition: joint_group_position_controller.h:147
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
realtime_tools::RealtimeBuffer::readFromRT
T * readFromRT()
effort_controllers::JointGroupPositionController::n_joints_
unsigned int n_joints_
Definition: joint_group_position_controller.h:150
hardware_interface::EffortJointInterface
effort_controllers::JointGroupPositionController
Forward command controller for a set of effort controlled joints (torque or force).
Definition: joint_group_position_controller.h:102
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::JointGroupPositionController::sub_command_
ros::Subscriber sub_command_
Definition: joint_group_position_controller.h:153
effort_controllers::JointGroupPositionController::commandCB
void commandCB(const std_msgs::Float64MultiArrayConstPtr &msg)
Definition: joint_group_position_controller.cpp:212
hardware_interface::HardwareInterfaceException
effort_controllers::JointGroupPositionController::init
bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &n)
Definition: joint_group_position_controller.cpp:96
ros::Time
urdf
effort_controllers::JointGroupPositionController::joint_urdfs_
std::vector< urdf::JointConstSharedPtr > joint_urdfs_
Definition: joint_group_position_controller.h:157
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)
effort_controllers::JointGroupPositionController::commands_buffer_
realtime_tools::RealtimeBuffer< std::vector< double > > commands_buffer_
Definition: joint_group_position_controller.h:149
ros::NodeHandle::getNamespace
const std::string & getNamespace() const
effort_controllers::JointGroupPositionController::joints_
std::vector< hardware_interface::JointHandle > joints_
Definition: joint_group_position_controller.h:148
realtime_tools::RealtimeBuffer::initRT
void initRT(const T &data)
effort_controllers::JointGroupPositionController::pid_controllers_
std::vector< control_toolbox::Pid > pid_controllers_
Definition: joint_group_position_controller.h:155
joint_group_position_controller.h
ros::Duration
effort_controllers::JointGroupPositionController::starting
void starting(const ros::Time &)
Definition: joint_group_position_controller.cpp:158
ros::NodeHandle
angles.h


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