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
@@ -1,4 +1,5 @@
|
||||
import json
|
||||
import math
|
||||
|
||||
class MotionCaptureData(object):
|
||||
def __init__(self):
|
||||
@@ -17,3 +18,25 @@ class MotionCaptureData(object):
|
||||
def KeyFrameDuraction(self):
|
||||
return self._motion_data['Frames'][0][0]
|
||||
|
||||
def getCycleTime(self):
|
||||
keyFrameDuration = self.KeyFrameDuraction()
|
||||
cycleTime = keyFrameDuration*(self.NumFrames()-1)
|
||||
return cycleTime
|
||||
|
||||
def calcCycleCount(self, simTime, cycleTime):
|
||||
phases = simTime / cycleTime;
|
||||
count = math.floor(phases)
|
||||
loop = True
|
||||
#count = (loop) ? count : cMathUtil::Clamp(count, 0, 1);
|
||||
return count
|
||||
|
||||
def computeCycleOffset(self):
|
||||
firstFrame=0
|
||||
lastFrame = self.NumFrames()-1
|
||||
frameData = self._motion_data['Frames'][0]
|
||||
frameDataNext = self._motion_data['Frames'][lastFrame]
|
||||
|
||||
basePosStart = [frameData[1],frameData[2],frameData[3]]
|
||||
basePosEnd = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
|
||||
self._cycleOffset = [basePosEnd[0]-basePosStart[0],basePosEnd[1]-basePosStart[1],basePosEnd[2]-basePosStart[2]]
|
||||
return self._cycleOffset
|
||||
51
examples/pybullet/gym/pybullet_envs/deep_mimic/env/quadrupedPoseInterpolator.py
vendored
Normal file
51
examples/pybullet/gym/pybullet_envs/deep_mimic/env/quadrupedPoseInterpolator.py
vendored
Normal file
@@ -0,0 +1,51 @@
|
||||
from pybullet_utils import bullet_client
|
||||
import math
|
||||
|
||||
class QuadrupedPoseInterpolator(object):
|
||||
def __init__(self):
|
||||
pass
|
||||
|
||||
|
||||
def ComputeLinVel(self,posStart, posEnd, deltaTime):
|
||||
vel = [(posEnd[0]-posStart[0])/deltaTime,(posEnd[1]-posStart[1])/deltaTime,(posEnd[2]-posStart[2])/deltaTime]
|
||||
return vel
|
||||
|
||||
def ComputeAngVel(self,ornStart, ornEnd, deltaTime, bullet_client):
|
||||
dorn = bullet_client.getDifferenceQuaternion(ornStart,ornEnd)
|
||||
axis,angle = bullet_client.getAxisAngleFromQuaternion(dorn)
|
||||
angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
|
||||
return angVel
|
||||
|
||||
def ComputeAngVelRel(self,ornStart, ornEnd, deltaTime, bullet_client):
|
||||
ornStartConjugate = [-ornStart[0],-ornStart[1],-ornStart[2],ornStart[3]]
|
||||
pos_diff, q_diff =bullet_client.multiplyTransforms([0,0,0], ornStartConjugate, [0,0,0], ornEnd)
|
||||
axis,angle = bullet_client.getAxisAngleFromQuaternion(q_diff)
|
||||
angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
|
||||
return angVel
|
||||
|
||||
def Slerp(self, frameFraction, frameData, frameDataNext,bullet_client ):
|
||||
keyFrameDuration = frameData[0]
|
||||
basePos1Start = [frameData[1],frameData[2],frameData[3]]
|
||||
basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
|
||||
self._basePos = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]),
|
||||
basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]),
|
||||
basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
|
||||
self._baseLinVel = self.ComputeLinVel(basePos1Start,basePos1End, keyFrameDuration)
|
||||
baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
|
||||
baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
|
||||
self._baseOrn = bullet_client.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
|
||||
self._baseAngVel = self.ComputeAngVel(baseOrn1Start,baseOrn1Next, keyFrameDuration, bullet_client)
|
||||
|
||||
jointPositions=[]
|
||||
jointVelocities=[]
|
||||
for j in range (12):
|
||||
index=j+8
|
||||
jointPosStart=frameData[index]
|
||||
jointPosEnd=frameDataNext[index]
|
||||
jointPos=jointPosStart+frameFraction*(jointPosEnd-jointPosStart)
|
||||
jointVel=(jointPosEnd-jointPosStart)/keyFrameDuration
|
||||
jointPositions.append(jointPos)
|
||||
jointVelocities.append(jointVel)
|
||||
self._jointPositions = jointPositions
|
||||
self._jointVelocities = jointVelocities
|
||||
return jointPositions,jointVelocities
|
||||
@@ -36,27 +36,6 @@ pybullet_client.setTimeStep(timeStep)
|
||||
pybullet_client.setPhysicsEngineParameter(numSubSteps=2)
|
||||
timeId = pybullet_client.addUserDebugParameter("time",0,10,0)
|
||||
|
||||
|
||||
#startPose = humanoid_pose_interpolator.HumanoidPoseInterpolator()
|
||||
#startPose.Reset(basePos=[-0.000000,0.889540,0.000000],baseOrn=[0.029215,-0.000525,-0.017963,0.999412],chestRot=[0.000432,0.000572,0.005500,0.999985],
|
||||
# neckRot=[0.001660,-0.011168,-0.140597,0.990003],rightHipRot=[-0.024450,-0.000839,-0.038869,0.998945],rightKneeRot=[-0.014186],rightAnkleRot=[0.010757,-0.105223,0.035405,0.993760],
|
||||
# rightShoulderRot=[-0.003003,-0.124234,0.073280,0.989539],rightElbowRot=[0.240463],leftHipRot=[-0.020920,-0.012925,-0.006300,0.999678],leftKneeRot=[-0.027859],
|
||||
# leftAnkleRot=[-0.010764,0.105284,-0.009276,0.994341],leftShoulderRot=[0.055661,-0.019608,0.098917,0.993344],leftElbowRot=[0.148934],
|
||||
# baseLinVel=[-0.340837,0.377742,0.009662],baseAngVel=[0.047057,0.285253,-0.248554],chestVel=[-0.016455,-0.070035,-0.231662],neckVel=[0.072168,0.097898,-0.059063],
|
||||
# rightHipVel=[-0.315908,-0.131685,1.114815],rightKneeVel=[0.103419],rightAnkleVel=[-0.409780,-0.099954,-4.241572],rightShoulderVel=[-3.324227,-2.510209,1.834637],
|
||||
# rightElbowVel=[-0.212299],leftHipVel=[0.173056,-0.191063,1.226781,0.000000],leftKneeVel=[-0.665322],leftAnkleVel=[0.282716,0.086217,-3.007098,0.000000],
|
||||
# leftShoulderVel=[4.253144,2.038637,1.170750],leftElbowVel=[0.387993])
|
||||
#
|
||||
#targetPose = humanoid_pose_interpolator.HumanoidPoseInterpolator()
|
||||
#targetPose.Reset(basePos=[0.000000,0.000000,0.000000],baseOrn=[0.000000,0.000000,0.000000,1.000000],chestRot=[-0.006711,0.007196,-0.027119,0.999584],neckRot=[-0.017613,-0.033879,-0.209250,0.977116],
|
||||
# rightHipRot=[-0.001697,-0.006510,0.046117,0.998913],rightKneeRot=[0.366954],rightAnkleRot=[0.012605,0.001208,-0.187007,0.982277],rightShoulderRot=[-0.468057,-0.044589,0.161134,0.867739],
|
||||
# rightElbowRot=[-0.593650],leftHipRot=[0.006993,0.017242,0.049703,0.998591],leftKneeRot=[0.395147],leftAnkleRot=[-0.008922,0.026517,-0.217852,0.975581],
|
||||
# leftShoulderRot=[0.426160,-0.266177,0.044672,0.863447],leftElbowRot=[-0.726281])
|
||||
|
||||
#out_tau= [0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,-33.338211,-1.381748,-118.708975,0.000000,-2.813919,-2.773850,-0.772481,0.000000,31.885372,2.658243,64.988216,0.000000,94.773133,1.784944,6.240010,5.407563,0.000000,-180.441290,-6.821173,-19.502417,0.000000,-44.518261,9.992627,-2.380950,53.057697,0.000000,111.728594,-1.218496,-4.630812,4.268995,0.000000,89.741829,-8.460265,-117.727884,0.000000,-79.481906]
|
||||
#,mSimWorld->stepSimulation(timestep:0.001667, mParams.mNumSubsteps:2, subtimestep:0.000833)
|
||||
#cImpPDController::CalcControlForces timestep=0.001667
|
||||
|
||||
|
||||
def isKeyTriggered(keys, key):
|
||||
o = ord(key)
|
||||
@@ -67,7 +46,7 @@ def isKeyTriggered(keys, key):
|
||||
animating = False
|
||||
singleStep = False
|
||||
|
||||
#humanoid.initializePose(pose=startPose, phys_model = humanoid._sim_model, initBase=True, initializeVelocity=True)
|
||||
|
||||
t=0
|
||||
while (1):
|
||||
|
||||
|
||||
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