URDF breaks when "flags = pybullet.URDF_MAINTAIN_LINK_ORDER" is called

[rohit-kumar-j]

When the pybullet.URDF_MAINTAIN_LINK_ORDER is used, URDF breaks, and the joint order is changed(although joint reordering is expected)

Here is the URDF:
V14_URDF.zip

Here is the code:

#!/usr/bin/env python3
#Simulation modules
import pybullet as p
import pybullet_data as p_data

#Other basic modules
import time
import math
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from datetime import datetime
import numpy as np  #to reshape for matplotlib

#Matplotlib reshaping, decoding,etc
import os, fnmatch
import argparse
from datetime import datetime
import struct
import sys
import math

# Load PyBullet
physicsClient=p.connect(p.GUI)
p.setAdditionalSearchPath(p_data.getDataPath())
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)

# Loading the plane and the Robot URDF
#planeId = p.loadURDF("plane.urdf")
robot = p.loadURDF("./V14_URDF/V!$_URDF.urdf",basePosition = [1,1,1],useFixedBase=True, flags = p.URDF_MAINTAIN_LINK_ORDER+p.URDF_USE_MATERIAL_COLORS_FROM_MTL)
print('humanoid UId:', p.getBodyUniqueId(robot))
print('humanoid has', p.getNumJoints(robot), 'joints' ,'\n')
robot2 = p.loadURDF("./V14_URDF/V!$_URDF.urdf"",useFixedBase=True)
print('Robot UId:', p.getBodyUniqueId(robot2))
print('robot has', p.getNumJoints(robot2), 'joints' ,'\n')

count = 0
num_ij = 0
for i in range (p.getNumJoints(robot2)):
    tmp = p.getJointInfo(robot2,i)
    count = count +1
    if tmp[2] == 0:
        print(i,':','red_robot = ', tmp[1], ' type = ', 'revolute', 'name = ', tmp[12])
        num_ij = num_ij + 1
    elif tmp[2] == 1:
        print(i,':','name = ', tmp[1], ' type = ', 'prismatic', 'name = ', tmp[12])
        num_ij = num_ij + 1
    elif tmp[2] == 2:
        print(i,':','name = ', tmp[1], ' type = ', 'spherical', 'name = ', tmp[12])
        num_ij = num_ij + 3
    elif tmp[2] == 3:
        print(i,':','name = ', tmp[1], ' type = ', 'planar', 'name = ', tmp[12])
        num_ij = num_ij + 3
    elif tmp[2] == 4:
        print(i,':','name = ', tmp[1], ' type = ', 'fixed', 'name = ', tmp[12])
        num_ij = num_ij + 1
    if i >=25:
        print('num of joint centers = ', count)
        print('total num of joints = ', num_ij-1)
        print('\nAction space = ', 43-7, '\n')
count = 0
num_ij = 0
for i in range (p.getNumJoints(robot)):
    tmp = p.getJointInfo(robot,i)
    count = count +1
    if tmp[2] == 0:
        print(i,':','blacK_robot = ', tmp[1], ' type = ', 'revolute', 'name = ', tmp[12])
        num_ij = num_ij + 1
    elif tmp[2] == 1:
        print(i,':','name = ', tmp[1], ' type = ', 'prismatic', 'name = ', tmp[12])
        num_ij = num_ij + 1
    elif tmp[2] == 2:
        print(i,':','name = ', tmp[1], ' type = ', 'spherical', 'name = ', tmp[12])
        num_ij = num_ij + 3
    elif tmp[2] == 3:
        print(i,':','name = ', tmp[1], ' type = ', 'planar', 'name = ', tmp[12])
        num_ij = num_ij + 3
    elif tmp[2] == 4:
        print(i,':','name = ', tmp[1], ' type = ', 'fixed', 'name = ', tmp[12])
        num_ij = num_ij + 1
    if i >=25:
        print('num of joint centers = ', count)
        print('total num of joints = ', num_ij-1)
        print('\nAction space = ', 43-7, '\n') 

qr = input('JOINT NUM: \n [y/n] ') 
print('robot =', robot,'\n','qr = ',qr)
jointFrictionForce = 3
tar_vel = 10
mv = 0.01 
pi = 3.14
tmp =  p.getJointInfo(robot,int(qr))
if tmp[2] == 2: #Spherical
    p.setJointMotorControlMultiDof(robot, np.array(qr), p.POSITION_CONTROL,
                                    targetPosition= [0, 0, 10, 0], 
                                    targetVelocity=[tar_vel, tar_vel, tar_vel],
                                    positionGain=0.1,
                                    velocityGain=0.1,
                                    maxVelocity=mv, 
                                    force=[jointFrictionForce, jointFrictionForce, jointFrictionForce])
elif tmp[2] == 0: #Revolute
    p.setJointMotorControl2(robot, np.array(qr), p.POSITION_CONTROL,
                            targetPosition= (pi/2), 
                            targetVelocity= tar_vel,
                            positionGain=1,
                            velocityGain=1, 
                            force=jointFrictionForce)
elif tmp[2] == 4: #Fixed
    p.setJointMotorControlMultiDof(robot, np.array(qr), p.POSITION_CONTROL,
                                    targetPosition= [0, 0, 10, 0], 
                                    targetVelocity=[tar_vel, tar_vel, tar_vel],
                                    positionGain=0.1,
                                    velocityGain=0.1,
                                    maxVelocity=mv, 
                                    force=[jointFrictionForce, jointFrictionForce, jointFrictionForce])

fixedTimeStep = (1.0/240.0)
t = 10000   
null = 0
p.setRealTimeSimulation(0)
while p.isConnected():
    time.sleep(fixedTimeStep)
p.setRealTimeSimulation(0)

Output:

[erwincoumans]

Parent links have to be in the URDF before any of the child links, for this feature to work.