MoveIt unable to reliable find motion plan

asked 2021-05-11 21:25:58 -0500

juular gravatar image

updated 2021-05-12 08:55:15 -0500

I have multiple static endpoint poses which my 7-dof robot arm (sawyer from rethink robotics) is supposed to move to. The issue is that sometimes moveit is unable to find a valid path. The endpoints are definitely reachable, as sometimes a valid path is found. Strangely enough even if a path from one point to another is found, often the reverse is not working, although it would only have to backtrack the same motions. The issue seems to stem from the orientation of the goal pose. Also sometimes the found path is really excessive, which makes me assume that the robot state is not properly propagated to moveit.

I tried multiple different ompl algorithms, increased planing time and numer of planing attempts. Increasing the goal tolerance to really large values works, but this is no satisfying solution. Setting the goal orientation to (0, 0, 0, 1) also works. My quaternion is normalized and has high precision.

I use the python move_group interface. The code looks something like this:

# Planing: interface_positions contains the pose of the endpoint goal
self.group.clear_pose_targets()
plan = self.group.plan(self.interface_positions[interface])

# Executing:
self.group.execute(plan)
self.group.stop()

The output im getting from moveit when no motion plan is found looks like this:

[ INFO] [1620779790.924320121, 21713.911000000]: right_arm[RRTstarkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1620779790.924383780, 21713.912000000]: right_arm[RRTstarkConfigDefault]: Initial k-nearest value of 3
[ INFO] [1620779795.916530371, 21715.323000000]: ProblemDefinition: Adding approximate solution from planner right_arm[RRTstarkConfigDefault]
[ INFO] [1620779795.921356170, 21715.325000000]: right_arm[RRTstarkConfigDefault]: Created 911 new states. Checked 18590 rewire options. 0 goal states in tree. Final solution cost inf
[ INFO] [1620779795.930454935, 21715.329000000]: ParallelPlan::solve(): Solution found by one or more threads in 5.006643 seconds
[ WARN] [1620779795.930512517, 21715.329000000]: Computed solution is approximate
[ INFO] [1620779795.930549658, 21715.329000000]: Unable to solve the planning problem

I would be really grateful if somebody has any idea what the problem may be or even how i can get some more information out of moveit so i can determine what this behaviour causes.

Edit: Two of these poses between which no motion plan can be found most of the time look like this: this

edit retag flag offensive close merge delete

Comments

This sounds like you're facing common problems with motion planning. It might help if you showed your scene, and images of example start and goal poses.

fvd gravatar image fvd  ( 2021-05-12 00:33:58 -0500 )edit

Seeing as pose goals are used: which IK solver is used? Sawyer is 7 dof, so there's quite a few possible solutions mapping to a single pose.

gvdhoorn gravatar image gvdhoorn  ( 2021-05-12 02:49:38 -0500 )edit

@fvd I added some pictures to the question. There are no obstacles in the scene.

@gvdhoorn I'll have to say i have no idea. The default ik solver seem to be kdl. How would one go about configuring an alternative? The move_group interface seems to provide no api for ik.

juular gravatar image juular  ( 2021-05-12 08:58:12 -0500 )edit

If you plan between cartesian poses, the IK solver might request an unfavorable joint configuration. Large motions like between these two configurations are more predictable if you use joint state goals (e.g. named states defined in your SRDF).

Also, how close to the obstacle is that second pose? If it's close it might just be genuinely hard to reach from different orientations. Try planning to a goal a little further up and then moving with a cartesian motion to the actual target.

fvd gravatar image fvd  ( 2021-05-13 01:27:35 -0500 )edit