Validity/return state of moveit's group.plan

asked 2019-03-29 22:16:04 -0500

karthikb
1 ●1 ●1 ●1

updated 2022-01-22 16:09:56 -0500

Evgeny
26 ●6

Hello All,

How do I figure out if the motion planner found a solution when invoking moveit's moveit_commander.MoveGroupCommander.plan()? As an example, when I run this snippet:

zrange = np.linspace(1,0.3,10)
for z in zrange:
   pose_target.position.z = z

   print("Moving to new pose z pos %f " % pose_target.position.z)

   # set the new pose
   group.set_pose_target(pose_target)

   # Now call the planner to compute the plan.
   plan1 = group.plan()

   # to execute the plan call, group.go
   group.go(wait=True)

   # sleep for a while
   rospy.sleep(0.3)

My robot can't go down to Z=0.3 mm, and what I see on the movit_config window is a whole bunch of errors for some z positions:

[ INFO] [1553915255.455107181, 4593.181000000]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ WARN] [1553915255.455373951, 4593.181000000]: RRTConnect: Skipping invalid start state (invalid bounds)
[ERROR] [1553915255.455499924, 4593.181000000]: RRTConnect: Motion planning start tree could not be initialized!
[ INFO] [1553915255.455590190, 4593.181000000]: No solution found after 0.000222 seconds
[ WARN] [1553915255.465173476, 4593.191000000]: Goal sampling thread never did any work.
[ INFO] [1553915255.465402585, 4593.191000000]: Unable to solve the planning problem

But before I even issue a group.go() command, how do I check if the computed plan is valid?

Thank you.

Karthik.

answered 2019-03-31 22:46:54 -0500

fvd
2180 ●15 ●98 ●56

updated 2019-04-02 02:09:40 -0500

The plan function returns an error code. You can check this error code for success, using this example from the tutorial:

bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

ROS_INFO_NAMED("tutorial", "Plan 1 (pose goal) %s", success ? "succeeded" : "FAILED");

All the error codes are listed here.

The Python interface also has a plan function. For reference, all the Python move_group's functions are defined in this file. Change to the branch of your release (melodic-devel, kinetic-devel...) if you are not building from source.

The function returns the plan instead of an error code. To check for success, you can check that the trajectory is not empty:

plan = move_group.plan()
if plan.joint_trajectory.points:  # True if trajectory contains points
  move_success = move_group.execute(plan)
else:
  rospy.logerr("Trajectory is empty. Planning was unsuccessful.")

This should work in Melodic (and Kinetic, I believe), but the API might be changing in the future (it seems like it is already different in the master branch). Feel free to add this use case to the tutorial. A pull request is very welcome.

PS: The errors in your post look like they are caused by your robot being in an invalid pose. Maybe you are restricting the possible joint angles in your moveit_config, and your robot arm is in a different configuration? E.g. is the elbow flipped, or a joint at +- 2*pi? Does the robot look correct in Rviz? Don't forget to set the orientation for your pose_target, too.