#os.sys.path.insert is only needed when pybullet is not installed
#but running from github repo instead
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
print("current_dir=" + currentdir)
parentdir = os.path.join(currentdir, "../gym")
os.sys.path.insert(0, parentdir)

import pybullet
import pybullet_data

import time

#choose connection method: GUI, DIRECT, SHARED_MEMORY
pybullet.connect(pybullet.GUI)
pybullet.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"), 0, 0, -1)
#load URDF, given a relative or absolute file+path
obj = pybullet.loadURDF(os.path.join(pybullet_data.getDataPath(), "r2d2.urdf"))

posX = 0
posY = 3
posZ = 2
obj2 = pybullet.loadURDF(os.path.join(pybullet_data.getDataPath(), "kuka_iiwa/model.urdf"), posX,
                         posY, posZ)

#query the number of joints of the object
numJoints = pybullet.getNumJoints(obj)

print(numJoints)

#set the gravity acceleration
pybullet.setGravity(0, 0, -9.8)

#step the simulation for 5 seconds
t_end = time.time() + 5
while time.time() < t_end:
  pybullet.stepSimulation()
  posAndOrn = pybullet.getBasePositionAndOrientation(obj)
  print(posAndOrn)

print("finished")
#remove all objects
pybullet.resetSimulation()

#disconnect from the physics server
pybullet.disconnect()
