import pybullet as p
import time

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

p.resetSimulation()

useRealTime = 0

p.setRealTimeSimulation(useRealTime)

p.setGravity(0,0,-10)

p.loadSDF("stadium.sdf")

obUids = p.loadMJCF("mjcf/humanoid_fixed.xml")
human = obUids[0]



for i in range (p.getNumJoints(human)):
	p.setJointMotorControl2(human,i,p.POSITION_CONTROL,targetPosition=0,force=500)

kneeAngleTargetId = p.addUserDebugParameter("kneeAngle",-4,4,-1)
maxForceId = p.addUserDebugParameter("maxForce",0,500,10)

kneeAngleTargetLeftId = p.addUserDebugParameter("kneeAngleL",-4,4,-1)
maxForceLeftId = p.addUserDebugParameter("maxForceL",0,500,10)


kneeJointIndex=11
kneeJointIndexLeft=18

while (1):
	time.sleep(0.01)
	kneeAngleTarget = p.readUserDebugParameter(kneeAngleTargetId)
	maxForce = p.readUserDebugParameter(maxForceId)
	p.setJointMotorControl2(human,kneeJointIndex,p.POSITION_CONTROL,targetPosition=kneeAngleTarget,force=maxForce)
	kneeAngleTargetLeft = p.readUserDebugParameter(kneeAngleTargetLeftId)
	maxForceLeft = p.readUserDebugParameter(maxForceLeftId)
	p.setJointMotorControl2(human,kneeJointIndexLeft,p.POSITION_CONTROL,targetPosition=kneeAngleTargetLeft,force=maxForceLeft)

	if (useRealTime==0):
		p.stepSimulation()