1、指令格式说明
getContactPoints指令说明
该指令根据最近一次调用stepSimulation指令，返回接触点信息。它的输入参数信息如下所示;
该指令执行后在有干涉的情况下返回信息如下所示。没有干涉时，返回数据为空。
以下为使用示例，该示例为机器人有干涉情形。如果希望无干涉，可以将obstacle_shape的圆柱半径改为0.5。另外，该演示后面会关闭3D显示，如果不希望关闭，可以将程序最后的p.disconnect()注释。
import pybullet as p
import time
import numpy as np# 连接到PyBullet服务器，选择GUI模式
p.connect(p.GUI)# 加载机器人模型，例如KUKA IIWA
robotId = p.loadURDF("kuka_iiwa/model.urdf", useFixedBase=True)
print("robotId=",robotId)# 创建障碍物
obstacle_shape = p.createCollisionShape(p.GEOM_CYLINDER, radius=1, height=1)
obstacle_pose = [1.5, 0, 0.5] # 设置障碍物位置
obstacleId = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=obstacle_shape, basePosition=obstacle_pose)print("obstacleId=",obstacleId)# 设置机器人初始关节角度
jointAngles = [0, np.pi/4, 0, -np.pi/4, 0, np.pi/2, 0]
for i in range(len(jointAngles)):p.resetJointState(robotId, i, jointAngles[i])p.stepSimulation()jointAngles2 = [np.pi/2, np.pi/2, np.pi/2, 0, np.pi/4, 0, np.pi/4]# 步骤模拟
for i in range(1000):p.stepSimulation()# 控制机器人关节运动p.setJointMotorControlArray(robotId, range(7), p.POSITION_CONTROL, targetPositions=jointAngles2)# 检测碰撞contactPoints = p.getContactPoints(robotId, obstacleId)if len(contactPoints) > 0:print("检测到碰撞！")breakelse:print('机器人与障碍物未发生干涉')time.sleep(1. / 240.) # 控制仿真速度print(contactPoints)
# 断开连接
p.disconnect()
运行上述程序后，contactPoints的返回数据如下所示：
((0, 0, 1, 4, -1, (0.7267713763928924, -0.06187698006452091, 0.6555865171411789), (0.5031878717065762, -0.07978455179650101, 0.655586517141584), (-0.9968078703881859, -0.07983777008515272, 1.8061582203282568e-12), -0.22429949775501498, 0.0, 0.0, (0.07983777008515272, -0.9968078703881859, 0.0), -0.0, (1.8003927291895255e-12, 1.4419964473197598e-13, 1.0)), (0, 0, 1, 5, -1, (0.7418498415436259, -0.007679285086848264, 0.6661223097072552), (0.5000514557764816, -0.010144251943691878, 0.6661223105795947), (-0.9999480421071588, -0.01019377682998052, 3.6075268435680132e-09), -0.2418109497545594, 0.0, 0.0, (0.010193776829980522, -0.999948042107159, 0.0), -0.0, (3.607339404074855e-09, 3.6774323551496385e-11, 1.0)), (0, 0, 1, 6, -1, (0.7249876472953181, 0.0006306772573657553, 0.5754345075215719), (0.5000003352373066, 0.0008172041516804874, 0.5754345075215048), (-0.9999996563339394, 0.0008290548853869341, -2.9811159043371856e-13), -0.22498738937854124, 0.0, -0.0, (-0.0008290548853869341, -0.9999996563339394, 0.0), -0.0, (-2.981114879828827e-13, 2.4715087043954326e-16, 1.0)))
getClosestPoints指令说明
该指令使用时，无需执行stepSimulation指令。而且允许计算两个分离物体距离最近的点。该指令执行时，不会返回法向力。该指令的输入参数信息如下所示：
该指令的返回数据和getContactPoints一样，只是法向力数据为一直为0.可以尝试将contactPoints的赋值指令修改为 p.getClosestPoints(robotId, obstacleId,0)，然后收到的返回数据如下所示：
((0, 0, 1, 4, -1, (0.7064359177093062, -0.06189476625067741, 0.6599250401999935), (0.5030275849004288, -0.07775604200086254, 0.6599250404595945), (-0.9969735485606818, -0.07774151703126006, 1.2723929256255023e-09), -0.20402580700614925, 0.0, 0.0, (0.0, 0.0, 0.0), 0.0, (0.0, 0.0, 0.0)), (0, 0, 1, 5, -1, (0.7217830085456394, -0.00769630809706323, 0.6700708069495462), (0.50004892644242, -0.009891643921746119, 0.6700708069495462), (-0.9999509910834924, -0.009900274296266515, 0.0), -0.22174494958293942, 0.0, 0.0, (0.0, 0.0, 0.0), 0.0, (0.0, 0.0, 0.0)), (0, 0, 1, 6, -1, (0.7025878308696102, 0.0006184769025243883, 0.5798484113266819), (0.5000003073626638, 0.0007809571542291879, 0.5798484113266819), (-0.9999996783781319, 0.0008020247083084872, -1.7125663891067024e-16), -0.20258758866354515, 0.0, 0.0, (0.0, 0.0, 0.0), 0.0, (0.0, 0.0, 0.0)))
2、指令使用注意事项
①使用getContactPoints指令判断有无干涉前，必须执行stepSimulation指令，而getClosestPoints指令不需要。
如下所示getContactPoints指令运行前，需要执行stepSimulation指令。
import pybullet as p# 连接到 PyBullet 物理服务器
env = p.connect(p.GUI)
#env.setAdditionalSearchPath(pybullet.getDataPath())# 创建两个球体碰撞形状
collision_shape_id1 = p.createCollisionShape(p.GEOM_SPHERE, radius=0.5)
collision_shape_id2 = p.createCollisionShape(p.GEOM_SPHERE, radius=0.5)# 创建两个球体视觉形状
visual_shape_id1 = p.createVisualShape(p.GEOM_SPHERE, radius=0.5, rgbaColor=[0.8, 0.2, 0.2, 1])
visual_shape_id2 = p.createVisualShape(p.GEOM_SPHERE, radius=0.5, rgbaColor=[0.2, 0.8, 0.2, 1])# 创建两个球体刚体对象，初始位置分别为 (1, 2, 3) 和 (1.1, 2.2, 3.3)
sphere1 = p.createMultiBody(baseMass=1, baseCollisionShapeIndex=collision_shape_id1, baseVisualShapeIndex=visual_shape_id1, basePosition=[1, 2, 3])
#sphere1 = p.createMultiBody(baseMass=1, baseCollisionShapeIndex=collision_shape_id1, baseVisualShapeIndex=visual_shape_id1, basePosition=[1, 2, 3])
# 运行模拟
p.stepSimulation()sphere2 = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=collision_shape_id2, baseVisualShapeIndex=visual_shape_id2, basePosition=[1.1, 2.2, 3.3])# 运行模拟,使用getContactPoints必须运行
p.stepSimulation()# 进行干涉检查
contact_points = p.getContactPoints(sphere1, sphere2)
#contact_points = p.getClosestPoints(sphere1, sphere2,0)
print('contact_points=',contact_points)# 如果存在接触点，则表示两个球体相互接触或重叠
if len(contact_points) > 0:print("两个球体相互接触或重叠")
else:print("两个球体没有相互接触或重叠")# 关闭 PyBullet 物理服务器
#p.disconnect()
②使用getContactPoints指令判断有无干涉时，检测对象的质量不能都为0。需要至少一个对象质量不为0。而getClosestPoints指令不需要。
具体测试测试程序可以参考上一个程序。
③当使用getClosestPoints进行干涉距离设置时，当实际间距距离大于设置值时，显示无干涉（即使实际干涉距离值为负时即有干涉，设置干涉距离为更小的负值也是一样），反之，当实际间距小于设置值时，即会显示干涉。
如设置值为0，实际间距为1e-17时，显示无干涉，而当设置值大于1e-17的任意值时，均会显示有干涉。而设置值为-1，实际已有干涉如-0.1时，显示仍然无干涉。
④当使用getClosestPoints时，当使用网格3D数据进行干涉检查时，当检测对象有孔洞或空腔，而另外一个检测对象位于其内侧时，默认会有干涉。需要将干涉检查对象的flags值设为1。
以上为getContactPoints与 getClosestPoints指令介绍及相关注意事项。
