在pybullet中查看URDF模型质心

对于在pybullet中可视化查看机器人连杆的质心位置的python脚本show_CM_for_urdf.py代码如下：

#!/usr/bin/env python
######################################################################
# @author : zhou 
# @file : test
# @created : Tuesday Feb 25, 2020 20:08:08 CST
#
# @description : 
######################################################################

import pybullet as p
import sys
import pybullet_data
import numpy as np
import time

if __name__ == '__main__':
    if len(sys.argv) < 2:
        print("should input a urdf name, a format like */*/*.urdf")
        sys.exit(0)
    urdf_file_name = sys.argv[1]

physicsClient = p.connect(p.GUI)#or p.GUI for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
##-------initial setting ---------

p.setGravity(0,0,0)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0,0,0.2]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
boxId = p.loadURDF(urdf_file_name,cubeStartPos, cubeStartOrientation)

num_joint = p.getNumJoints(boxId)

link_pos = np.zeros((num_joint + 1,3))
obj_list = np.zeros(num_joint + 1)

# initial state setting of the joint 


for j in range(num_joint):
    p.resetJointState(boxId, j, targetValue=0.0, targetVelocity=0.0)

for i in range (1):
    p.stepSimulation()
    state_base = p.getBasePositionAndOrientation(boxId)
    link_pos[0,:] = state_base[0][:3]
    obj_list[0] = p.loadURDF("sphere_1cm.urdf", link_pos[0,:], useMaximalCoordinates=True)
    for m in np.arange(num_joint): 
        link_pos[m+1,:] = p.getLinkState(boxId,m)[0][:3]
        obj_list[m+1] = p.loadURDF("sphere_1cm.urdf", link_pos[m+1,:], useMaximalCoordinates=True)
    for im in np.arange(-1,num_joint):
       p.changeVisualShape(boxId, im, rgbaColor=[0.5, 0.5, 0.5, 0.2])   
    time.sleep(1000000)

p.disconnect()

 

在linux终端运行命令

 python3 show_CM_for_urdf.py "../simple_robot.urdf" 

 

 

 

 

效果如下：.

<wiz_tmp_tag id="wiz-table-range-border" contenteditable="false" style="display: none;">