32 lines
964 B
Python
32 lines
964 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) |