Errors with Informed RRT* OMPL Planner in MoveIt! with Baxter

[anthonysimeonov]

Environment

I'm using ROS Kinetic with a Baxter Gazebo simulator and the baxter_moveit_config package, running on Ubuntu 16.04 with kernel 4.15.0-42-generic. I have OMPL version 1.4.2 and MoveIt! kinetic-devel branch both installed from source.

Description

I am working on motion planning experiments on the simulated Baxter robot in MoveIt, using various OMPL default planners. So far I have had success registering and running planning tests in my various environments using RRT-Connect, RRT*, and BIT*. Today I went to try and start testing Informed RRT* by registering it as a new default planner in MoveIt, and updating my Baxter ompl_planning.yaml file accordingly. The catkin workspace builds fine and I can launch the whole planning scene using the usual baxter_grippers.launch file in the baxter_moveit_config package. Informed RRT* is successfully loaded as the planner that will be used for any motion plan requests.

However, when I try to create a planning request from the MoveitCommander Python API (as I have for all my tests) I get a variety of ROS Error messages in the console and the planner immediately returns a failure and moves on to the next planning request. Strangely, this behavior is not consistent -- MoveIt is occasionally able to successfully start and complete a full motion plan request with Informed RRT*, about 10% of the time. I cannot currently see any correlation with the planning requests where it succeeds versus where it fails and throws errors.

Expected behaviour

Below is an example of the console output when the planner works as expected.

[ INFO] [1550713417.195105342, 55.565000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1550713417.195209687, 55.565000000]: Starting state is just outside bounds (joint 'right_s1'). Assuming within bounds.
[ INFO] [1550713417.196379665, 55.566000000]: Planner configuration 'right_arm' will use planner 'geometric::InformedRRTstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1550713417.196687350, 55.567000000]: InformedRRTstar: problem definition is not set, deferring setup completion...
[ INFO] [1550713417.196966267, 55.567000000]: InformedRRTstar: Using informed sampling.
[ INFO] [1550713417.211630536, 55.582000000]: InformedRRTstar: Started planning with 1 states. Seeking a solution better than 0.00000.
[ INFO] [1550713417.211670248, 55.582000000]: InformedRRTstar: Initial k-nearest value of 607
[ INFO] [1550713419.378140552, 57.737000000]: InformedRRTstar: Found an initial solution with a cost of 17.62 in 3006 iterations (1156 vertices in the graph)
[ INFO] [1550713422.199787949, 60.550000000]: InformedRRTstar: Created 2519 new states. Checked 1906784 rewire options. 10 goal states in tree. Final solution cost 17.276
[ INFO] [1550713422.199839747, 60.550000000]: Solution found in 5.003105 seconds
[ INFO] [1550713422.244673189, 60.595000000]: SimpleSetup: Path simplification took 0.044764 seconds and changed from 5 to 6 states
[ INFO] [1550713422.246356340, 60.596000000]: Planning adapters have added states at index positions: [ 0 ]

Actual behaviour

Below are some examples of the errors and warnings I obtain (in no particular order, these all show up somewhat randomly and the errors all cause the planner to fail).

[ERROR] [1550708380.811518404, 19661.729000000]: Exception caught executing *next* adapter 'Fix Start State Bounds': PathLengthDirectInfSampler: There must be at least 1 start and and 1 goal state when the informed sampler is created.

[ERROR] [1550708380.800947757, 19661.720000000]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': PathLengthDirectInfSampler: There must be at least 1 start and and 1 goal state when the informed sampler is created.

[ERROR] [1550708380.790290485, 19661.711000000]: Exception caught executing *next* adapter 'Fix Start State In Collision': PathLengthDirectInfSampler: There must be at least 1 start and and 1 goal state when the informed sampler is created.

[ERROR] [1550708380.775913789, 19661.698000000]: Exception caught executing *next* adapter 'Fix Workspace Bounds': PathLengthDirectInfSampler: There must be at least 1 start and and 1 goal state when the informed sampler is created.

