Measure the shortest distance from the robot to each voxel in the octree

[changqinghuang]

Hello

I want to measure the shortest distance from all voxels in the octree to the robot UR5, but I'm having trouble. My scene only includes the floor, UR5, and octree (with a lot of voxels inserted in between). I loop through all possible robot joint configurations and measure the shortest distance from each voxel in the octree to UR5 in each configuration.

Currently, I can't find an API to get the handle of each voxel in the official manual, only an API to get the handle of the octree. Is it because voxels are not entities? sim.checkDistance(int entity1Handle, int entity2Handle, float threshold = 0), since I can't get the handle of the voxel, I can only get the shortest distance from the octree to UR5, and I need the distance from each voxel to UR5.

Cheers

[coppelia]

Hello,

you will have to first get the position of all voxels, then do as many distance calculations as you have voxels. Something like:

Code: Select all

local ocTreePose = sim.getObjectPose(ocTreeHandle)
local ocTreeVoxels = sim.getOctreeVoxels(ocTreeHandle)
ocTreeVoxels = sim.multiplyVector(ocTreePose, ocTreeVoxels)
local threshold = 0
for i = 1, #ocTreeVoxels // 3 do
    local p = {ocTreeVoxels[3 * (i - 1) + 0], ocTreeVoxels[3 * (i - 1) + 1], ocTreeVoxels[3 * (i - 1) + 2]}
    sim.setObjectPosition(dummyHandle, p)
    local r, dist, sim.checkDistance(dummyHandle, robotCollection, threshold)
    if r > 0 then
        minDist = dist[7]
        threshold = minDist
    end
end

Above uses a dummy for each voxel position. But one could also use a cube shape instead.

Cheers

[changqinghuang]

Thank you very much for your prompt and helpful response. The solution you provided was invaluable and I was able to successfully apply it to solve the problem at hand.
Thanks again for your help!