（没有找到太多参考的资料，有点作弊的完成了想要的目标，当作一个学习的记录吧，如果想到了更好的表达再来修改）
一、获取link位置API:getLinkState
getLinkState
输入参数：
| 必须 |
bodyUniqueId
|
int | URDF文件ID |
| 必须 |
linkIndex
|
int | link序号 |
| 可选 |
computeLinkVelocity
|
int | 如果设置为1，将计算笛卡尔世界速度，并返回。 |
| 可选 |
physicsClientId
|
int | 如果连接到多个服务器，可以选择一个 |
返回值：
|
linkWorldPosition
|
vec3 | 质心笛卡尔位置 |
|
linkWorldOrientation
|
vec4 | 四元数中质心的笛卡尔方向 |
|
localInertialFramePosition
|
vec3 | 在URDF文件的link frame中表示的惯性框架（质心）的局部位置偏移 |
|
localInertialFrameOrientation
|
vec4 | 在URDF文件的link frame中惯性系的局部方向（四元数[x，y，z，w]）偏移量 |
|
worldLinkFramePosition
|
vec3 | 在URDF文件的link frame中世界位置 |
|
worldLinkFrameOrientation
|
vec4 | 在URDF文件的link frame中世界方向 |
|
worldLinkLinearVelocity
|
vec3 | |
|
worldLinkAngularVelocity
|
vec3 |
二、添加轨迹API：addUserDebugLine
可以添加由三维起点（起点）和终点（终点）、颜色[红、绿、蓝]、线宽和持续时间（秒）指定的三维线。
输入参数：
| 必须 |
lineFromXYZ
|
vec3 | 初始位置 |
| 必须 |
lineToXYZ
|
vec3 | 结束位置 |
| 可选 |
lineColorRGB
|
vec3 | 颜色 |
| 可选 |
lineWidth
|
float | 线宽 |
| 可选 |
lifeTime
|
float | 持续时间 |
| 可选 |
parentObjectUniqueId
|
int | |
| 可选 |
parentLinkIndex
|
int | |
| 可选 |
physicsClientId
|
int |
三、代码示例：
# 足端轨迹显示
foot_p1, foot_q1 = p.getLinkState(robotId, 4)[4:6]
x1,y1,z1=foot_p1
foot_end1=[x1+0.005,y1,z1]
guiji=p.addUserDebugLine(foot_p1,foot_end1,[1,0,0])
foot_p2,foot_q2= p.getLinkState(robotId, 6)[4:6]
x2,y2,z2=foot_p2
foot_end2=[x2+0.005,y2,z2]
guiji=p.addUserDebugLine(foot_p2,foot_end2,[1,0,0])
四、运行结果示例：
五、总结
在了解到可以用adduserdebugline显示足端轨迹后一直想表示出来，但始终没有想好初始位置和中止位置该如何表达。想了很久才想到试着用很短一段线的在每一时刻的持续出现这种想法来实现，不太符合自己期望的表达，但又想不出来什么更好的办法。勉强实现目标，就当作最近的一点记录吧。顺便说一点就是自己对这些API的掌握真的太垃圾了啊！希望以后可以慢慢完完全全了解一下吧，自己试验比百度翻译有用太多了...
本文含有隐藏内容，请 开通VIP 后查看