[ERROR] [1550713422.840970844, 61.159000000]: InformedRRTstar: There are no valid initial states!

[ WARN] [1550713422.840896038, 61.159000000]: InformedRRTstar: Skipping invalid start state (invalid bounds)

[ WARN] [1550713422.743253679, 61.071000000]: Goal sampling thread never did any work.

They all generally have to do with the MoveIt planning adapters and the start state of the robot. However, throughout all of my experiments I have used exactly the same start state (the neutral pose the Baxter starts in when the Gazebo world is launched) and it has never had an issue when using the other planners. I have also tried running this test after manually moving the start state to a few different configurations and the errors persist. Clearly the planner is not being set up properly in each of these cases.

To reproduce

I don't have a docker at the moment to reproduce, and to avoid dependence on the Baxter simulation environment I've demonstrated the issue by using the MoveIt panda robot tutorial (the errors that arise are the same).

Steps:

• Register Informed RRT* as a default MoveIt planner by editing planning_context_manager.cpp in the moveit_planners_ompl package:

#include <ompl/geometric/planners/rrt/InformedRRTstar.h>

and in void ompl_interface::PlanningContextManager::registerDefaultPlanners(), add at the bottom

registerPlannerAllocator(   //
      "geometric::InformedRRTstar",  //
      std::bind(&allocatePlanner<og::InformedRRTstar>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));

• Modify the config/ompl_planning.yaml file in the panda_moveit_config package to load Informed RRT* as the default planner for the Baxter planning groups.

at the bottom of planner_configs:

  InformedRRTStarkConfigDefault:
    type: geometric::InformedRRTstar

and for the planning groups (I generally just change the default for all of them, panda_arm shown as example)

panda_arm:
  default_planner_config: InformedRRTStarkConfigDefault
  planner_configs:
    - InformedRRTStarkConfigDefault

• rebuild the workspace
• Run the MoveIt Move Group Python Interface tutorial, as described at http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html

in one shell

roslaunch panda_moveit_config demo.launch

and in another shell

roslaunch baxter_moveit_config baxter_grippers.launch 

and go through the tutorial.

If anyone has an idea of where these issues may be coming from, and advice as to how I can fix them and use the OMPL implementation of Informed RRT* with MoveIt I will very much appreciate it.

[welcome[bot]]

Thanks for reporting an issue. We will have a look asap. If you can think of a fix, please consider providing it as a pull request.

[jonbinney]

@anthonysimeonov thanks for reporting, this, and really appreciate you taking the time to reproduce the error with the panda robot. I don't know the state of InformedRRT* within OMPL - this sounds like it might just be buggy?

In cases like this debugging can be a bit tricky; when a bug is in OMPL you have to dig through a bunch of layers of stack trace to get at the variables that are directly involved. If you want to dive in to debugging, I'd recommend running this in GDB, setting a breakpoint inside the OMPL planner, and looking at the variables that OMPL has. Also probably worth asking the OMPL folks whether there are known issues with that planner.

[rofgmd]

Hello, I'd like to ask if your moveit is installed with source code. I can’t execute the trajectory while using baxter or Baxter simulation

[wkinger]

Hello,I have met this problem too, could anyone have a good way to implement Informed RRT* in MoveIt? Thanks a lot

[74284853]

Hello,I have met this problem too, could anyone have a good way to implement Informed RRT* in MoveIt? Thanks a lot

Have you resolved the problem？

[wkinger]

sorry,I do not find a way to solve it yet.

---Original--- From: @.> Date: Wed, Nov 1, 2023 15:45 PM To: @.>; Cc: @.@.>; Subject: Re: [ros-planning/moveit] Errors with Informed RRT* OMPL Planner inMoveIt! with Baxter (#1361)

Hello,I have met this problem too, could anyone have a good way to implement Informed RRT* in MoveIt? Thanks a lot

Have you resolved the problem？

— Reply to this email directly, view it on GitHub, or unsubscribe. You are receiving this because you commented.Message ID: @.***>