How to display the frame of each joint in pybullet

[huxiaoyi0625]

I want to display the frame of each joint so that I can debug the simulation of my robot better. However, I found that the frame of joints seems different from the results shown in the RVIZ, and it gives me big trouble. What should I do?

[cvshah]

hello, I wrote a short function for this. I hope you find this useful.

def axiscreator(bodyId, linkId = -1):
print(f'axis creator at bodyId = {bodyId} and linkId = {linkId} as XYZ->RGB')
x_axis = self.minitaur._pybullet_client.addUserDebugLine(lineFromXYZ = [0, 0, 0] ,
lineToXYZ = [0.1, 0, 0],
lineColorRGB = [1, 0, 0] ,
lineWidth = 0.1 ,
lifeTime = 0 ,
parentObjectUniqueId = bodyId ,
parentLinkIndex = linkId )

 y_axis = self.minitaur._pybullet_client.addUserDebugLine(lineFromXYZ          = [0, 0, 0]  ,
                                                           lineToXYZ            = [0, 0.1, 0],
                                                           lineColorRGB         = [0, 1, 0]  ,
                                                           lineWidth            = 0.1        ,
                                                           lifeTime             = 0          ,
                                                           parentObjectUniqueId = bodyId     ,
                                                           parentLinkIndex      = linkId     )

 z_axis = self.minitaur._pybullet_client.addUserDebugLine(lineFromXYZ          = [0, 0, 0]  ,
                                                           lineToXYZ            = [0, 0, 0.1],
                                                           lineColorRGB         = [0, 0, 1]  ,
                                                           lineWidth            = 0.1        ,
                                                           lifeTime             = 0          ,
                                                           parentObjectUniqueId = bodyId     ,
                                                           parentLinkIndex      = linkId     )
return [x_axis, y_axis, z_axis]

[huxiaoyi0625]

Thanks for your great help！ I want to show the URDF link frame. Although the code couldn't thoroughly solve my problem, you inspired me with the idea of how to solve it!

[cadop]

I think if you use the visualizer in pybullet, pressing w makes it wireframe, and j shows the axis.

[cvshah]

wow, it works. Thank you

[huxiaoyi0625]

Thanks for your great help!