MoveIt - Check if single point is in collision with the robot

edit

Hi everyone,

for a project I want to scan the environment with a stereo camera that is mounted on an 6 axis robotic arm. The goal is to generate a map of the surrounding of the robot to plan an optimal path with MoveIt. The problem is now, that every time the camera "sees" the robot itself, it gets mapped and so it is handled as a collision object.

So before I map the environment I want to modify the pointcloud that comes from the stereo camera (Intel RealSense D435) in a way that every point that is in collision with the robot gets ignored.

I already tried to generate a collision object (small box) for every point in the pointcloud and check the planning scene for collisions. Sadly that is way to slow. Is there a faster way to just check if a point is in collision with the current robot model? Or does anyone has any other idea to solve that problem?

Thank you in advance for any kind of help!

(I'm working with Ubuntu 16.04, ROS Kinetic, MoveIt, UR10, Realsense D435)