I want to generate a 3D Occupancy Map in Simulink that includes static environment, target object, dynamic obstacles and a robotic manipulator that shall move through the environment to pick up objects avoiding the obstacles during the handling task. I want to use the Occupancy Map to plan collision-free paths.
I am trying to figure out a efficient way to calculate the occupancy of voxels. I have the TCP Pose and the Joint configurations of the robot. So i can calculate how the robot is positioned. I would like to use cylinders as approximation for links. I know how to check if points are inside cylinders of arbitrary orientation. But i want to avoid checking the whole 3D Mapp for every Object again. I could potentially use the center of a link to choose a smaller region of voxels because I know from geometry that only a certain area is possible.
My question is if there is a more efficient way to calculate occupancy in R2016b where I am able to just input the configuration and by this already tell the map whch voxels are occupied? Similar to R2018a robotics.OccupancyMap3D. But I can not use R2018a.