The generated trajectory is too close to obstacles
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:
- Is there a way to set the distance to obstacles (or octomap obstacles specifically)?
- Is there a way to increase the number of trajectory points in
MoveGroupsomove_group.plancan 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+1 (on the back side of the table)
a more closer look:
Don't have time to dig into 1. right now, but for 2., you can shorten the
longest_valid_segment_fractionin the fetch_moveit_config (see https://github.com/fetchrobotics/fetc... ). I suggest starting with0.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.
Still have the same problem. See the photo I just appended to the question...
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_fractioneven 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.