前两篇记录了pybullet中的基础环境搭建和模型载入。
本篇记录在pybullet中让模型运动起来，并检测与其它物体的碰撞状态。
第一步仍然是建立pybullet环境，并载入pybullet_data自带的r2d2模型，此外再加载一个自带的正方体和球体模型：
if __name__ == '__main__':
client = pybullet.connect(pybullet.GUI)
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.setPhysicsEngineParameter(numSolverIterations=10)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_TINY_RENDERER, 0)
pybullet.setGravity(0, 0, -9.8)
pybullet.setRealTimeSimulation(1)
shift = [0, 0, 0]
scale = [1, 1, 1]
visual_shape_id = pybullet.createVisualShape(
shapeType=pybullet.GEOM_MESH,
fileName="sphere_smooth.obj",
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, 0.4, 0],
visualFramePosition=[0, 0, 0],
meshScale=scale)
collision_shape_id = pybullet.createCollisionShape(
shapeType=pybullet.GEOM_MESH,
fileName="sphere_smooth.obj",
collisionFramePosition=[0, 0, 0],
meshScale=scale)
pybullet.createMultiBody(
baseMass=1,
baseCollisionShapeIndex=collision_shape_id,
baseVisualShapeIndex=visual_shape_id,
basePosition=[0, 0, 2],
useMaximalCoordinates=True)
plane_id = pybullet.loadURDF("plane100.urdf", useMaximalCoordinates=True)
cube_ind = pybullet.loadURDF('cube.urdf', (0, 1, 0), pybullet.getQuaternionFromEuler([0, 0, 0.785]))
r_ind = pybullet.loadURDF('r2d2.urdf', (3, 1, 1), pybullet.getQuaternionFromEuler([0, 0, 0.785]))
# 创建结束，重新开启渲染
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 1)
运行效果：
机器人是靠关节运动的，因此通过向关节发出控制指令，就能让机器人动起来。
首先要得到r2d2机器人的各关节信息：
# 得到机器人的关节数量
num_joints = pybullet.getNumJoints(r_ind)
# 获得各关节的信息
joint_infos = []
for i in range(num_joints):
joint_info = pybullet.getJointInfo(r_ind, i)
print(joint_info)
if joint_info[2] != pybullet.JOINT_FIXED:
if 'wheel' in str(joint_info[1]):
joint_infos.append(joint_info)
我们打印了关节信息，可以得到：
(0, b'base_to_right_leg', 4, -1, -1, 0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'right_leg', (0.0, 0.0, 0.0), (0.22, 0.0, 0.25), (0.0, -0.7070904020014416, 0.0, 0.7071231599922604), -1)
依次分别表示关节序号，关节名称，关节类型等等。
当关节类型为pybullet.JOINT_FIXED时，是不能动的。因此要筛掉这种关节，剩下来的就可以控制了。
r2d2机器人是有轮子的，因此我们想获得这些控制轮子的关节，即wheel在关节名字内。
获得轮子关节后，我们就可以设置并控制这些关节的速度和加速度了：
maxforce = 1
velocity = 31.4
for i in range(len(joint_infos)):
pybullet.setJointMotorControl2(bodyUniqueId=r_ind,
jointIndex=joint_infos[i][0],
controlMode=pybullet.VELOCITY_CONTROL,
targetVelocity=target_v,
force=max_force)
while True:
pybullet.stepSimulation()
time.sleep(1. / 240.)
setJointMotorControl2()是控制单个关节的api，也可以用setJointMotorControlArray()一次设置多个关节。其中，controlMode参数常用速度控制和位置控制，对应参数为targetVelocity和targetPosition，force参数限制了最大的力（加速度），防止加速度太大导致运动不稳定。速度、位置是通过PID控制获得的。
最后循环步进仿真。
运行结果：可以看到机器人向正方体运动。
上面的机器人与正方体发生了碰撞，接下来就是在pybullet中检测到这种碰撞情况。
首先要知道常用的AABB碰撞检测方法。为了易于理解，先从2D碰撞检测解释：2D中的AABB碰撞检测，就是把场景物体框住，然后比较各物体框的坐标。如下图目标检测结果所示，这个框是与xy坐标轴平行的：
3D场景中的AABB也是一样的，用平行于xyz坐标轴的3D框包围物体，然后根据3D框坐标来判断碰撞：
3D框坐标为
P
m
i
n
=
{
x
m
i
n
,
y
m
i
n
,
z
m
i
n
}
,
P
m
a
x
=
{
x
m
a
x
,
y
m
a
x
,
z
m
a
x
}
{P_{min}}=\{x_{min}, y_{min}, z_{min}\}, P_{max}=\{x_{max}, y_{max}, z_{max}\}
Pmin={xmin,ymin,zmin},Pmax={xmax,ymax,zmax}，物体所有点都在
{
P
m
i
n
,
P
m
a
x
}
\{P_{min},P_{max}\}
{Pmin,Pmax}内。如果两个物体的3D AABB框的
P
P
P有交并，则两者碰撞了。
pybullet中，碰撞检测分为两步：获得需要检测碰撞的机器人的AABB框，检测AABB框的交并情况。
使用getAABB()这个api，可以获得机器人的AABB框：
pmin, pmax = pybullet.getAABB(r_ind)
返回机器人AABB框的 P m i n , P m a x P_{min}, P_{max} Pmin,Pmax
使用getOverlappintObject()这个api，可以获得所有与指定AABB框有交并的模型的id：
collide_ids = pybullet.getOverlappingObjects(pmin, pmax)
返回的collide_ids是一个元组，其元素是二元tuple，比如：
((3, 8), (3, 1), (3, 0), (3, 5), (3, 4), (3, 13), (3, -1))
元组中每个元素代表一个与该AABB框交并的模型，每个元素tuple中，第一个元素是交并的模型id，第二个元素是交并的关节id（-1表示与该模型的base交并）。
注意：pybullet在检测碰撞时，默认会检测到自己与自己交并。比如上面我要检测碰撞的机器人id是3，碰撞检测结果也会检测到与id=3发生碰撞。
import time
import numpy as np
import pybullet
import pybullet_data
if __name__ == '__main__':
client = pybullet.connect(pybullet.GUI)
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.setPhysicsEngineParameter(numSolverIterations=10)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_TINY_RENDERER, 0)
pybullet.setGravity(0, 0, -9.8)
pybullet.setRealTimeSimulation(1)
shift = [0, 0, 0]
scale = [1, 1, 1]
visual_shape_id = pybullet.createVisualShape(
shapeType=pybullet.GEOM_MESH,
fileName="sphere_smooth.obj",
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, 0.4, 0],
visualFramePosition=[0, 0, 0],
meshScale=scale)
collision_shape_id = pybullet.createCollisionShape(
shapeType=pybullet.GEOM_MESH,
fileName="sphere_smooth.obj",
collisionFramePosition=[0, 0, 0],
meshScale=scale)
pybullet.createMultiBody(
baseMass=1,
baseCollisionShapeIndex=collision_shape_id,
baseVisualShapeIndex=visual_shape_id,
basePosition=[-2, -1, 1],
useMaximalCoordinates=True)
plane_id = pybullet.loadURDF("plane100.urdf", useMaximalCoordinates=True)
cube_ind = pybullet.loadURDF('cube.urdf', (3, 1, 1), pybullet.getQuaternionFromEuler([0, 0, 0]))
r_ind = pybullet.loadURDF('r2d2.urdf', (1, 1, 1), pybullet.getQuaternionFromEuler([0, 0, 1.57]))
# 创建结束，重新开启渲染
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 1)
num_joints = pybullet.getNumJoints(r_ind)
# 获得各关节的信息
joint_infos = []
for i in range(num_joints):
joint_info = pybullet.getJointInfo(r_ind, i)
if joint_info[2] != pybullet.JOINT_FIXED:
if 'wheel' in str(joint_info[1]):
print(joint_info)
joint_infos.append(joint_info)
maxforce = 10
velocity = 31.4
for i in range(len(joint_infos)):
pybullet.setJointMotorControl2(bodyUniqueId=r_ind,
jointIndex=joint_infos[i][0],
controlMode=pybullet.VELOCITY_CONTROL,
targetVelocity=velocity,
force=maxforce)
while True:
pmin, pmax = pybullet.getAABB(r_ind)
collide_ids = pybullet.getOverlappingObjects(pmin, pmax)
print(collide_ids)
for collide_id in collide_ids:
if collide_id[0] != r_ind:
print('detect robot collide with object id {}!'.format(r_ind))
pybullet.stepSimulation()
time.sleep(1. / 240.)