Page 1 of 1

getContactPoints() returns wrong points positions

Posted: Mon Sep 12, 2022 1:58 pm
by renatyv
I'm trying to get contact points between rigid body and soft body, see attached zip archive with the source code, .vtk, and .obj files.
https://github.com/bulletphysics/bullet ... ntsBug.zip

Here the soft body cylinder is dropped on a plane located at height z=0. After cylinder reaches the plane, z-coordinate of the contact points must be around zero, definitely not z=1. Moreover, contact points in world coordinates for both the plane and cylinder must be approximately the same (z coordinate must not be reversed).
But I get the following tuple returned by the p.getContactPoints():
(0, 1, 0, -1, -1, (-2.759741412921471e-12, -2.26367090816533e-12, -1.0), (2.759741412921471e-12, 2.26367090816533e-12, 1.0), (2.759741412921471e-12, 2.26367090816533e-12, 1.0), -0.000944430001919847, 1.0, 0.0, (0.0, 0.0, 0.0), 0.0, (0.0, 0.0, 0.0))
Look at the contact points and normals, they are completely wrong (probably swapped?): p.getContactPoints() returns contact points coordinates with z=1

It seems that I get the normals instead of the contact points(z). Moreover, number of contact points is too large.

Code: Select all

import pybullet as p

DEBUG_CAMERA_YAW = 0.0
DEBUG_CAMERA_PITCH = -20.0
DEBUG_CAMERA_DISTANCE = 0.3

p.connect(p.GUI)
# p.connect(p.DIRECT)
p.setRealTimeSimulation(0)  # 240 fps
p.setPhysicsEngineParameter(enableFileCaching=0)
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
p.setGravity(0, 0, -9.8)
p.resetDebugVisualizerCamera(cameraDistance=DEBUG_CAMERA_DISTANCE,
                             cameraYaw=DEBUG_CAMERA_YAW,
                             cameraPitch=DEBUG_CAMERA_PITCH,
                             cameraTargetPosition=[0, 0, 0.15])


E = 400
# nu = 0...0.5, poisson ratio
# nu =  0.45
nu = 0.45
nh_lamda = E*nu/(1+nu)/(1-2*nu)
nh_mu = E/2/(1+nu)

plane_id = p.loadURDF("plane.urdf",
                      basePosition=[0, 0, 0],
                      useFixedBase=1)

cylinder_id: int = p.loadSoftBody("cylinder.vtk",
                                  basePosition=[0, 0, 0.1],
                                  baseOrientation=[0, 0, 0, 1],
                                  simFileName="cylinder.vtk",
                                  mass=0.02,
                                  scale=1.0,
                                  useNeoHookean=1,
                                  NeoHookeanMu=nh_mu,
                                  NeoHookeanLambda=nh_lamda,
                                  NeoHookeanDamping=0.05,
                                  collisionMargin=0.0005,
                                  useSelfCollision=0,
                                  frictionCoeff=0.05,
                                  repulsionStiffness=100)

for _ in range(320):
    p.stepSimulation()
    contact_points = p.getContactPoints()
    if len(contact_points) > 0:
        print(contact_points[0])
        break
M1 MacBook Pro, pybullet==3.2.5, numpy==1.23.2