Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[effort_controllers] JointGroupPositionController: Check output of angles::shortest_angular_distance_with_large_limits #509

Open
wants to merge 1 commit into
base: noetic-devel
Choose a base branch
from
Open

[effort_controllers] JointGroupPositionController: Check output of angles::shortest_angular_distance_with_large_limits #509

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 15 additions & 1 deletion effort_controllers/src/joint_group_position_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down