引言

在gazebo+rviz这一套仿真系统中，若我们想要看看连杆坐标系，只需要在rviz中进行相应设置就可以，但在pybullet中，则没有现成的绘制两岸坐标系的程序，今天我们就来新建一个这样的项目，来方便机器人行业的同行食用。

代码

话不多说，上代码：

    def plot_line(self,num):
        start = list(p.getLinkState(self.UR5,num)[4])
        rot = np.array(p.getMatrixFromQuaternion(p.getLinkState(self.UR5,num)[5])).reshape(3,3)
        add_local = np.array([2,0,0]).reshape(3,1)
        add_world = np.dot(rot,add_local).tolist()
        add_world_one = [one for token in add_world for one in token]
        end = [a+b for a,b in zip(start,add_world_one)]
        p.addUserDebugLine(start,end,[1,0,0])
        add_local = np.array([0,2,0]).reshape(3,1)
        add_world = np.dot(rot,add_local).tolist()
        add_world_one = [one for token in add_world for one in token]
        end = [a+b for a,b in zip(start,add_world_one)]
        p.addUserDebugLine(start,end,[0,1,0])
        add_local = np.array([0,0,2]).reshape(3,1)
        add_world = np.dot(rot,add_local).tolist()
        add_world_one = [one for token in add_world for one in token]
        end = [a+b for a,b in zip(start,add_world_one)]
        p.addUserDebugLine(start,end,[0,0,1])

整个代码由三个部分组成，分别是绘制x轴，y轴和z轴坐标轴。

在每一个部分，我们

1、获得连杆坐标系的原点位置

2、获得连杆坐标系到笛卡尔世界坐标系的旋转变换矩阵

3、确定增量点：连杆坐标系的某一轴上的点，如[0,2,0]为y轴上

4、根据正向运动学，得到增量点在世界坐标系中的表示

5、使用list数据结构进行表示

6、将初始位置和增量位置相加得到末端点

还有一些细节，比如说x轴使用[1,0,0]表示的红色进行表示，y轴使用[0,1,0]的绿色进行表示，z轴使用[0,0,1]的蓝色进行表示。

效果

最终的效果图如下：

完整代码如下：

# _*_coding:utf-8-*-
import gym
import pybullet as p
import pybullet_data
import numpy as np
gym.logger.set_level(40)
class UREnv(gym.Env):
    def __init__(self, gui=False):
        if gui:
            p.connect(p.GUI)
        else:
            p.connect(p.DIRECT)
        p.resetDebugVisualizerCamera(cameraDistance=1.48, cameraYaw=-0.8, cameraPitch=-11.20,
                                     cameraTargetPosition=[0.49, 0.11, 0.03])
        p.setAdditionalSearchPath(pybullet_data.getDataPath())
        p.setGravity(0, 0, 0)
    def plot_line(self, num):
        start = list(p.getLinkState(self.UR5, num)[4])
        rot = np.array(p.getMatrixFromQuaternion(p.getLinkState(self.UR5, num)[5])).reshape(3, 3)
        add_local = np.array([2, 0, 0]).reshape(3, 1)
        add_world = np.dot(rot, add_local).tolist()
        add_world_one = [one for token in add_world for one in token]
        end = [a + b for a, b in zip(start, add_world_one)]
        p.addUserDebugLine(start, end, [1, 0, 0])
        add_local = np.array([0, 2, 0]).reshape(3, 1)
        add_world = np.dot(rot, add_local).tolist()
        add_world_one = [one for token in add_world for one in token]
        end = [a + b for a, b in zip(start, add_world_one)]
        p.addUserDebugLine(start, end, [0, 1, 0])
        add_local = np.array([0, 0, 2]).reshape(3, 1)
        add_world = np.dot(rot, add_local).tolist()
        add_world_one = [one for token in add_world for one in token]
        end = [a + b for a, b in zip(start, add_world_one)]
        p.addUserDebugLine(start, end, [0, 0, 1])
    def step(self):
        p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)  #平滑视觉渲染
    def reset(self):
        p.resetSimulation()
        p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
        p.loadURDF("plane.urdf", basePosition=[0, 0, -0.65])
        self.UR5 = p.loadURDF("ur5_pos.urdf", useFixedBase=True)
        rest_poses = [0.019493980630576238, -1.6104386316325356, 2.371476945587111, -2.3317793248031182,
                      -1.57079440431533, 0]
        for j in range(6):
            p.resetJointState(self.UR5, j + 1, rest_poses[j])
        p.stepSimulation()
        self.plot_line(1)
        self.plot_line(2)
        self.plot_line(3)
        self.plot_line(4)
        self.plot_line(5)
        self.plot_line(6)
        p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)

if __name__ == '__main__':
    U = UREnv(True)
    U.reset()
    n = 1000000
    while(n):
        n = n-1
        U.step()
    U.close()