RRTConnect/TRAC IK fails only on Distance solve_type

Question

So I'm trying to change the solve_type to "Distance" as opposed to "Speed" for TRAC IK. RRTConnect and TRAC IK acknowledge the change, but I'm getting an error in the console and a failed compute solve. (it doesn't fail with Speed, and the target position I'm trying is essentially the start position).

The error is as follows:

Error:   RRTConnect: Unable to sample any valid states for goal tree
     at line 215 in /tmp/binarydeb/ros-kinetic-ompl-1.2.1/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp

The code I'm using to solve is as follows:

    planning_interface::MotionPlanRequest req;
    planning_interface::MotionPlanResponse res;

    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 = 1;

    std::vector<double> tolerancePose(3, 0.01);
    std::vector<double> toleranceAngle(3, 0.01);

    req.workspace_parameters.min_corner.x = -50;
    req.workspace_parameters.min_corner.y = -50;
    req.workspace_parameters.min_corner.z = -50;
    req.workspace_parameters.max_corner.x = 50;
    req.workspace_parameters.max_corner.y = 50;
    req.workspace_parameters.max_corner.z = 50;
    req.group_name = "arm";
    req.start_state.joint_state = this->getCurrentJointState(true);
    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);

Again, it works with "Speed" but not with "Distance".

My kinematics.yml file looks like this:

arm:
  solve_type: Distance
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_timeout: 10

I wouldn't mind changing to a different planner (if I could figure out how), but I think TRAC IK is right for our application.

Is there a way I can fix it? Am I not including the right information in the request?

---

Originally posted by rivertam on ROS Answers with karma: 56 on 2017-11-09

Post score: 0

Answer 1

My problem was actually that my timeout was too high. I suppose MoveIt! or OMPL somewhere down the line has its own timeout that TRAC_IK was violating because it thought it had more time. Removing the timeout parameter makes the error go away (though the solve is incorrect, but I'm still working on that; any input would be nice)

---

Originally posted by rivertam with karma: 56 on 2017-11-09

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by gvdhoorn on 2017-11-10:\

I suppose MoveIt! or OMPL somewhere down the line has its own timeout

that is most likely the regular planning request timeout, which is 5 seconds by default.