MoveIt collision checking and planning scene update speed up

Question

Hello, I am interested in speeding up the MoveIt pipeline of planning scene update and collision checking.

I currently have a planning_scene_monior set up. For planning scene update and collision checking, I do,

1. update the current planning scene by publishing new obstacle poses through a service call of "apply_planning_scene"

2. get a locked copy of the current planning scene by

   planning_scene_monitor->requestPlanningSceneState(); planning_scene_monitor::LockedPlanningSceneRO ps(planning_scene_monitor);

3. actual collision checking

   ps->isStateColliding(*kinematic_state)

I measured the time it took to do these steps and found the following,

for doing step 1), 2) and 3) 20 times, step 1) and step 2) combined in total took 78ms and step 3) in total only took 3ms. Thus, the actual scene update took much longer time than the actual collision checking! Collision checking in this case only requires checking a 6-DOF robot arm with a few shape primitives. I am using ROS Indigo with Ubuntu 14.04.

I am now very interested in speeding up this process (especially step 1 and 2). Please let me know if you have any ideas! Thanks in advance.

---

Originally posted by Huitan on ROS Answers with karma: 63 on 2018-03-27

Post score: 2

---

Original comments

Comment by gvdhoorn on 2018-03-27:
This is a cross-post of MoveIt collision checking and planning scene update speed up on moveit-users. Please don't cross-post questions like this. At best it leads to split discussions, but more likely to duplication of effort.

Comment by Huitan on 2018-03-27:
Sorry for the inconvenience. I will remember next time and also spread the word.

Comment by CesareT on 2023-02-28:
Hi, have you solved it?

Answer 1

Does your application actually warrant speeding step #1 & 2 up? In most applications, the planning scene is fairly static (the world doesn't change that fast) and you want to run step 3 many, many times, with different kinematic states to check a variety of poses the robot may plan to/through -- thus while the planning scene setup may take a while, it should be reusable and the setup time often still pales in comparison to the many collision checking calls.

---

Originally posted by fergs with karma: 13902 on 2018-03-27

This answer was ACCEPTED on the original site

Post score: 0

---

Original comments

Comment by Huitan on 2018-03-27:
Thanks for the reply. I do need to speed up step 1 and 2 since I am planning with dynamic obstacles. My goal is to periodically evaluate a set of trajectories based on the new scenes. Do you think it will help if I maintain my own planning scene instead of relying on the move_group node to do that?