1import pybullet as p
2import time
3import pybullet_data
4
5
6
7cid = p.connect(p.SHARED_MEMORY)
8if (cid < 0):
9  cid = p.connect(p.GUI)
10
11p.resetSimulation()
12
13useRealTime = 0
14
15p.setRealTimeSimulation(useRealTime)
16
17p.setAdditionalSearchPath(pybullet_data.getDataPath())
18
19p.setGravity(0, 0, -10)
20
21p.loadSDF("stadium.sdf")
22
23obUids = p.loadMJCF("mjcf/humanoid_fixed.xml")
24human = obUids[0]
25
26for i in range(p.getNumJoints(human)):
27  p.setJointMotorControl2(human, i, p.POSITION_CONTROL, targetPosition=0, force=500)
28
29kneeAngleTargetId = p.addUserDebugParameter("kneeAngle", -4, 4, -1)
30maxForceId = p.addUserDebugParameter("maxForce", 0, 500, 10)
31
32kneeAngleTargetLeftId = p.addUserDebugParameter("kneeAngleL", -4, 4, -1)
33maxForceLeftId = p.addUserDebugParameter("maxForceL", 0, 500, 10)
34
35kneeJointIndex = 11
36kneeJointIndexLeft = 18
37
38while (1):
39  time.sleep(0.01)
40  kneeAngleTarget = p.readUserDebugParameter(kneeAngleTargetId)
41  maxForce = p.readUserDebugParameter(maxForceId)
42  p.setJointMotorControl2(human,
43                          kneeJointIndex,
44                          p.POSITION_CONTROL,
45                          targetPosition=kneeAngleTarget,
46                          force=maxForce)
47  kneeAngleTargetLeft = p.readUserDebugParameter(kneeAngleTargetLeftId)
48  maxForceLeft = p.readUserDebugParameter(maxForceLeftId)
49  p.setJointMotorControl2(human,
50                          kneeJointIndexLeft,
51                          p.POSITION_CONTROL,
52                          targetPosition=kneeAngleTargetLeft,
53                          force=maxForceLeft)
54
55  if (useRealTime == 0):
56    p.stepSimulation()
57