33 lines
1010 B
Python
33 lines
1010 B
Python
import pybullet as p
|
|
import time
|
|
|
|
p.connect(p.GUI)
|
|
obUids = p.loadMJCF("mjcf/humanoid.xml")
|
|
humanoid = obUids[1]
|
|
|
|
gravId = p.addUserDebugParameter("gravity", -10, 10, -10)
|
|
jointIds = []
|
|
paramIds = []
|
|
|
|
p.setPhysicsEngineParameter(numSolverIterations=10)
|
|
p.changeDynamics(humanoid, -1, linearDamping=0, angularDamping=0)
|
|
|
|
for j in range(p.getNumJoints(humanoid)):
|
|
p.changeDynamics(humanoid, j, linearDamping=0, angularDamping=0)
|
|
info = p.getJointInfo(humanoid, j)
|
|
#print(info)
|
|
jointName = info[1]
|
|
jointType = info[2]
|
|
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
|
jointIds.append(j)
|
|
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4, 0))
|
|
|
|
p.setRealTimeSimulation(1)
|
|
while (1):
|
|
p.setGravity(0, 0, p.readUserDebugParameter(gravId))
|
|
for i in range(len(paramIds)):
|
|
c = paramIds[i]
|
|
targetPos = p.readUserDebugParameter(c)
|
|
p.setJointMotorControl2(humanoid, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.)
|
|
time.sleep(0.01)
|