Errors with Informed RRT* OMPL Planner in MoveIt! with Baxter
I'm using ROS Kinetic with a Baxter Gazebo simulator and the baxtermoveitconfig 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.
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 omplplanning.yaml file accordingly. The catkin workspace builds fine and I can launch the whole planning scene using the usual baxtergrippers.launch file in the baxtermoveitconfig 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.
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. However, 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 ]
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.
Asked by petersimi on 2019-02-20 21:13:07 UTC
Comments