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.initParam("robot_description"))
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  try
90  {
91  joints_.push_back(hw->getHandle(joint_names_[i]));
92  }
94  {
95  ROS_ERROR_STREAM("Exception thrown: " << e.what());
96  return false;
97  }
98 
99  urdf::JointConstSharedPtr joint_urdf = urdf.getJoint(joint_names_[i]);
100  if (!joint_urdf)
101  {
102  ROS_ERROR("Could not find joint '%s' in urdf", joint_names_[i].c_str());
103  return false;
104  }
105  joint_urdfs_.push_back(joint_urdf);
106 
107  // Load PID Controller using gains set on parameter server
108  if (!pid_controllers_[i].init(ros::NodeHandle(n, joint_names_[i] + "/pid")))
109  {
110  ROS_ERROR_STREAM("Failed to load PID parameters from " << joint_names_[i] + "/pid");
111  return false;
112  }
113  }
114 
115  commands_buffer_.writeFromNonRT(std::vector<double>(n_joints_, 0.0));
116 
117  sub_command_ = n.subscribe<std_msgs::Float64MultiArray>("command", 1, &JointGroupPositionController::commandCB, this);
118  return true;
119  }
120 
122  {
123  std::vector<double> & commands = *commands_buffer_.readFromRT();
124  for(unsigned int i=0; i<n_joints_; i++)
125  {
126  double command_position = commands[i];
127 
128  double error; //, vel_error;
129  double commanded_effort;
130 
131  double current_position = joints_[i].getPosition();
132 
133  // Make sure joint is within limits if applicable
134  enforceJointLimits(command_position, i);
135 
136  // Compute position error
137  if (joint_urdfs_[i]->type == urdf::Joint::REVOLUTE)
138  {
140  current_position,
141  command_position,
142  joint_urdfs_[i]->limits->lower,
143  joint_urdfs_[i]->limits->upper,
144  error);
145  }
146  else if (joint_urdfs_[i]->type == urdf::Joint::CONTINUOUS)
147  {
148  error = angles::shortest_angular_distance(current_position, command_position);
149  }
150  else //prismatic
151  {
152  error = command_position - current_position;
153  }
154 
155  // Set the PID error and compute the PID command with nonuniform
156  // time step size.
157  commanded_effort = pid_controllers_[i].computeCommand(error, period);
158 
159  joints_[i].setCommand(commanded_effort);
160  }
161  }
162 
163  void JointGroupPositionController::commandCB(const std_msgs::Float64MultiArrayConstPtr& msg)
164  {
165  if(msg->data.size()!=n_joints_)
166  {
167  ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!");
168  return;
169  }
170  commands_buffer_.writeFromNonRT(msg->data);
171  }
172 
173  void JointGroupPositionController::enforceJointLimits(double &command, unsigned int index)
174  {
175  // Check that this joint has applicable limits
176  if (joint_urdfs_[index]->type == urdf::Joint::REVOLUTE || joint_urdfs_[index]->type == urdf::Joint::PRISMATIC)
177  {
178  if( command > joint_urdfs_[index]->limits->upper ) // above upper limnit
179  {
180  command = joint_urdfs_[index]->limits->upper;
181  }
182  else if( command < joint_urdfs_[index]->limits->lower ) // below lower limit
183  {
184  command = joint_urdfs_[index]->limits->lower;
185  }
186  }
187  }
188 
189 } // namespace
190 
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::vector< hardware_interface::JointHandle > joints_
void commandCB(const std_msgs::Float64MultiArrayConstPtr &msg)
Forward command controller for a set of effort controlled joints (torque or force).
JointGroupPositionController()
Forward command controller for a set of effort controlled joints (torque or force).
void enforceJointLimits(double &command, unsigned int index)
void writeFromNonRT(const T &data)
void update(const ros::Time &, const ros::Duration &)
const std::string & getNamespace() const
URDF_EXPORT bool initParam(const std::string &param)
realtime_tools::RealtimeBuffer< std::vector< double > > commands_buffer_
def shortest_angular_distance_with_limits(from_angle, to_angle, left_limit, right_limit)
JointHandle getHandle(const std::string &name)
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_STREAM(args)
bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &n)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
#define ROS_ERROR(...)
def shortest_angular_distance(from_angle, to_angle)


effort_controllers
Author(s): Vijay Pradeep
autogenerated on Sat Apr 18 2020 03:58:11