tolerances.h
Go to the documentation of this file.
1 // Copyright (C) 2013, PAL Robotics S.L.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of PAL Robotics S.L. nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 
29 
30 #pragma once
31 
32 
33 // C++ standard
34 #include <cassert>
35 #include <cmath>
36 #include <string>
37 #include <vector>
38 
39 // ROS
40 #include <ros/node_handle.h>
41 
42 // ROS messages
43 #include <control_msgs/FollowJointTrajectoryAction.h>
44 
46 {
47 
53 template<class Scalar>
55 {
56  StateTolerances(Scalar position_tolerance = static_cast<Scalar>(0.0),
57  Scalar velocity_tolerance = static_cast<Scalar>(0.0),
58  Scalar acceleration_tolerance = static_cast<Scalar>(0.0))
59  : position(position_tolerance),
60  velocity(velocity_tolerance),
61  acceleration(acceleration_tolerance)
62  {}
63 
64  Scalar position;
65  Scalar velocity;
66  Scalar acceleration;
67 };
68 
72 template<class Scalar>
74 {
75  SegmentTolerances(const typename std::vector<StateTolerances<Scalar> >::size_type& size = 0)
76  : state_tolerance(size, static_cast<Scalar>(0.0)),
77  goal_state_tolerance(size, static_cast<Scalar>(0.0)),
78  goal_time_tolerance(static_cast<Scalar>(0.0))
79  {}
80 
82  std::vector<StateTolerances<Scalar> > state_tolerance;
83 
85  std::vector<StateTolerances<Scalar> > goal_state_tolerance;
86 
89 };
90 
94 template<class Scalar>
96 {
98  : state_tolerance(static_cast<Scalar>(0.0)),
99  goal_state_tolerance(static_cast<Scalar>(0.0)),
100  goal_time_tolerance(static_cast<Scalar>(0.0))
101  {}
102 
105 
108 
111 };
112 
119 template <class State>
120 inline bool checkStateTolerance(const State& state_error,
121  const std::vector<StateTolerances<typename State::Scalar> >& state_tolerance,
122  bool show_errors = false)
123 {
124  const unsigned int n_joints = state_tolerance.size();
125 
126  // Preconditions
127  assert(n_joints == state_error.position.size());
128  assert(n_joints == state_error.velocity.size());
129  assert(n_joints == state_error.acceleration.size());
130 
131  for (unsigned int i = 0; i < n_joints; ++i)
132  {
133  using std::abs;
134  const StateTolerances<typename State::Scalar>& tol = state_tolerance[i]; // Alias for brevity
135  const bool is_valid = !(tol.position > 0.0 && abs(state_error.position[i]) > tol.position) &&
136  !(tol.velocity > 0.0 && abs(state_error.velocity[i]) > tol.velocity) &&
137  !(tol.acceleration > 0.0 && abs(state_error.acceleration[i]) > tol.acceleration);
138 
139  if (!is_valid)
140  {
141  if( show_errors )
142  {
143  ROS_ERROR_STREAM_NAMED("tolerances","Path state tolerances failed on joint " << i);
144 
145  if (tol.position > 0.0 && abs(state_error.position[i]) > tol.position)
146  ROS_ERROR_STREAM_NAMED("tolerances","Position Error: " << state_error.position[i] <<
147  " Position Tolerance: " << tol.position);
148  if (tol.velocity > 0.0 && abs(state_error.velocity[i]) > tol.velocity)
149  ROS_ERROR_STREAM_NAMED("tolerances","Velocity Error: " << state_error.velocity[i] <<
150  " Velocity Tolerance: " << tol.velocity);
151  if (tol.acceleration > 0.0 && abs(state_error.acceleration[i]) > tol.acceleration)
152  ROS_ERROR_STREAM_NAMED("tolerances","Acceleration Error: " << state_error.acceleration[i] <<
153  " Acceleration Tolerance: " << tol.acceleration);
154  }
155  return false;
156  }
157  }
158  return true;
159 }
160 
167 template <class State>
168 inline bool checkStateTolerancePerJoint(const State& state_error,
169  const StateTolerances<typename State::Scalar>& state_tolerance,
170  bool show_errors = false)
171 {
172 
173  using std::abs;
174 
175  const bool is_valid = !(state_tolerance.position > 0.0 && abs(state_error.position[0]) > state_tolerance.position) &&
176  !(state_tolerance.velocity > 0.0 && abs(state_error.velocity[0]) > state_tolerance.velocity) &&
177  !(state_tolerance.acceleration > 0.0 && abs(state_error.acceleration[0]) > state_tolerance.acceleration);
178 
179  if (!is_valid)
180  {
181  if( show_errors )
182  {
183  ROS_ERROR_STREAM_NAMED("tolerances","Path state tolerances failed:");
184 
185  if (state_tolerance.position > 0.0 && abs(state_error.position[0]) > state_tolerance.position)
186  ROS_ERROR_STREAM_NAMED("tolerances","Position Error: " << state_error.position[0] <<
187  " Position Tolerance: " << state_tolerance.position);
188  if (state_tolerance.velocity > 0.0 && abs(state_error.velocity[0]) > state_tolerance.velocity)
189  ROS_ERROR_STREAM_NAMED("tolerances","Velocity Error: " << state_error.velocity[0] <<
190  " Velocity Tolerance: " << state_tolerance.velocity);
191  if (state_tolerance.acceleration > 0.0 && abs(state_error.acceleration[0]) > state_tolerance.acceleration)
192  ROS_ERROR_STREAM_NAMED("tolerances","Acceleration Error: " << state_error.acceleration[0] <<
193  " Acceleration Tolerance: " << state_tolerance.acceleration);
194  }
195  return false;
196  }
197  return true;
198 }
199 
211 template<class Scalar>
212 void updateStateTolerances(const control_msgs::JointTolerance& tol_msg, StateTolerances<Scalar>& tols)
213 {
214  if (tol_msg.position > 0.0) {tols.position = static_cast<Scalar>(tol_msg.position);}
215  else if (tol_msg.position < 0.0) {tols.position = static_cast<Scalar>(0.0);}
216 
217  if (tol_msg.velocity > 0.0) {tols.velocity = static_cast<Scalar>(tol_msg.velocity);}
218  else if (tol_msg.velocity < 0.0) {tols.velocity = static_cast<Scalar>(0.0);}
219 
220  if (tol_msg.acceleration > 0.0) {tols.acceleration = static_cast<Scalar>(tol_msg.acceleration);}
221  else if (tol_msg.acceleration < 0.0) {tols.acceleration = static_cast<Scalar>(0.0);}
222 }
223 
231 template<class Scalar>
232 void updateSegmentTolerances(const control_msgs::FollowJointTrajectoryGoal& goal,
233  const std::vector<std::string>& joint_names,
235 )
236 {
237  // Preconditions
238  assert(joint_names.size() == tols.state_tolerance.size());
239  assert(joint_names.size() == tols.goal_state_tolerance.size());
240 
241  typedef typename std::vector<std::string>::const_iterator StringConstIterator;
242  typedef typename std::vector<control_msgs::JointTolerance>::const_iterator TolMsgConstIterator;
243 
244  for (StringConstIterator names_it = joint_names.begin(); names_it != joint_names.end(); ++names_it)
245  {
246  const typename std::vector<std::string>::size_type id = std::distance(joint_names.begin(), names_it);
247 
248  // Update path tolerances
249  const std::vector<control_msgs::JointTolerance>& state_tol = goal.path_tolerance;
250  for(TolMsgConstIterator state_tol_it = state_tol.begin(); state_tol_it != state_tol.end(); ++state_tol_it)
251  {
252  if (*names_it == state_tol_it->name) {updateStateTolerances(*state_tol_it, tols.state_tolerance[id]);}
253  }
254 
255  // Update goal state tolerances
256  const std::vector<control_msgs::JointTolerance>& g_state_tol = goal.goal_tolerance;
257  for(TolMsgConstIterator g_state_tol_it = g_state_tol.begin(); g_state_tol_it != g_state_tol.end(); ++g_state_tol_it)
258  {
259  if (*names_it == g_state_tol_it->name) {updateStateTolerances(*g_state_tol_it, tols.goal_state_tolerance[id]);}
260  }
261  }
262 
263  // Update goal time tolerance
264  const ros::Duration& goal_time_tolerance = goal.goal_time_tolerance;
265  if (goal_time_tolerance < ros::Duration(0.0)) {tols.goal_time_tolerance = 0.0;}
266  else if (goal_time_tolerance > ros::Duration(0.0)) {tols.goal_time_tolerance = goal_time_tolerance.toSec();}
267 }
268 
290 template<class Scalar>
292  const std::vector<std::string>& joint_names)
293 {
294  const unsigned int n_joints = joint_names.size();
296 
297  // State and goal state tolerances
298  double stopped_velocity_tolerance;
299  nh.param("stopped_velocity_tolerance", stopped_velocity_tolerance, 0.01);
300 
301  tolerances.state_tolerance.resize(n_joints);
302  tolerances.goal_state_tolerance.resize(n_joints);
303  for (unsigned int i = 0; i < n_joints; ++i)
304  {
305  nh.param(joint_names[i] + "/trajectory", tolerances.state_tolerance[i].position, 0.0);
306  nh.param(joint_names[i] + "/goal", tolerances.goal_state_tolerance[i].position, 0.0);
307  tolerances.goal_state_tolerance[i].velocity = stopped_velocity_tolerance;
308  }
309 
310  // Goal time tolerance
311  nh.param("goal_time", tolerances.goal_time_tolerance, 0.0);
312 
313  return tolerances;
314 }
315 
316 } // namespace
joint_trajectory_controller::SegmentTolerances
Trajectory segment tolerances.
Definition: tolerances.h:73
node_handle.h
joint_trajectory_controller::SegmentTolerances::goal_state_tolerance
std::vector< StateTolerances< Scalar > > goal_state_tolerance
Definition: tolerances.h:85
joint_trajectory_controller::SegmentTolerances::SegmentTolerances
SegmentTolerances(const typename std::vector< StateTolerances< Scalar > >::size_type &size=0)
Definition: tolerances.h:75
joint_trajectory_controller::updateSegmentTolerances
void updateSegmentTolerances(const control_msgs::FollowJointTrajectoryGoal &goal, const std::vector< std::string > &joint_names, SegmentTolerances< Scalar > &tols)
Update data in tols from data in goal.
Definition: tolerances.h:232
joint_trajectory_controller::checkStateTolerance
bool checkStateTolerance(const State &state_error, const std::vector< StateTolerances< typename State::Scalar > > &state_tolerance, bool show_errors=false)
Definition: tolerances.h:120
joint_trajectory_controller::SegmentTolerancesPerJoint
Trajectory segment tolerances per Joint.
Definition: tolerances.h:95
joint_trajectory_controller::SegmentTolerancesPerJoint::SegmentTolerancesPerJoint
SegmentTolerancesPerJoint()
Definition: tolerances.h:97
joint_trajectory_controller::StateTolerances::StateTolerances
StateTolerances(Scalar position_tolerance=static_cast< Scalar >(0.0), Scalar velocity_tolerance=static_cast< Scalar >(0.0), Scalar acceleration_tolerance=static_cast< Scalar >(0.0))
Definition: tolerances.h:56
joint_trajectory_controller
Definition: hold_trajectory_builder.h:35
joint_trajectory_controller::checkStateTolerancePerJoint
bool checkStateTolerancePerJoint(const State &state_error, const StateTolerances< typename State::Scalar > &state_tolerance, bool show_errors=false)
Definition: tolerances.h:168
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
joint_trajectory_controller::SegmentTolerancesPerJoint::goal_time_tolerance
Scalar goal_time_tolerance
Definition: tolerances.h:110
joint_trajectory_controller::StateTolerances
Trajectory state tolerances for position, velocity and acceleration variables.
Definition: tolerances.h:54
joint_trajectory_controller::SegmentTolerances::goal_time_tolerance
Scalar goal_time_tolerance
Definition: tolerances.h:88
joint_trajectory_controller::SegmentTolerancesPerJoint::goal_state_tolerance
StateTolerances< Scalar > goal_state_tolerance
Definition: tolerances.h:107
joint_trajectory_controller::StateTolerances::acceleration
Scalar acceleration
Definition: tolerances.h:66
joint_trajectory_controller::getSegmentTolerances
SegmentTolerances< Scalar > getSegmentTolerances(const ros::NodeHandle &nh, const std::vector< std::string > &joint_names)
Populate trajectory segment tolerances from data in the ROS parameter server.
Definition: tolerances.h:291
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
joint_trajectory_controller::StateTolerances::velocity
Scalar velocity
Definition: tolerances.h:65
DurationBase< Duration >::toSec
double toSec() const
ros::Duration
joint_trajectory_controller::SegmentTolerancesPerJoint::state_tolerance
StateTolerances< Scalar > state_tolerance
Definition: tolerances.h:104
joint_trajectory_controller::SegmentTolerances::state_tolerance
std::vector< StateTolerances< Scalar > > state_tolerance
Definition: tolerances.h:82
joint_trajectory_controller::StateTolerances::position
Scalar position
Definition: tolerances.h:64
ros::NodeHandle
joint_trajectory_controller::updateStateTolerances
void updateStateTolerances(const control_msgs::JointTolerance &tol_msg, StateTolerances< Scalar > &tols)
Update data in tols from data in msg_tol.
Definition: tolerances.h:212


joint_trajectory_controller
Author(s): Adolfo Rodriguez Tsouroukdissian
autogenerated on Fri May 24 2024 02:41:24