How to use the pybullet.loadURDF function in pybullet

### Taken from pb tutorial

import pybullet as p
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setGravity(0,0,-10)
planeId = p.loadURDF("jaco_robot.urdf")

#!/usr/bin/env python

import pybullet as p
import time
import math

useRealTime = 0
fixedTimeStep = 0.001

physId = p.connect(p.SHARED_MEMORY)
if (physId<0):
	p.connect(p.GUI)

p.loadURDF("ground.urdf")
p.setGravity(0,0,-9.81)
p.setTimeStep(fixedTimeStep)

p.setRealTimeSimulation(0)
#body = p.loadURDF("simple-snakev0.urdf",1,-2,1)
#body = p.loadURDF("springy-snakev0.urdf",1,-2,1)
body = p.loadURDF("phantomx/phantomx.urdf",0,0,2)

position,orientation = p.getBasePositionAndOrientation(body)
print position

num_joints = p.getNumJoints(body)

minLimit = -math.pi/2
maxLimit = math.pi/2

def init(self, furniture_type, directory, id, np_random, wheelchair_mounted=False):
        if 'wheelchair' in furniture_type:
            furniture = p.loadURDF(os.path.join(directory, 'wheelchair', 'wheelchair.urdf' if not wheelchair_mounted else 'wheelchair_jaco.urdf'), basePosition=[0, 0, 0.06], baseOrientation=p.getQuaternionFromEuler([np.pi/2.0, 0, np.pi], physicsClientId=id), physicsClientId=id)
        elif furniture_type == 'bed':
            furniture = p.loadURDF(os.path.join(directory, 'bed', 'bed.urdf'), basePosition=[-0.1, 0, 0], baseOrientation=p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=id), physicsClientId=id)

            # mesh_scale = [1.1]*3
            # bed_visual = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=os.path.join(self.directory, 'bed', 'bed_single_reduced.obj'), rgbaColor=[1, 1, 1, 1], specularColor=[0.2, 0.2, 0.2], meshScale=mesh_scale, physicsClientId=self.id)
            # bed_collision = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName=os.path.join(self.directory, 'bed', 'bed_single_reduced_vhacd.obj'), meshScale=mesh_scale, flags=p.GEOM_FORCE_CONCAVE_TRIMESH, physicsClientId=self.id)
            # furniture = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=bed_collision, baseVisualShapeIndex=bed_visual, basePosition=[0, 0, 0], useMaximalCoordinates=True, physicsClientId=self.id)
            # # Initialize bed position
            # p.resetBasePositionAndOrientation(furniture, [-0.1, 0, 0], p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=self.id), physicsClientId=self.id)
        elif furniture_type == 'table':
            furniture = p.loadURDF(os.path.join(directory, 'table', 'table_tall.urdf'), basePosition=[0.25, -0.9, 0], baseOrientation=[0, 0, 0, 1], physicsClientId=id)
        elif furniture_type == 'bowl':
            bowl_pos = np.array([-0.15, -0.55, 0.75]) + np.array([np_random.uniform(-0.05, 0.05), np_random.uniform(-0.05, 0.05), 0])
            furniture = p.loadURDF(os.path.join(directory, 'dinnerware', 'bowl.urdf'), basePosition=bowl_pos, baseOrientation=p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=id), physicsClientId=id)
        elif furniture_type == 'nightstand':
            furniture = p.loadURDF(os.path.join(directory, 'nightstand', 'nightstand.urdf'), basePosition=np.array([-0.9, 0.7, 0]), baseOrientation=p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=id), physicsClientId=id)
        else:
            furniture = None

        super(Furniture, self).init(furniture, id, np_random, indices=-1)

def init(self, directory, id, np_random, fixed_base=True):
        self.body = p.loadURDF(os.path.join(directory, 'baxter', 'baxter_custom.urdf'), useFixedBase=fixed_base, basePosition=[-2, -2, 0.975], physicsClientId=id)
        super(Baxter, self).init(self.body, id, np_random)

        # Recolor robot
        for i in [20, 21, 23, 31, 32, 42, 43, 45, 53, 54]:
            p.changeVisualShape(self.body, i, rgbaColor=[1.0, 1.0, 1.0, 0.0], physicsClientId=id)

#  2 = two items; cart & pole
		#  7d tuple for pos + orientation pose
		self.state_shape = (self.repeats, 2, 7)
		
		# check reward type
		assert opts.reward_calc in ['fixed', 'angle', 'action', 'angle_action']
		self.reward_calc = opts.reward_calc
		
		# no state until reset.
		self.state = np.empty(self.state_shape, dtype=np.float32)
		
		# setup bullet
		p.connect(p.GUI if self.gui else p.DIRECT)
		p.setGravity(0, 0, -9.81)
		p.loadURDF("envs/models/ground.urdf", 0,0,0, 0,0,0,1)
		self.cartpole = p.loadURDF('envs/models/cartpolev0.urdf',0,0,0)
		p.setJointMotorControl2(self.cartpole,0, controlMode=p.VELOCITY_CONTROL, force=0) # turn off active motor/ turn it to 0 force

