RRTConnect: Unable to sample any valid states for goal tree, when adding obstacle to planning scene which is not in the way
I'm controlling a robot arm with a planning scene in which I can add 1 box shaped collision obstacle. When I give a pose target without the box in the scene, the planner immediately finds a plan which would not collide with the box I want to add. After adding the box (2 cm from the end-effector target pose), the planner is not able to find a goal state. After implementing a max of 10 re-planning attempts with a planning time of max 5 seconds I had a few cases where the planner succeeded with a trajectory similar to the plan without the box obstacle. But this often takes more then 10, 5 second attempts.
So if I understand correctly
- without the box, the planner is quickly able to find a plan which often wouldn't even hit the box
- there is a valid goal state reachable with the box present
- with the box, the planner has a really hard time finding this same plan because it cannot find a goal state easily
Rviz shows the collision box at the intended place:
Target pose: blue circle, end-effector: red circle
Edit:
since the successful planning attempts are often faster then 0.25 seconds, i decreased the planning time to 0.25 and increased the number of attempts but in most cases even 100 attempts are not enough :(
Aligning the RViz interactive marker with the targetpose gives a goal state and plan immediately with the box present.
Log without obstacle:
[ INFO] [1592472225.380376591, 9.927000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1592472225.381545645, 9.927000000]: Planner configuration 'right_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1592472225.392477089, 9.928000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1592472225.393170007, 9.928000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1592472225.394860715, 9.928000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1592472225.396387718, 9.929000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1592472225.559092946, 9.942000000]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1592472225.572847128, 9.944000000]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1592472225.574713999, 9.944000000]: RRTConnect: Created 5 states (3 start + 2 goal)
[ INFO] [1592472225.581433461, 9.945000000]: RRTConnect: Created 5 states (3 start + 2 goal)
[ INFO] [1592472225.593754240, 9.946000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.201472 seconds
Log with obstacle (failed attempt):
[ INFO] [1592472566.479733074, 26.371000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1592472566.485185171, 26.371000000]: Planner configuration 'right_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1592472566.494172887, 26.372000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1592472566.495547100, 26.372000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1592472566.496542708, 26.372000000]: RRTConnect: Starting planning with 1 states already in datastructure ...
As far as I remember, since you set a pose goal the planner will try to generate "online" a number of goal states in the configuration space. I might be wrong, but it should use the IK solver for this. From the "failure" log, it seems to me that the planner is struggling to find any valid IK solution corresponding to the goal pose - note the message "Unable to sample any valid states for goal tree". To check if this is the case, you could try to manually generate few valid joint configurations corresponding to your goal pose, and see if the planner has any issue with planning from the start state to these (joint) goals. If everything runs fine, I believe that the problem is indeed with the IK solver. You could then try to investigate more in details why it is failing - perhaps the configuration of its parameter ...(more)
I am using the default KDL plugin. Joint value target seems to work fine. The strange thing is that when I move the interactive marker in RViz to the pose target, it does show the goal state immediately and plans without problems. And I assume that it would be using the same IK solver. I will do some more testing
When you use the marker in RViz, the IK plugin is indeed used to compute the joints, so that you can see "online" what the joint state would be at the goal. Then, when you press on "plan", the goal is set as the joint state goal, while the maker is totally ignored. I hope this bit is clear, if not I'll rephrase it ;)
From what you said in your comment, I think that my hypothesis is correct, and the problem is with the IK plugin not being able to sample valid states. Regarding the reasons, there might be many... From the pic, I guess the arm is a 7dof anthropomorphic manipulator? In my experience, the KDL plugin can some time have a hard time with redundant robots. You could then try to see if using another plugin, e.g., TracIK, works for you. There is also IKFast, but ...(more)