import pybullet as p
import time

cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
	p.connect(p.GUI)
q = p.loadURDF("quadruped/quadruped.urdf",useFixedBase=True)
rollId = p.addUserDebugParameter("roll",-1.5,1.5,0)
pitchId = p.addUserDebugParameter("pitch",-1.5,1.5,0)
yawId = p.addUserDebugParameter("yaw",-1.5,1.5,0)
fwdxId = p.addUserDebugParameter("fwd_x",-1,1,0)
fwdyId = p.addUserDebugParameter("fwd_y",-1,1,0)
fwdzId = p.addUserDebugParameter("fwd_z",-1,1,0)

while True:
	roll = p.readUserDebugParameter(rollId)
	pitch = p.readUserDebugParameter(pitchId)
	yaw = p.readUserDebugParameter(yawId)
	x = p.readUserDebugParameter(fwdxId)
	y = p.readUserDebugParameter(fwdyId)
	z = p.readUserDebugParameter(fwdzId)

	orn = p.getQuaternionFromEuler([roll,pitch,yaw])
	p.resetBasePositionAndOrientation(q,[x,y,z],orn)
	#p.stepSimulation()#not really necessary for this demo, no physics used