The generated trajectory is too close to obstacles

asked 2019-04-18 09:29:10 -0500

hanzhaq gravatar image

updated 2019-04-18 10:23:08 -0500

The robot (Fetch) is going to pick up something on a table in front of him. I added the octomap (octomap_resolution=0.01) but the robot keeps colliding with the table, especially the table edge.

I think it is because the generated trajectory points are too close to obstacles and the generated trajectory is too sparse (around 12 trajectory points). Even though all trajectory points are avoiding obstacles but the robot arm still moves through table edge because the line between one trajectory point and the another one are through the table edge. (See the pictures appended)

I am currently using setPoseTargetof the MoveGroup class (moveit::planning_interface::MoveGroup). I tried the computeCartesianPath method because I can fine-tune the double eef_step parameter to but it won't give me a solution mostly, i.e., no motion plan found.

So my questions are:

  1. Is there a way to set the distance to obstacles (or octomap obstacles specifically)?
  2. Is there a way to increase the number of trajectory points in MoveGroup so move_group.plan can return more trajectory points?

I am using Melodic on Ubuntu 18.04 and the binary version of moveit (1.0.1-0bionic.20190320.192509).

Here are some log from terminal:

[ INFO] [1555596197.327901434] [MoveGroupMoveAction::executeMoveCallback_PlanOnly]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1555596197.331271450] [ModelBasedPlanningContext::useConfig]: Planner configuration 'arm_with_torso' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1555596197.332833512] [OMPLPlannerManager::OMPLPlannerManager]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1555596197.364304111] [OMPLPlannerManager::OMPLPlannerManager]: RRTConnect: Created 6 states (2 start + 4 goal)
[ INFO] [1555596197.364438718] [OMPLPlannerManager::OMPLPlannerManager]: Solution found in 0.032557 seconds
[ INFO] [1555596197.414208802] [OMPLPlannerManager::OMPLPlannerManager]: SimpleSetup: Path simplification took 0.049537 seconds and changed from 5 to 5 states

trajectory point n (on the right side of the table): trajectory point n (on the right side of the table)

trajectory point n+1 (on the back side of the table) trajectory point n+1 (on the back side of the table)

a more closer look: a more closer look

edit retag flag offensive close merge delete


Don't have time to dig into 1. right now, but for 2., you can shorten the longest_valid_segment_fraction in the fetch_moveit_config (see ). I suggest starting with 0.01, seeing if that helps. This will check for collisions more densely while planning.

The final path might still have fewer points, but it will have been checked at more points. These extra collisions along a path seem to be common with Fetch, especially with it's gripper.

BryceWilley gravatar image BryceWilley  ( 2019-04-18 09:37:15 -0500 )edit

Still have the same problem. See the photo I just appended to the question...

hanzhaq gravatar image hanzhaq  ( 2019-04-18 10:23:28 -0500 )edit

From your last picture, it doesn't look like the fetch would collide with the table, even interpolating through that path (you can verify this by making longest_valid_segment_fraction even smaller). The fetch generally won't follow the exact path given to it, so if it's off by even a few millimeters, it'll collide with the table. I'd suggest trying to fix this through the octomap instead.

BryceWilley gravatar image BryceWilley  ( 2019-04-18 12:53:41 -0500 )edit