pybullet: getVisualShapeData() returns wrong link indexes

[olegklimov]

Here is isolated example:

import pybullet as p
p.connect(p.GUI)

huskypos = [0,0,0.1]
husky = p.loadURDF(sys.argv[1], huskypos[0],huskypos[1],huskypos[2])
cnt = p.getNumJoints(husky)

for joint in range(cnt) :
    print(p.getJointInfo(husky, joint))
    p.setJointMotorControl(husky, joint, p.VELOCITY_CONTROL, 0, 0)

viz = [""]*cnt
to_parse_array = p.getVisualShapeData(husky)
for uid, link_idx, geom, dims, filename, pos, orient in to_parse_array:
    if link_idx==-1: continue
    viz[link_idx] = str(filename)
    print("link %i have filename '%s'" % (link_idx, filename))

debug_text = []
for step in range (4000000):
    p.stepSimulation()

    for tid in debug_text:
        p.removeUserDebugItem(tid)
    debug_text = []
    for j in range(-1, cnt):
        if j==-1:
            p.getBasePositionAndOrientation(husky)
            continue
        pos, orient, inert_pos, inert_orient, frame_pos, frame_orient = p.getLinkState(husky, j)
        debug_text.append( p.addUserDebugText("O:" + viz[j][-12:], (pos[0]+1, pos[1], pos[2]) ) )

Output for this:

(0, b'back_bkz', 0, 7, 6, 1, 0.1, 0.0)
(1, b'back_bky', 0, 8, 7, 1, 0.1, 0.0)
(2, b'back_bkx', 0, 9, 8, 1, 0.1, 0.0)
(3, b'l_arm_shz', 0, 10, 9, 1, 0.1, 0.0)
(4, b'l_arm_shx', 0, 11, 10, 1, 0.1, 0.0)
...
link 23 have filename 'b'unplugged/pelvis.dae''
link 21 have filename 'b'unplugged/ltorso.dae''
link 22 have filename 'b'unplugged/mtorso.dae''
link 37 have filename 'b'unplugged/utorso.dae''
link 8 have filename 'b'unplugged/r_clav.dae''
link 16 have filename 'b'unplugged/r_scap.dae''
link 18 have filename 'b'unplugged/r_uarm.dae''
link 13 have filename 'b'meshes_v3/r_larm.dae''

First several joints connect parts of body (torso, not arms or legs). Seems improbable link 21 or 22 (which apparently are parts of left arm) should load torso models.

Here is how it looks:

First models returned by getVisualShapeData() are body, first joints connect parts of body together, so it's logical to try link_index==array_index, but that attempt was also unsuccessful.

[erwincoumans]

It could be an issue with the particular URDF file. Do you have a link to the URDF file? (possibly with any modifications you needed to make)

[olegklimov]

It is roboschool/models_robot/atlas_description/urdf/atlas_v4_with_multisense.urdf, with removed "package://". Also symlinks necessary for it to find .daes.

Here is what I found, for R2D2:

link 0 have filename 'b'''
link 1 have filename 'b'''
link 2 have filename 'b'''
link 3 have filename 'b'''
link 4 have filename 'b'''
link 5 have filename 'b'''
link 6 have filename 'b'''
link 7 have filename 'b'''
link 8 have filename 'b'''
link 9 have filename 'b'''
link 10 have filename 'b's_robot/l_finger.stl''
link 11 have filename 'b'bot/l_finger_tip.stl''
link 12 have filename 'b's_robot/l_finger.stl''
link 13 have filename 'b'bot/l_finger_tip.stl''
link 14 have filename 'b'''
link 15 have filename 'b'''

Link numbers increase monotonically.

And for Atlas:

link 23 have filename 'b'unplugged/pelvis.dae''
link 21 have filename 'b'unplugged/ltorso.dae''
link 22 have filename 'b'unplugged/mtorso.dae''
link 37 have filename 'b'unplugged/utorso.dae''
link 8 have filename 'b'unplugged/r_clav.dae''
link 16 have filename 'b'unplugged/r_scap.dae''
link 18 have filename 'b'unplugged/r_uarm.dae''
link 13 have filename 'b'meshes_v3/r_larm.dae''
link 9 have filename 'b'meshes_v3/r_farm.dae''
link 11 have filename 'b'meshes_v3/r_hand.dae''
link 12 have filename 'b'''
link 0 have filename 'b'tion/meshes/head.dae''
link 1 have filename 'b'shes/head_camera.dae''
link 3 have filename 'b'''
link 5 have filename 'b'''
link 7 have filename 'b'''
link 24 have filename 'b'unplugged/r_clav.dae''
link 32 have filename 'b'unplugged/r_scap.dae''
link 34 have filename 'b'unplugged/r_uarm.dae''
link 29 have filename 'b'meshes_v3/r_larm.dae''
link 25 have filename 'b'meshes_v3/r_farm.dae''
link 27 have filename 'b'meshes_v3/r_hand.dae''
link 28 have filename 'b'''
link 38 have filename 'b'''
link 40 have filename 'b'''
link 19 have filename 'b'nplugged/l_uglut.dae''
link 14 have filename 'b'nplugged/l_lglut.dae''
link 20 have filename 'b'unplugged/l_uleg.dae''
link 15 have filename 'b'unplugged/l_lleg.dae''
link 17 have filename 'b'nplugged/l_talus.dae''
link 10 have filename 'b'unplugged/l_foot.dae''
link 35 have filename 'b'nplugged/r_uglut.dae''
link 30 have filename 'b'nplugged/r_lglut.dae''
link 36 have filename 'b'unplugged/r_uleg.dae''
link 31 have filename 'b'unplugged/r_lleg.dae''
link 33 have filename 'b'nplugged/r_talus.dae''
link 26 have filename 'b'unplugged/r_foot.dae''

You see, link 2 missing, link 4 is missing. Not in order. Strange.

[erwincoumans]

Ok, I'll have a look at it later. By the way, the Atlas is one of the most complex robots out there, it might require quite some work to get the simulation in a nice shape.

[olegklimov]

Well it's regular URDF, links and joints... Rigid bodies with constraints kind of worked:

[olegklimov]

Oh wait!

It's not that Atlas is hard. It is rendered by internal rendering pipeline just fine! And simulated too, as far as I can tell. It is C API bug (present without using python as well).

[erwincoumans]

This 'package://' stuff is ROS being overly complex (at least you don't use the xacro version) Don't you have a version with simple relative paths to assets? We shouldn't need to worry about those details (symlinks, parsing or ignoring 'package://' etc), when we use paths relative to the URDF file.

[olegklimov]

I was going to modify URDF load to eat these files without modification.

[olegklimov]

(if you want to test Atlas, just kill package:// and make a lot of symlinks to make valid paths it tries to use)

[olegklimov]

Wow. Same code, except:

p.loadMJCF("gym/envs/mujoco/assets/humanoid.xml")

For all objects (floor or humanoid in humanoid.xml, or other objects in other files) getVisualShapeData() returns array of length 1 with uninitialized values.

(Joints work for MJCF, and URDF work with bug above, at least it's initialized)

[erwincoumans]

Thanks for the report, I'll look at it soon. (there hasn't been much effort in the 'getVisualShapeData' yet)

[erwincoumans]

Great. Your robot rendering looks great!