RRTConnect/TRAC IK fails only on Distance solve_type

[Ben Berman]

Hello everyone!

So I'm trying to use MoveIt! with OMPL and TRAC IK. The default solve_type for TRAC IK is "Speed", but this is giving weird, inaccurate answers, so I'd like to try to change the solve_type to "Distance". RRTConnect and TRAC IK acknowledge the change in the console, but I'm getting an error 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?

Thank you!

[Ben Berman]

Partial answer found: My timeout was too high. I suppose one or more of MoveIt!, OMPL, and RRTConnect has a timeout that is significantly less than 10 seconds (as it should be), so TRAC IK was inadvertently violating the other timeout. Removing the timeout parameter (thus restricting it to its default timeout, which I believe is 0.05s) makes the error go away, although it is now responding with only 0s for joint angles (hence why this is only a partial answer).

[Ben Berman]

Sorry to spam, especially because it's not particularly relevant to the topic at hand, but I was incorrect about it responding with only 0s. I was looking at the wrong point. =)