pybullet机器人仿真环境搭建 5.机器人位姿可视化

前言

本篇记录一下如何在pybullet中可视化机器人的位姿。

在仿真环境中画线

pybullet提供了在仿真环境中添加点线文本的api，比如addUserDebugLine, addUserDebugPoints等，并返回这些点线的id，可用于后续的删除修改。

这里我写了一个画物体坐标系的函数，来可视化位姿，代码应该很容易懂：

def draw_pose_in_pybullet(*pose):
    """ *Draw pose frame in pybullet* :param pose: np.ndarray, shape=[4, 4] or tuple of (position, orientation) """
    if len(pose) == 1:
        position = pose[0][:3, 3]
        rotation = pose[0][:3, :3]
    else:
        position, orientation = pose
        print(orientation)
        rotation = np.array(p.getMatrixFromQuaternion(orientation)).reshape([3, 3])
        print(rotation)
    start_point = position
    end_point_x = position + rotation[:, 0] * 2
    end_point_y = position + rotation[:, 1] * 2
    end_point_z = position + rotation[:, 2] * 2
    p.addUserDebugLine(start_point, end_point_x, [1, 0, 0])
    p.addUserDebugLine(start_point, end_point_y, [0, 1, 0])
    p.addUserDebugLine(start_point, end_point_z, [0, 0, 1])

代码例程

把上面的函数与上一篇pybullet环境的博客结合，给出机器人当前的位姿：

import time

import numpy as np
import pybullet
import pybullet_data


def draw_pose_in_pybullet(*pose):
    """ *Draw pose frame in pybullet* :param pose: np.ndarray, shape=[4, 4] or tuple of (position, orientation) """
    if len(pose) == 1:
        position = pose[0][:3, 3]
        rotation = pose[0][:3, :3]
    else:
        position, orientation = pose
        print(orientation)
        rotation = np.array(pybullet.getMatrixFromQuaternion(orientation)).reshape([3, 3])
        print(rotation)
    start_point = position
    end_point_x = position + rotation[:, 0] * 2
    end_point_y = position + rotation[:, 1] * 2
    end_point_z = position + rotation[:, 2] * 2
    pybullet.addUserDebugLine(start_point, end_point_x, [1, 0, 0])
    pybullet.addUserDebugLine(start_point, end_point_y, [0, 1, 0])
    pybullet.addUserDebugLine(start_point, end_point_z, [0, 0, 1])


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
    while True:
        pybullet.removeAllUserDebugItems() # 把之前的线删除，否则会一直在仿真环境中出现
        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)
        position, orientation = pybullet.getBasePositionAndOrientation(r_ind)
        draw_pose_in_pybullet(position, orientation)
        pybullet.stepSimulation()
        time.sleep(1./240)

可视化效果如下：

需要注意，画线操作和删除线操作都会严重影响pybullet引擎的运行速度，实际感觉一卡一卡的。