From d732920f36989c31d353227577119be77b610b3f Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Thu, 3 Sep 2020 15:41:06 +0100 Subject: [PATCH] [effort_controllers] JointGroupPositionController: Check output of angles::shortest_angular_distance_with_large_limits The current implementation does not check the output of angles::shortest_angular_distance_with_large_limits which indicates if the returned distance is an unspecified (and potentially wrong) case. This commit checks the return statement and adds a detailed warning. Secondly, the implementation in angles::shortest_angular_distance_with_large_limits is only valid if the `from` argument is within the range given by the upper and lower joint limit. As thus, the current_position is now enforced/clamped to the joint limits. --- .../src/joint_group_position_controller.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/effort_controllers/src/joint_group_position_controller.cpp b/effort_controllers/src/joint_group_position_controller.cpp index 2cb03411c..ad7e536ec 100644 --- a/effort_controllers/src/joint_group_position_controller.cpp +++ b/effort_controllers/src/joint_group_position_controller.cpp @@ -138,12 +138,26 @@ namespace effort_controllers // Compute position error if (joint_urdfs_[i]->type == urdf::Joint::REVOLUTE) { - angles::shortest_angular_distance_with_large_limits( + // angles::shortest_angular_distance_with_large_limits assumes implicitly (and only works correctly) if + // current_position (`from`) is within the valid range. As thus, we need to enforce the joint limits + // prior to passing it as an argument to angles::shortest_angular_distance_with_large_limits: + enforceJointLimits(current_position, i); + + bool is_valid = angles::shortest_angular_distance_with_large_limits( current_position, command_position, joint_urdfs_[i]->limits->lower, joint_urdfs_[i]->limits->upper, error); + + // Warn if angles::shortest_angular_distance_with_large_limits indicates that it could not correctly + // determine the error. This could potentially lead to unsafe behaviour. + // Note, an epsilon margin to reduce the enforced joint limits by a small epsilon could resolve a + // number of floating point issues in angles::shortest_angular_distance_with_large_limits. + if (!is_valid) + { + ROS_WARN_STREAM("Error in angles::shortest_angular_distance_with_large_limits for joint #" << i << ", q_current=" << current_position << ", q_command=" << command_position << ", q_lb=" << joint_urdfs_[i]->limits->lower << ", q_ub=" << joint_urdfs_[i]->limits->upper << ", error=" << error << " - NOT SAFE!"); + } } else if (joint_urdfs_[i]->type == urdf::Joint::CONTINUOUS) {