ros2_control - rolling
tolerances.hpp
1 // Copyright 2013 PAL Robotics S.L.
2 // All rights reserved.
3 //
4 // Software License Agreement (BSD License 2.0)
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 // * Redistributions of source code must retain the above copyright notice,
8 // this list of conditions and the following disclaimer.
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 // * Neither the name of PAL Robotics S.L. nor the names of its
13 // contributors may be used to endorse or promote products derived from
14 // this software without specific prior written permission.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 // POSSIBILITY OF SUCH DAMAGE.
27 
29 
30 #ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
31 #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
32 
33 #include <limits>
34 #include <stdexcept>
35 #include <string>
36 #include <vector>
37 
38 #include "control_msgs/action/follow_joint_trajectory.hpp"
39 #include "joint_trajectory_controller_parameters.hpp"
40 
42 {
49 {
50  double position = 0.0;
51  double velocity = 0.0;
52  double acceleration = 0.0;
53 };
54 
59 {
60  explicit SegmentTolerances(size_t size = 0) : state_tolerance(size), goal_state_tolerance(size) {}
61 
63  std::vector<StateTolerances> state_tolerance;
64 
66  std::vector<StateTolerances> goal_state_tolerance;
67 
69  double goal_time_tolerance = 0.0;
70 };
71 
93 SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Params & params)
94 {
95  auto const & constraints = params.constraints;
96  auto const n_joints = params.joints.size();
97 
98  SegmentTolerances tolerances;
99  tolerances.goal_time_tolerance = constraints.goal_time;
100  static auto logger = jtc_logger.get_child("tolerance");
101  RCLCPP_DEBUG(logger, "goal_time %f", constraints.goal_time);
102 
103  // State and goal state tolerances
104  tolerances.state_tolerance.resize(n_joints);
105  tolerances.goal_state_tolerance.resize(n_joints);
106  for (size_t i = 0; i < n_joints; ++i)
107  {
108  auto const joint = params.joints[i];
109  tolerances.state_tolerance[i].position = constraints.joints_map.at(joint).trajectory;
110  tolerances.goal_state_tolerance[i].position = constraints.joints_map.at(joint).goal;
111  tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance;
112 
113  RCLCPP_DEBUG(
114  logger, "%s %f", (joint + ".trajectory.position").c_str(),
115  tolerances.state_tolerance[i].position);
116  RCLCPP_DEBUG(
117  logger, "%s %f", (joint + ".goal.position").c_str(),
118  tolerances.goal_state_tolerance[i].position);
119  RCLCPP_DEBUG(
120  logger, "%s %f", (joint + ".goal.velocity").c_str(),
121  tolerances.goal_state_tolerance[i].velocity);
122  }
123 
124  return tolerances;
125 }
126 
127 double resolve_tolerance_source(const double default_value, const double goal_value)
128 {
129  // from
130  // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg
131  // There are two special values for tolerances:
132  // * 0 - The tolerance is unspecified and will remain at whatever the default is
133  // * -1 - The tolerance is "erased".
134  // If there was a default, the joint will be allowed to move without restriction.
135  constexpr double ERASE_VALUE = -1.0;
136  auto is_erase_value = [=](double value)
137  { return fabs(value - ERASE_VALUE) < std::numeric_limits<float>::epsilon(); };
138 
139  if (goal_value > 0.0)
140  {
141  return goal_value;
142  }
143  else if (is_erase_value(goal_value))
144  {
145  return 0.0;
146  }
147  else if (goal_value < 0.0)
148  {
149  throw std::runtime_error("Illegal tolerance value.");
150  }
151  return default_value;
152 }
153 
164  rclcpp::Logger & jtc_logger, const SegmentTolerances & default_tolerances,
165  const control_msgs::action::FollowJointTrajectory::Goal & goal,
166  const std::vector<std::string> & joints)
167 {
168  SegmentTolerances active_tolerances(default_tolerances);
169  static auto logger = jtc_logger.get_child("tolerance");
170 
171  try
172  {
173  active_tolerances.goal_time_tolerance = resolve_tolerance_source(
174  default_tolerances.goal_time_tolerance, rclcpp::Duration(goal.goal_time_tolerance).seconds());
175  }
176  catch (const std::runtime_error & e)
177  {
178  RCLCPP_ERROR_STREAM(
179  logger, "Specified illegal goal_time_tolerance: "
180  << rclcpp::Duration(goal.goal_time_tolerance).seconds()
181  << ". Using default tolerances");
182  return default_tolerances;
183  }
184  RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance);
185 
186  // State and goal state tolerances
187  for (auto joint_tol : goal.path_tolerance)
188  {
189  auto const joint = joint_tol.name;
190  // map joint names from goal to active_tolerances
191  auto it = std::find(joints.begin(), joints.end(), joint);
192  if (it == joints.end())
193  {
194  RCLCPP_ERROR(
195  logger, "%s",
196  ("joint '" + joint +
197  "' specified in goal.path_tolerance does not exist. "
198  "Using default tolerances.")
199  .c_str());
200  return default_tolerances;
201  }
202  auto i = static_cast<size_t>(std::distance(joints.cbegin(), it));
203  std::string interface = "";
204  try
205  {
206  interface = "position";
207  active_tolerances.state_tolerance[i].position = resolve_tolerance_source(
208  default_tolerances.state_tolerance[i].position, joint_tol.position);
209  interface = "velocity";
210  active_tolerances.state_tolerance[i].velocity = resolve_tolerance_source(
211  default_tolerances.state_tolerance[i].velocity, joint_tol.velocity);
212  interface = "acceleration";
213  active_tolerances.state_tolerance[i].acceleration = resolve_tolerance_source(
214  default_tolerances.state_tolerance[i].acceleration, joint_tol.acceleration);
215  }
216  catch (const std::runtime_error & e)
217  {
218  RCLCPP_ERROR_STREAM(
219  logger, "joint '" << joint << "' specified in goal.path_tolerance has a invalid "
220  << interface << " tolerance. Using default tolerances.");
221  return default_tolerances;
222  }
223 
224  RCLCPP_DEBUG(
225  logger, "%s %f", (joint + ".state_tolerance.position").c_str(),
226  active_tolerances.state_tolerance[i].position);
227  RCLCPP_DEBUG(
228  logger, "%s %f", (joint + ".state_tolerance.velocity").c_str(),
229  active_tolerances.state_tolerance[i].velocity);
230  RCLCPP_DEBUG(
231  logger, "%s %f", (joint + ".state_tolerance.acceleration").c_str(),
232  active_tolerances.state_tolerance[i].acceleration);
233  }
234  for (auto goal_tol : goal.goal_tolerance)
235  {
236  auto const joint = goal_tol.name;
237  // map joint names from goal to active_tolerances
238  auto it = std::find(joints.begin(), joints.end(), joint);
239  if (it == joints.end())
240  {
241  RCLCPP_ERROR(
242  logger, "%s",
243  ("joint '" + joint +
244  "' specified in goal.goal_tolerance does not exist. "
245  "Using default tolerances.")
246  .c_str());
247  return default_tolerances;
248  }
249  auto i = static_cast<size_t>(std::distance(joints.cbegin(), it));
250  std::string interface = "";
251  try
252  {
253  interface = "position";
254  active_tolerances.goal_state_tolerance[i].position = resolve_tolerance_source(
255  default_tolerances.goal_state_tolerance[i].position, goal_tol.position);
256  interface = "velocity";
257  active_tolerances.goal_state_tolerance[i].velocity = resolve_tolerance_source(
258  default_tolerances.goal_state_tolerance[i].velocity, goal_tol.velocity);
259  interface = "acceleration";
260  active_tolerances.goal_state_tolerance[i].acceleration = resolve_tolerance_source(
261  default_tolerances.goal_state_tolerance[i].acceleration, goal_tol.acceleration);
262  }
263  catch (const std::runtime_error & e)
264  {
265  RCLCPP_ERROR_STREAM(
266  logger, "joint '" << joint << "' specified in goal.goal_tolerance has a invalid "
267  << interface << " tolerance. Using default tolerances.");
268  return default_tolerances;
269  }
270 
271  RCLCPP_DEBUG(
272  logger, "%s %f", (joint + ".goal_state_tolerance.position").c_str(),
273  active_tolerances.goal_state_tolerance[i].position);
274  RCLCPP_DEBUG(
275  logger, "%s %f", (joint + ".goal_state_tolerance.velocity").c_str(),
276  active_tolerances.goal_state_tolerance[i].velocity);
277  RCLCPP_DEBUG(
278  logger, "%s %f", (joint + ".goal_state_tolerance.acceleration").c_str(),
279  active_tolerances.goal_state_tolerance[i].acceleration);
280  }
281 
282  return active_tolerances;
283 }
284 
293  const trajectory_msgs::msg::JointTrajectoryPoint & state_error, size_t joint_idx,
294  const StateTolerances & state_tolerance, bool show_errors = false)
295 {
296  using std::abs;
297  const double error_position = state_error.positions[joint_idx];
298  const double error_velocity =
299  state_error.velocities.empty() ? 0.0 : state_error.velocities[joint_idx];
300  const double error_acceleration =
301  state_error.accelerations.empty() ? 0.0 : state_error.accelerations[joint_idx];
302 
303  const bool is_valid =
304  !(state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) &&
305  !(state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity) &&
306  !(state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration);
307 
308  if (is_valid)
309  {
310  return true;
311  }
312 
313  if (show_errors)
314  {
315  const auto logger = rclcpp::get_logger("tolerances");
316  RCLCPP_ERROR(logger, "State tolerances failed for joint %lu:", joint_idx);
317 
318  if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position)
319  {
320  RCLCPP_ERROR(
321  logger, "Position Error: %f, Position Tolerance: %f", error_position,
322  state_tolerance.position);
323  }
324  if (state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity)
325  {
326  RCLCPP_ERROR(
327  logger, "Velocity Error: %f, Velocity Tolerance: %f", error_velocity,
328  state_tolerance.velocity);
329  }
330  if (
331  state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration)
332  {
333  RCLCPP_ERROR(
334  logger, "Acceleration Error: %f, Acceleration Tolerance: %f", error_acceleration,
335  state_tolerance.acceleration);
336  }
337  }
338  return false;
339 }
340 
341 } // namespace joint_trajectory_controller
342 
343 #endif // JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
Definition: interpolation_methods.hpp:24
SegmentTolerances get_segment_tolerances(rclcpp::Logger &jtc_logger, const Params &params)
Populate trajectory segment tolerances using data from the ROS node.
Definition: tolerances.hpp:93
bool check_state_tolerance_per_joint(const trajectory_msgs::msg::JointTrajectoryPoint &state_error, size_t joint_idx, const StateTolerances &state_tolerance, bool show_errors=false)
Definition: tolerances.hpp:292
Trajectory segment tolerances.
Definition: tolerances.hpp:59
std::vector< StateTolerances > goal_state_tolerance
Definition: tolerances.hpp:66
std::vector< StateTolerances > state_tolerance
Definition: tolerances.hpp:63
double goal_time_tolerance
Definition: tolerances.hpp:69
Trajectory state tolerances for position, velocity and acceleration variables.
Definition: tolerances.hpp:49