diff --git a/data/mjcf/humanoid_fixed.xml b/data/mjcf/humanoid_fixed.xml new file mode 100644 index 000000000..94e1dd2a0 --- /dev/null +++ b/data/mjcf/humanoid_fixed.xml @@ -0,0 +1,115 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/pybullet/examples/humanoid_knee_position_control.py b/examples/pybullet/examples/humanoid_knee_position_control.py new file mode 100644 index 000000000..f4398b7fa --- /dev/null +++ b/examples/pybullet/examples/humanoid_knee_position_control.py @@ -0,0 +1,37 @@ +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,0) + +p.loadURDF("plane.urdf") + +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=50) + +kneeAngleTargetId = p.addUserDebugParameter("kneeAngle",-4,4,-1) +maxForceId = p.addUserDebugParameter("maxForce",0,100,10) + +kneeJointIndex=11 + +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) + if (useRealTime==0): + p.stepSimulation() \ No newline at end of file