negative Signed distance

paperstiger · May'20

1 #1

How to compute negative signed distance, i.e. penetration and direction to move? It seems that now I can only get 0 distance and some non-sense points.

Code snippet is

world = WorldModel()
    create.primitives.box(1, 1, 1, world=world, name='Y')  #, type='GeometricPrimitive')
    create.primitives.box(2, 2, 2, center=[1.4,0,0], world=world, name='Z')  #, type='GeometricPrimitive')
    obj1 = world.terrain(0)
    obj2 = world.terrain(1)
    g1 = obj1.geometry()
    g2 = obj2.geometry()
    vis.add("world", world)
    vis.spin(float('inf'))
    rst = g1.distance(g2)
    print('distance is ', rst.d)
    p1 = [rst.cp1[i] for i in range(3)]
    p2 = [rst.cp2[i] for i in range(3)]
    print(p1, p2)
    print(rst.hasGradients

result is
distance is 0.0
[-0.30000000000000004, 0.5, -0.30000000000000004] [0.4, 0.5, -1.0]

replies 4
views 3.5K
likes 0

krishauser · May'20 Team

There are some geometry types that support penetration depth estimation. Namely, primitive-primitive, volume grid-primitive, and volume grid-point cloud.

Penetration depth estimation between meshes is not (yet?) supported in Python. There are some approximation techniques in the C++ API that may be ok for small penetrations but they are not generally accurate.

paperstiger · May'20 Author

The thing is, even if I uncomment it into

world = WorldModel()
    create.primitives.box(1, 1, 1, world=world, name='Y', type='GeometricPrimitive')
    create.primitives.box(2, 2, 2, center=[1.4,0,0], world=world, name='Z', type='GeometricPrimitive')
    obj1 = world.terrain(0)
    obj2 = world.terrain(1)
    g1 = obj1.geometry()
    g2 = obj2.geometry()
    print(g1.type(), g2.type())
    vis.add("world", world)
    vis.spin(float('inf'))
    rst = g1.distance(g2)
    print('distance is ', rst.d)
    p1 = [rst.cp1[i] for i in range(3)]
    p2 = [rst.cp2[i] for i in range(3)]
    print(p1, p2)
    print(rst.hasGradients)

and the outputs are

GeometricPrimitive GeometricPrimitive
distance is 0.0

and rst.cp1 has zero size

paperstiger · May'20 Author

With the help of someone else, I can adapt code from

semiinfinite/geometryopt.py @master

from __future__ import print_function,division
from klampt import *
from klampt.math import vectorops,so3,se3
from klampt.model.trajectory import Trajectory,RobotTrajectory
from sip import *
import numpy as np
import scipy

try:
    import pyccd
    PYCCD_ENABLED = True
except ImportError:
    PYCCD_ENABLED = False
TEST_PYCCD = False

#whether to populate point clouds with interior points
ADD_INTERIOR_POINTS = False       #adds interior points in the point cloud
USE_BALLS = False                  #associates interior points in the point cloud with balls (can only be True if ADD_INTERIOR_POINTS=True)


def grid_to_numpy(grid):
    vg = grid.getVolumeGrid()
    m,n,p = vg.dims[0],vg.dims[1],vg.dims[2]
    res = np.zeros((m,n,p))
    for i in xrange(m):
        for j in xrange(n):
            for k in xrange(p):
                res[i,j,k] = vg.get(i,j,k)
    return res

def dump_grid_mat(grid,fn='grid.mat'):
    arr = grid_to_numpy(grid)
    slices = dict()
    for i in xrange(arr.shape[0]):
        slices["slice_%d"%(i,)] = arr[i,:,:]
    scipy.io.savemat(fn,slices)

class PenetrationDepthGeometry:
    def __init__(self,geom,gridres=0,pcres=0):
        """Initializes this with a Geometry3D or PenetrationDepthGeometry.  Geometry3D objects
        are converted into dual grid and point cloud form.  The parameters for conversion are
        gridres and pcres.  Either can be set to 0 to disable that representation
        """
        if isinstance(geom,PenetrationDepthGeometry):
            #copy constructor
            self.geom = geom.geom
            self.pc = geom.pc
            self.pcdata = geom.pcdata
            self.grid = geom.grid
            self.polyhedron = geom.polyhedron
        else:
            assert isinstance(geom,Geometry3D)
            self.geom = geom
            self.pc = None
            self.pcdata = None
            self.grid = None
            self.polyhedron = None
            if gridres is not None:
                try:
                    self.grid = geom.convert('VolumeGrid',gridres)...

The content has been truncated. see original

to make distance computation correct. It gives me negative distance for penetrated objects.

A follow-up question, it seems that the implementation is to compute shortest distance between two point clouds. Should I worry about its computational complexity?

krishauser · Nov'20 Team

The complexity and availability of signed distances depends very much on the geometry types. The most recent pythondocs.klampt.org documentation should provide up to date information on the availability.

As for point-cloud distances, these are indeed somewhat slow for large point clouds (roughly O(n log n))

krishauser / Klampt

negative Signed distance #27