def init_sawyer(self, print_joints=False):
        # Enable self collisions to prevent the arm from going through the torso
        if self.task == 'arm_manipulation':
            robot = p.loadURDF(os.path.join(self.directory, 'sawyer', 'sawyer_arm_manipulation.urdf'), useFixedBase=True, basePosition=[0, 0, 0], flags=p.URDF_USE_SELF_COLLISION, physicsClientId=self.id)
            # Disable collisions between the fingers and the tool
            for i in range(16, 24):
                p.setCollisionFilterPair(robot, robot, i, 24, 0, physicsClientId=self.id)
        else:
            robot = p.loadURDF(os.path.join(self.directory, 'sawyer', 'sawyer.urdf'), useFixedBase=True, basePosition=[0, 0, 0], flags=p.URDF_USE_SELF_COLLISION, physicsClientId=self.id)
        # Remove collisions between the various arm links for stability
        for i in range(3, 24):
            for j in range(3, 24):
                p.setCollisionFilterPair(robot, robot, i, j, 0, physicsClientId=self.id)
        for i in range(0, 3):
            for j in range(0, 9):
                p.setCollisionFilterPair(robot, robot, i, j, 0, physicsClientId=self.id)
        robot_arm_joint_indices = [3, 8, 9, 10, 11, 13, 16]
        if print_joints:
            self.print_joint_info(robot, show_fixed=True)

        # Initialize and position
        p.resetBasePositionAndOrientation(robot, [-2, -2, 0.975], [0, 0, 0, 1], physicsClientId=self.id)

        # Grab and enforce robot arm joint limits
        lower_limits, upper_limits = self.enforce_joint_limits(robot)

'''

        rospack = rospkg.RosPack()
        path = rospack.get_path('costar_simulation')
        filename = os.path.join(path, self.xacro_filename)
        urdf_filename = os.path.join(path, 'robot', self.urdf_filename)
        urdf = open(urdf_filename, "r")
        # urdf =
        # open("/home/albert/costar_ws/src/costar_plan/costar_simulation/robot/jaco_robot.urdf",
        # "r")

        # Recompile the URDF to make sure it's up to date
        # subprocess.call(['rosrun', 'xacro', 'xacro.py', filename],
        # stdout=urdf)

        self.handle = pb.loadURDF(urdf_filename)
        self.grasp_idx = self.findGraspFrame()
        self.loadKinematicsFromURDF(urdf_filename, "robot_root")

        return self.handle

Returns
    -------
    baxterId : int
    endEffectorId : int 
    """
    p.resetSimulation()

    
    # Load plane
    p.loadURDF("plane.urdf", [0, 0, -1], useFixedBase=True)

    sleep(0.1)
    p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
    # Load Baxter
    baxterId = p.loadURDF("baxter_common/baxter_description/urdf/toms_baxter.urdf", useFixedBase=True)
    p.resetBasePositionAndOrientation(baxterId, [0.5, -0.8, 0.0], [0., 0., -1., -1.])
    #p.resetBasePositionAndOrientation(baxterId, [0.5, -0.8, 0.0],[0,0,0,1])
    #p.resetBasePositionAndOrientation(baxterId, [0, 0, 0], )

    p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)

    # Grab relevant joint IDs
    endEffectorId = 48 # (left gripper left finger)

    # Set gravity
    p.setGravity(0., 0., -10.)

    # Let the world run for a bit
    for _ in range(initialSimSteps):
        p.stepSimulation()

def reset(self):
    if self.isEnableSelfCollision:
      self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath, [0,0,.2], flags=p.URDF_USE_SELF_COLLISION)
    else:
      self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath, [0,0,.2])
    self.kp = 1
    self.kd = 1
    self.maxForce = 3.5
    self.nMotors = 8
    self.motorIdList = []
    self.motorDir = [-1, -1, -1, -1, 1, 1, 1, 1]
    self.buildJointNameToIdDict()
    self.buildMotorIdList()

Args:
      urdfList: The list of urdf files to place in the bin.

    Returns:
      The list of object unique ID's.
    """

        # Randomize positions of each object urdf.
        objectUids = []
        for urdf_name in urdfList:
            xpos = 0.4 + self._blockRandom * random.random()
            ypos = self._blockRandom * (random.random() - .5)
            angle = np.pi / 2 + self._blockRandom * np.pi * random.random()
            orn = p.getQuaternionFromEuler([0, 0, angle])
            urdf_path = os.path.join(self._urdfRoot, urdf_name)
            uid = p.loadURDF(urdf_path, [xpos, ypos, .15],
                             [orn[0], orn[1], orn[2], orn[3]])
            objectUids.append(uid)
            # Let each object fall to the tray individual, to prevent object
            # intersection.
            for _ in range(500):
                p.stepSimulation()
        return objectUids