RRTconnect with 6DOF pose goal in 30s planning time returns failure after 2s

[ros-planning]

RRTconnect with 6DOF pose goal in 30s planning time returns failure after 2s

[ryanluna]

The ConstrainedGoalSampler will only attempt to sample a finite number of states (default is 1000) before declaring failure. This parameter is stored in the PlanningContext, a MoveIt concept, not an OMPL concept. If every goal state is invalid, the goal sampler stops sampling and RRTConnect quits early because there is no root for the goal tree.

Since no valid goal states are sampled, I suspect a problem with how you construct your goal constraints.

[arjungm]

Thanks for your comment! That explains why it only tries 1000 calls to IK.

I thought my goal pose was incorrect at first, but I derived the 6DOF goal pose from a collision-free set of joint positions in the planning scene (just moving arms around with interactive markers).

It bears some investigation if there's a mismatch in the goal pose it is trying to solve an IK for in the Goal Sampling Thread, and the goal pose I grabbed from the scene. I will try this out, and report back!

[arjungm]

Alright so I managed to figure out why I wasn't able to find an IK for a pose that should most definitely have a solution.

In the goal sampling thread, an outer loop iterates 1000 times (default) calling constraint_samplers::ConstrainedSampler::project.

The method resolves to constraint_samplers::IKConstraintSampler::project.

This calls IKConstraintSampler::sampleHelper with the project argument as true.

Then in the method, it loops 4 times (default) and calls IKConstraintSampler::callIK each time. Note the last argument is (project && a==0).

This means IKConstraintSampler::callIK is called with the use_as_seed argument as true the first inner iteration, and then false for the remaining attempts in loop in IKConstraintSampler::sampleHelper.

Inside IKConstraintSampler::callIK, it checks if there is a StateValidityCallback available. If so, it runs inverse kinematics with the callback. If not, it runs IK without checking for collisions, Line 564.

Because adapted_ik_validity_callback is not available, then the search for IK is done and a solution is found (possibly in collision). Because use_as_seed is true, it uses the passed in state as a seed.

Then it saves the solution back into the state passed in as a seed. This state is a reference to workstate from before.

Then it always returns true back to IKConstraintSampler::sampleHelper which then exits out of the inner loop immediately and returns true back to the outer loop in ConstrainedGoalSampler::sampleUsingConstraintSampler.

From here it checks for collisions.

If the collision check fails, it loops for another attempt and passes the workstate that is initialized to the previous IK solution (which is in collision) to IKConstrainstSampler::callIK as the seed because project && a==0 is true for the first inner attempt.

callIK returns the same solution as before, leading to a cycle that exhausts all 1000 attempts, and it never uses a randomized state because each callIK succeeded ( a solution was found ).

I attempted a fix for this issue and found that it let's RRTConnect succeed in planning to positions that are kind of cramped, where as before it would just declare failure to find an IK.

Pull requests incoming.

[arjungm]

Here's the pull requests that should fix it:

Adds a bool flag that forces random seed to ConstrainedSampler::project and its derived classes
ros-planning/moveit_core#188

Makes use of above bool flag, after failing to sample a goal once
#46

[davetcoleman]

Great write up! I'm trying to understand your issue, see comments in first PR.

[arjungm]

In this most recent pull request, you don't need to change the moveit_core classes for constraint samplers.

Instead, by setting the validity callback function means that randomization of the seed will be taken care of by IKConstraintSampler if it fails to find a valid IK.

Line 525 the call to IK is made. a==0 so the current state is used as a seed.
Line 564 the searchPositionIK uses the validity callback because it is set.

If the state it finds is invalid, it will correctly return false. Then it will loop again to Line 525 and now a!=0 so the use_as_seed flag is now false. This triggers a random seed for the remaining iterations.

[mikeferguson]

looks like this was fixed