diff options
Diffstat (limited to 'examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py')
| -rw-r--r-- | examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py | 201 |
1 files changed, 201 insertions, 0 deletions
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py new file mode 100644 index 000000000..a429bad5b --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py @@ -0,0 +1,201 @@ +import pybullet as p +import pybullet_data + +import time +import motion_capture_data +import quadrupedPoseInterpolator + +p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) + +plane = p.loadURDF("plane.urdf") +p.setGravity(0,0,-9.8) +timeStep=1./500 +p.setTimeStep(timeStep) +#p.setDefaultContactERP(0) +#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS +urdfFlags = p.URDF_USE_SELF_COLLISION + + +startPos=[0.007058990464444105, 0.03149299192130908, 0.4918981912395484] +startOrn=[0.005934649695708604, 0.7065453990917289, 0.7076373820553712, -0.0027774940359030264] +quadruped = p.loadURDF("laikago/laikago.urdf",startPos,startOrn, flags = urdfFlags,useFixedBase=False) +p.resetBasePositionAndOrientation(quadruped,startPos,startOrn) + +#This cube is added as a soft constraint to keep the laikago from falling +#since we didn't train it yet, it doesn't balance +cube = p.loadURDF("cube_no_rotation.urdf",[0,0,-0.5],[0,0.5,0.5,0]) +p.setCollisionFilterGroupMask(cube,-1,0,0) +for j in range(p.getNumJoints(cube)): + p.setJointMotorControl2(cube,j,p.POSITION_CONTROL,force=0) + p.setCollisionFilterGroupMask(cube,j,0,0) + p.changeVisualShape(cube,j,rgbaColor=[1,0,0,0]) +cid = p.createConstraint(cube,p.getNumJoints(cube)-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,1,0],[0,0,0]) +p.changeConstraint(cid, maxForce=10) + + +jointIds=[] +paramIds=[] +jointOffsets=[] +jointDirections=[-1,1,1,1,1,1,-1,1,1,1,1,1] +jointAngles=[0,0,0,0,0,0,0,0,0,0,0,0] + +for i in range (4): + jointOffsets.append(0) + jointOffsets.append(-0.7) + jointOffsets.append(0.7) + +maxForceId = p.addUserDebugParameter("maxForce",0,100,20) + +for j in range (p.getNumJoints(quadruped)): + p.changeDynamics(quadruped,j,linearDamping=0, angularDamping=0) + info = p.getJointInfo(quadruped,j) + #print(info) + jointName = info[1] + jointType = info[2] + if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): + jointIds.append(j) + + +startQ=[0.08389, 0.8482, -1.547832, -0.068933, 0.625726, -1.272086, 0.074398, 0.61135, -1.255892, -0.068262, 0.836745, -1.534517] +for j in range(p.getNumJoints(quadruped)): + p.resetJointState(quadruped,jointIds[j],jointDirections[j]*startQ[j]+jointOffsets[j]) + + +qpi = quadrupedPoseInterpolator.QuadrupedPoseInterpolator() + +#enable collision between lower legs + +for j in range (p.getNumJoints(quadruped)): + print(p.getJointInfo(quadruped,j)) + +#2,5,8 and 11 are the lower legs +lower_legs = [2,5,8,11] +for l0 in lower_legs: + for l1 in lower_legs: + if (l1>l0): + enableCollision = 1 + print("collision for pair",l0,l1, p.getJointInfo(quadruped,l0)[12],p.getJointInfo(quadruped,l1)[12], "enabled=",enableCollision) + p.setCollisionFilterPair(quadruped, quadruped, 2,5,enableCollision) + +jointIds=[] +paramIds=[] +jointOffsets=[] +jointDirections=[-1,1,1,1,1,1,-1,1,1,1,1,1] +jointAngles=[0,0,0,0,0,0,0,0,0,0,0,0] + +for i in range (4): + jointOffsets.append(0) + jointOffsets.append(-0.7) + jointOffsets.append(0.7) + +maxForceId = p.addUserDebugParameter("maxForce",0,100,20) + +for j in range (p.getNumJoints(quadruped)): + p.changeDynamics(quadruped,j,linearDamping=0, angularDamping=0) + info = p.getJointInfo(quadruped,j) + #print(info) + jointName = info[1] + jointType = info[2] + if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): + jointIds.append(j) + + +p.getCameraImage(480,320) +p.setRealTimeSimulation(0) + +joints=[] + + +mocapData = motion_capture_data.MotionCaptureData() + +motionPath = pybullet_data.getDataPath()+"/data/motions/laikago_walk.json" + +mocapData.Load(motionPath) +print("mocapData.NumFrames=",mocapData.NumFrames()) +print("mocapData.KeyFrameDuraction=",mocapData.KeyFrameDuraction()) +print("mocapData.getCycleTime=",mocapData.getCycleTime()) +print("mocapData.computeCycleOffset=",mocapData.computeCycleOffset()) + + +cycleTime = mocapData.getCycleTime() +t=0 + +while t<10.*cycleTime: + #get interpolated joint + keyFrameDuration = mocapData.KeyFrameDuraction() + cycleTime = mocapData.getCycleTime() + cycleCount = mocapData.calcCycleCount(t, cycleTime) + + #print("cycleTime=",cycleTime) + #print("cycleCount=",cycleCount) + + #print("cycles=",cycles) + frameTime = t - cycleCount*cycleTime + #print("frameTime=",frameTime) + if (frameTime<0): + frameTime += cycleTime + + frame = int(frameTime/keyFrameDuration) + frameNext = frame+1 + if (frameNext >= mocapData.NumFrames()): + frameNext = frame + frameFraction = (frameTime - frame*keyFrameDuration)/(keyFrameDuration) + #print("frame=",frame) + #print("frameFraction=",frameFraction) + frameData = mocapData._motion_data['Frames'][frame] + frameDataNext = mocapData._motion_data['Frames'][frameNext] + + joints,qdot=qpi.Slerp(frameFraction, frameData, frameDataNext, p) + + maxForce = p.readUserDebugParameter(maxForceId) + for j in range (12): + targetPos = float(joints[j]) + p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce) + p.stepSimulation() + t+=timeStep + time.sleep(timeStep) + +useOrgData=False +if useOrgData: + with open("data1.txt","r") as filestream: + for line in filestream: + maxForce = p.readUserDebugParameter(maxForceId) + currentline = line.split(",") + frame = currentline[0] + t = currentline[1] + joints=currentline[2:14] + for j in range (12): + targetPos = float(joints[j]) + p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce) + p.stepSimulation() + for lower_leg in lower_legs: + #print("points for ", quadruped, " link: ", lower_leg) + pts = p.getContactPoints(quadruped,-1, lower_leg) + #print("num points=",len(pts)) + #for pt in pts: + # print(pt[9]) + time.sleep(1./500.) + + +for j in range (p.getNumJoints(quadruped)): + p.changeDynamics(quadruped,j,linearDamping=0, angularDamping=0) + info = p.getJointInfo(quadruped,j) + js = p.getJointState(quadruped,j) + #print(info) + jointName = info[1] + jointType = info[2] + if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): + paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,(js[0]-jointOffsets[j])/jointDirections[j])) + + +p.setRealTimeSimulation(1) + +while (1): + + for i in range(len(paramIds)): + c = paramIds[i] + targetPos = p.readUserDebugParameter(c) + maxForce = p.readUserDebugParameter(maxForceId) + p.setJointMotorControl2(quadruped,jointIds[i],p.POSITION_CONTROL,jointDirections[i]*targetPos+jointOffsets[i], force=maxForce) + |