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.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!
I've attempted the following to change the planner: 1) Setting a different value for the request's
planner_id
2) Removing all otherplanner_configs
fromompl_planning.yaml
3) settingdefault_planner_config
inompl_planning.yaml
, but it always still usesRRTConnect
I'm also somewhat unclear on the terminology. By "Planner" I mean RRTConnect or similar, not OMPL.