Pybullet robot simulation environment construction 5 Robot pose visualization

Preface

This article records how to pybullet Visualization of robot pose in .

Draw lines in the simulation environment

pybullet Provides a way to add dotted line text in the simulation environment api, such as addUserDebugLine, addUserDebugPoints etc. , And return these dotted lines id, It can be used for subsequent deletion and modification .

Here I write a function that draws the coordinate system of an object , To visualize pose , The code should be easy to understand ：

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])

Code routines

Compare the above function with the previous one pybullet Blog combination of environment , Give the current pose of the robot ：

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]))
    #  End of creation , Turn rendering back on 
    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 1)

    num_joints = pybullet.getNumJoints(r_ind)
    #  Get information about each joint 
    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() #  Delete the previous line , Otherwise, it will always appear in the simulation environment 
        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)

The visualization is as follows ：

We need to pay attention to , Drawing and deleting lines will seriously affect pybullet The speed of the engine , Actually, I feel that one card after another .