pybullet机器人仿真环境搭建 4.镜头跟随移动机器人，在机器人上设置图像传感器

前言

本篇继续记录在pybullet环境中配置机器人，包括让镜头跟随机器人，在机器人上设置图像传感器。

建立仿真环境

第一步仍然是建立仿真环境，我把上次运动的r2d2环境放到这来：

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)

镜头跟随移动机器人

使用resetresetDebugVisualizerCamera()这个api可以设置当前镜头的视角。因为我们要让镜头跟随机器人，因此就实时获得机器人的位置，然后把镜头对准机器人：

position, orientation = pybullet.getBasePositionAndOrientation(r_ind)
pybullet.resetDebugVisualizerCamera(cameraDistance=5,
                                    cameraYaw=120,
                                    cameraPitch=-45,
                                    cameraTargetPosition=position)

resetresetDebugVisualizerCamera()的参数中，cameraTargetPosition表示镜头对准的位置，其它参数表示镜头与该位置的距离、视角方向。

因为要跟随移动机器人，因此上面的代码需要与循环步进仿真放在一起。

在机器人上设置图像传感器

pybullet中图像传感器的设置是一个伪相机的放置，即在机器人的位置上实时获取其视角上的图像，因此要自己设置传感器在世界坐标中的位置，以及传感器的投影矩阵。

获取伪相机的位姿

首先，通过computeViewMatrix()这个api设置伪相机的视角位姿，需要提供的参数为相机位置，视角位置和相机纵轴。

其中，相机位置决定成像位置，相机位置与视角位置形成的矢量方向决定了相机朝向，相机纵轴朝上并与相机朝向垂直。

r_mat = pybullet.getMatrixFromQuaternion(orientation)
tx_vec = np.array([r_mat[0], r_mat[3], r_mat[6]])
tz_vec = np.array([r_mat[2], r_mat[5], r_mat[8]])

camera_position = np.array(position)
target_position = cameraPos - 1 * ty_vec

view_mat = pybullet.computeViewMatrix(cameraEyePosition=camera_position,
                                      cameraTargetPosition=target_position,
                                      cameraUpVector=tz_vec)

获取伪相机的投影参数

通过computeProjectionMatrixFOV()这个函数来设置伪相机的FOV，视距：

proj_mat = pybullet.computeProjectionMatrixFOV(fov=60.0, # field of view
                                               aspect=1.0, # scale, default=1
                                               nearVal=0.01,  # view distance min
                                               farVal=10  # view distance max)

从相机位姿与投影参数获得图像

最后，就是从上面的两个矩阵，获得仿真环境中对应伪相机采集的图像了：

w, h, rgb, depth, seg = pybullet.getCameraImage(width=640,
                                                height=480,
                                                viewMatrix=view_mat,
                                                projectionMatrix=proj_mat,
                                                physicsClientId=server_id)

完整代码

import time

import numpy as np
import pybullet
import pybullet_data


def set_camera(robot_id: int, width: int = 224, height: int = 224, server_id: int = 0):
    position, orientation = pybullet.getBasePositionAndOrientation(robot_id, physicsClientId=server_id)
    r_mat = pybullet.getMatrixFromQuaternion(orientation)
    tx_vec = np.array([r_mat[0], r_mat[3], r_mat[6]])
    ty_vec = np.array([r_mat[1], r_mat[4], r_mat[7]])
    tz_vec = np.array([r_mat[2], r_mat[5], r_mat[8]])
    camera_position = np.array(position)
    target_position = camera_position - 1 * ty_vec

    view_mat = pybullet.computeViewMatrix(cameraEyePosition=camera_position,
                                          cameraTargetPosition=target_position,
                                          cameraUpVector=tz_vec)

    proj_mat = pybullet.computeProjectionMatrixFOV(fov=60.0,  # 摄像头的视线夹角
                                                   aspect=1.0,
                                                   nearVal=0.01,  # 摄像头视距min
                                                   farVal=10  # 摄像头视距max)

    w, h, rgb, depth, seg = pybullet.getCameraImage(width=width,
                                                    height=height,
                                                    viewMatrix=view_mat,
                                                    projectionMatrix=proj_mat,
                                                    physicsClientId=server_id)

    return w, h, rgb, depth, seg


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_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, 2), pybullet.getQuaternionFromEuler([0, 0, 0]))
    r_ind = pybullet.loadURDF('r2d2.urdf', (1, 1, 2), 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 = 3.14
    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)

    flag = True
    while True:
        position, orientation = pybullet.getBasePositionAndOrientation(r_ind)
        pybullet.resetDebugVisualizerCamera(cameraDistance=5,
                                            cameraYaw=120,
                                            cameraPitch=-45,
                                            cameraTargetPosition=position)
        pmin, pmax = pybullet.getAABB(r_ind)
        collide_ids = pybullet.getOverlappingObjects(pmin, pmax)
        for collide_id in collide_ids:
            if collide_id[0] != r_ind:
                print('detect robot collide with object id {}!'.format(r_ind))
                flag = False
                break

        set_camera(r_ind)
        pybullet.stepSimulation()
        time.sleep(1. / 240.)
        if not flag:
            break

运行效果：