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 tolerancePose(3, 0.0001); // testing values; I've also tried higher values
std::vector 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). It could also be something entirely different.
I would appreciate any debugging steps or notes on how to resolve this issue.
Thank you!
↧