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 | 342 |
1 files changed, 180 insertions, 162 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 index 1ad8972ed..ef2b21c33 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py @@ -8,240 +8,258 @@ import motion_capture_data import quadrupedPoseInterpolator useConstraints = False - -p = bullet_client.BulletClient(connection_mode=p1.GUI) + +p = bullet_client.BulletClient(connection_mode=p1.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) plane = p.loadURDF("plane.urdf") -p.setGravity(0,0,-10) -timeStep=1./500 +p.setGravity(0, 0, -10) +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+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) +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) if not useConstraints: for j in range(p.getNumJoints(quadruped)): - p.setJointMotorControl2(quadruped,j,p.POSITION_CONTROL,force=0) - -#This cube is added as a soft constraint to keep the laikago from falling + p.setJointMotorControl2(quadruped, j, p.POSITION_CONTROL, force=0) + +#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) +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.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] -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): +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) +maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20) - -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]) - + 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)) +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] +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) + 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) -joints=[] +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" +motionPath = pybullet_data.getDataPath() + "/data/motions/laikago_walk.txt" mocapData.Load(motionPath) -print("mocapData.NumFrames=",mocapData.NumFrames()) -print("mocapData.KeyFrameDuraction=",mocapData.KeyFrameDuraction()) -print("mocapData.getCycleTime=",mocapData.getCycleTime()) -print("mocapData.computeCycleOffset=",mocapData.computeCycleOffset()) +print("mocapData.NumFrames=", mocapData.NumFrames()) +print("mocapData.KeyFrameDuraction=", mocapData.KeyFrameDuraction()) +print("mocapData.getCycleTime=", mocapData.getCycleTime()) +print("mocapData.computeCycleOffset=", mocapData.computeCycleOffset()) stablePD = pd_controller_stable.PDControllerStable(p) cycleTime = mocapData.getCycleTime() -t=0 +t = 0 -while t<10.*cycleTime: +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 + frameTime = t - cycleCount * cycleTime #print("frameTime=",frameTime) - if (frameTime<0): + if (frameTime < 0): frameTime += cycleTime - frame = int(frameTime/keyFrameDuration) - frameNext = frame+1 - if (frameNext >= mocapData.NumFrames()): + frame = int(frameTime / keyFrameDuration) + frameNext = frame + 1 + if (frameNext >= mocapData.NumFrames()): frameNext = frame - frameFraction = (frameTime - frame*keyFrameDuration)/(keyFrameDuration) + frameFraction = (frameTime - frame * keyFrameDuration) / (keyFrameDuration) #print("frame=",frame) #print("frameFraction=",frameFraction) frameData = mocapData._motion_data['Frames'][frame] frameDataNext = mocapData._motion_data['Frames'][frameNext] - - jointsStr,qdot=qpi.Slerp(frameFraction, frameData, frameDataNext, p) - + + jointsStr, qdot = qpi.Slerp(frameFraction, frameData, frameDataNext, p) + maxForce = p.readUserDebugParameter(maxForceId) - print("jointIds=",jointIds) + print("jointIds=", jointIds) if useConstraints: - for j in range (12): + for j in range(12): #skip the base positional dofs - targetPos = float(jointsStr[j+7]) - p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce) - + targetPos = float(jointsStr[j + 7]) + p.setJointMotorControl2(quadruped, + jointIds[j], + p.POSITION_CONTROL, + jointDirections[j] * targetPos + jointOffsets[j], + force=maxForce) + else: - desiredPositions=[] - for j in range (7): + desiredPositions = [] + for j in range(7): targetPosUnmodified = float(jointsStr[j]) desiredPositions.append(targetPosUnmodified) - for j in range (12): - targetPosUnmodified = float(jointsStr[j+7]) - targetPos=jointDirections[j]*targetPosUnmodified+jointOffsets[j] + for j in range(12): + targetPosUnmodified = float(jointsStr[j + 7]) + targetPos = jointDirections[j] * targetPosUnmodified + jointOffsets[j] desiredPositions.append(targetPos) - numBaseDofs=6 - totalDofs=12+numBaseDofs - desiredVelocities=None - if desiredVelocities==None: - desiredVelocities = [0]*totalDofs - taus = stablePD.computePD(bodyUniqueId=quadruped, - jointIndices = jointIds, - desiredPositions = desiredPositions, - desiredVelocities = desiredVelocities, - kps = [4000]*totalDofs, - kds = [40]*totalDofs, - maxForces = [500]*totalDofs, - timeStep=timeStep) - - dofIndex=6 + numBaseDofs = 6 + totalDofs = 12 + numBaseDofs + desiredVelocities = None + if desiredVelocities == None: + desiredVelocities = [0] * totalDofs + taus = stablePD.computePD(bodyUniqueId=quadruped, + jointIndices=jointIds, + desiredPositions=desiredPositions, + desiredVelocities=desiredVelocities, + kps=[4000] * totalDofs, + kds=[40] * totalDofs, + maxForces=[500] * totalDofs, + timeStep=timeStep) + + dofIndex = 6 scaling = 1 - for index in range (len(jointIds)): + for index in range(len(jointIds)): jointIndex = jointIds[index] - force=[scaling*taus[dofIndex]] - print("force[", jointIndex,"]=",force) - p.setJointMotorControlMultiDof(quadruped, jointIndex, controlMode=p.TORQUE_CONTROL, force=force) - dofIndex+=1 - + force = [scaling * taus[dofIndex]] + print("force[", jointIndex, "]=", force) + p.setJointMotorControlMultiDof(quadruped, + jointIndex, + controlMode=p.TORQUE_CONTROL, + force=force) + dofIndex += 1 p.stepSimulation() - t+=timeStep + t += timeStep time.sleep(timeStep) -useOrgData=False +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])) + 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) - + + 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) |