Skip to content

Commit

Permalink
Allow single IK attempt by using minimal timeout
Browse files Browse the repository at this point in the history
The zero timeout used before was reset to DefaultIKTimeout, still causing some randomly seeded additional attempts
  • Loading branch information
rhaschke authored and LeroyR committed Jul 11, 2024
1 parent 9ac8d31 commit e8916b7
Showing 1 changed file with 3 additions and 2 deletions.
5 changes: 3 additions & 2 deletions moveit_core/robot_state/src/cartesian_interpolator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -157,9 +157,10 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons
Eigen::Isometry3d pose(start_quaternion.slerp(percentage, target_quaternion));
pose.translation() = percentage * rotated_target.translation() + (1 - percentage) * start_pose.translation();

// Explicitly use a single IK attempt only: We want a smooth trajectory.
// Explicitly use a single IK attempt only (minimal timeout): We want a smooth trajectory.
// Random seeding (of additional attempts) would probably create IK jumps.
if (start_state->setFromIK(group, pose * offset, link->getName(), consistency_limits, 0.0, validCallback, options))
if (start_state->setFromIK(group, pose * offset, link->getName(), consistency_limits,
std::numeric_limits<double>::epsilon(), validCallback, options))
traj.push_back(std::make_shared<moveit::core::RobotState>(*start_state));
else
break;
Expand Down

0 comments on commit e8916b7

Please sign in to comment.