MoveIt collision checking and planning scene update speed up

edit

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.