52 lines
1.5 KiB
Python
52 lines
1.5 KiB
Python
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()
|