Added laikago mocap data for a DeepMimic compatible walk cycle
Added testLaikago.py script to test this mocap data.
This commit is contained in:
erwincoumans
201
examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py
vendored
Normal file
201
examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py
vendored
Normal file
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user