MoveIt! path final position is off significantly

Hi everyone.

I'm trying to use MoveIt! for path planning. I'm using the TRAC_IK and I set the timeout to 10 seconds (just as a debugging measure), but the final position is consistently off by an inconsistent amount. This is frustrating in particular because I was led to understand the path planners would be pretty much perfect given enough time.

I'm running the planner pretty much as the tutorial suggests I do it:

    geometry_msgs::PoseStamped pose;
pose.pose.position.x = messageTarget.x();
pose.pose.position.y = messageTarget.y();
pose.pose.position.z = messageTarget.z();

pose.pose.orientation.w = 0;

std::vector<double> tolerancePose(3, 0.0001); // testing values; I've also tried higher values
std::vector<double> toleranceAngle(3, 0.0001);

req.group_name = "arm";
moveit_msgs::Constraints poseGoal =
req.goal_constraints.push_back(poseGoal);

planning_interface::PlanningContextPtr context =
this->plannerInstance->getPlanningContext(this->planningScene, req, res.error_code_);

context->solve(res);


and I'm getting back a result as a moveit_msgs::MotionPlanResponse. However, the final point in the response is often off by as much as .07m in any axis (though I will grant that's pretty rare; it's usually closer to 0.03m). I'm checking the final point as such:

    this->updateState(planResponse.trajectory.joint_trajectory.points[0].positions);
logger->info("Moving EE to <{}, {}, {}> (should be <{}, {}, {}>)", eeLinkTranslate[0], eeLinkTranslate[1], eeLinkTranslate[2], messageTarget.x(), messageTarget.y(), messageTarget.z());


I suspect my issue is the inverse kinematics in particular because I've also tried KDL and that seemed to make the problem worse, but it could also be the planner (I've thus far been unable to change the planner. All instructions have failed me. I'll comment with steps I've attempted, but this is not part of my question unless someone thinks it's relevant). It could also be something entirely different.

I would appreciate any debugging steps or notes on how to resolve this issue.

Thank you!

edit retag close merge delete

I've attempted the following to change the planner: 1) Setting a different value for the request's planner_id 2) Removing all other planner_configs from ompl_planning.yaml 3) setting default_planner_config in ompl_planning.yaml, but it always still uses RRTConnect

( 2017-11-02 13:07:25 -0600 )edit

I'm also somewhat unclear on the terminology. By "Planner" I mean RRTConnect or similar, not OMPL.

( 2017-11-02 13:11:20 -0600 )edit

Sort by » oldest newest most voted

The issue was that i wasn't setting TRAC IK's solve_type, so it was defaulting to Speed. To be totally blunt, I'm not sure why Speed is the default option because it's totally cool with giving answers that are off by substantial amounts. Changing to Distance made TRAC IK fulfill the tolerances I required.

more

Speed and Distance both give answers within the correct tolerances. The only difference is that if multiple solutions exist, Distance will try to return a solution that is closest to the current location. Speed just returns the first solution found. Perhaps something else changed at the same time?

( 2017-12-27 14:42:57 -0600 )edit

It's possible, but the commit literally just had that one change in it, and it solved the problem. It was a pretty long time ago, so I don't have the time to reproduce, but I do recall that being the only issue.

( 2017-12-29 15:02:10 -0600 )edit