Ask Your Question
0

MoveIt! path final position is off significantly

asked 2017-11-02 13:11:02 -0600

rivertam gravatar image

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.header.frame_id = "world";
    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 =
            kinematic_constraints::constructGoalConstraints("ee_link", pose, tolerancePose, toleranceAngle);
    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);
    const auto & eeLinkTranslate = this->planningScene->getCurrentStateNonConst().getGlobalLinkTransform("ee_link").translation();
    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 flag offensive close merge delete

Comments

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

rivertam gravatar imagerivertam ( 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.

rivertam gravatar imagerivertam ( 2017-11-02 13:11:20 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-11-09 15:55:23 -0600

rivertam gravatar image

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.

edit flag offensive delete link more

Comments

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?

pbeeson gravatar imagepbeeson ( 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.

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

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-11-02 13:05:29 -0600

Seen: 73 times

Last updated: Nov 09 '17