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
pybullet.connect(pybullet.GUI)
pybullet.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"), 0, 0, -1)
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)
numJoints = pybullet.getNumJoints(obj)
print(numJoints)
pybullet.setGravity(0, 0, -9.8)
t_end = time.time() + 5
while time.time() < t_end:
pybullet.stepSimulation()
posAndOrn = pybullet.getBasePositionAndOrientation(obj)
print(posAndOrn)
print("finished")
pybullet.resetSimulation()
pybullet.disconnect()