add deepmimic args file (for testing)
implement deepmimic reward function (only joint angles/velocities for now) fix an out-of-date comment, related to contactPoint.m_lateralFrictionInitialized in btSequentialImpulseConstraintSolver.cpp
This commit is contained in:
erwincoumans
@@ -4,6 +4,9 @@ currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentfram
|
||||
parentdir = os.path.dirname(os.path.dirname(currentdir))
|
||||
os.sys.path.insert(0,parentdir)
|
||||
|
||||
from pybullet_utils.bullet_client import BulletClient
|
||||
import pybullet_data
|
||||
|
||||
jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC",
|
||||
"JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"]
|
||||
|
||||
@@ -166,14 +169,24 @@ class Humanoid(object):
|
||||
"""
|
||||
self._baseShift = baseShift
|
||||
self._pybullet_client = pybullet_client
|
||||
|
||||
self.kin_client = BulletClient(pybullet_client.DIRECT)# use SHARED_MEMORY for visual debugging, start a GUI physics server first
|
||||
self.kin_client.resetSimulation()
|
||||
self.kin_client.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
self.kin_client.configureDebugVisualizer(self.kin_client.COV_ENABLE_Y_AXIS_UP,1)
|
||||
self.kin_client.setGravity(0,-9.8,0)
|
||||
|
||||
self._motion_data = motion_data
|
||||
self._humanoid = self._pybullet_client.loadURDF(
|
||||
"humanoid/humanoid.urdf", [0,0.9,0],globalScaling=0.25, useFixedBase=False)
|
||||
|
||||
self._kinematicHumanoid = self.kin_client.loadURDF(
|
||||
"humanoid/humanoid.urdf", [0,0.9,0],globalScaling=0.25, useFixedBase=False)
|
||||
|
||||
#self._humanoidDebug = self._pybullet_client.loadURDF(
|
||||
# "humanoid/humanoid.urdf", [0,0.9,3],globalScaling=0.25, useFixedBase=True)
|
||||
|
||||
print("human #joints=", self._pybullet_client.getNumJoints(self._humanoid))
|
||||
#print("human #joints=", self._pybullet_client.getNumJoints(self._humanoid))
|
||||
pose = HumanoidPose()
|
||||
|
||||
for i in range (self._motion_data.NumFrames()-1):
|
||||
@@ -186,8 +199,8 @@ class Humanoid(object):
|
||||
ji = self._pybullet_client.getJointInfo(self._humanoid,j)
|
||||
self._pybullet_client.changeDynamics(self._humanoid, j, linearDamping=0, angularDamping=0)
|
||||
self._pybullet_client.changeVisualShape(self._humanoid, j , rgbaColor=[1,1,1,1])
|
||||
print("joint[",j,"].type=",jointTypes[ji[2]])
|
||||
print("joint[",j,"].name=",ji[1])
|
||||
#print("joint[",j,"].type=",jointTypes[ji[2]])
|
||||
#print("joint[",j,"].name=",ji[1])
|
||||
|
||||
self._initial_state = self._pybullet_client.saveState()
|
||||
self._allowed_body_parts=[11,14]
|
||||
@@ -195,9 +208,9 @@ class Humanoid(object):
|
||||
|
||||
def Reset(self):
|
||||
self._pybullet_client.restoreState(self._initial_state)
|
||||
self.SetSimTime(100)
|
||||
self.SetSimTime(0)
|
||||
pose = self.InitializePoseFromMotionData()
|
||||
self.ApplyPose(pose, True, True)
|
||||
self.ApplyPose(pose, True, True, self._humanoid, self._pybullet_client)
|
||||
|
||||
def CalcCycleCount(self, simTime, cycleTime):
|
||||
phases = simTime / cycleTime;
|
||||
@@ -208,7 +221,7 @@ class Humanoid(object):
|
||||
|
||||
def SetSimTime(self, t):
|
||||
self._simTime = t
|
||||
print("SetTimeTime time =",t)
|
||||
#print("SetTimeTime time =",t)
|
||||
keyFrameDuration = self._motion_data.KeyFrameDuraction()
|
||||
cycleTime = keyFrameDuration*(self._motion_data.NumFrames()-1)
|
||||
#print("self._motion_data.NumFrames()=",self._motion_data.NumFrames())
|
||||
@@ -253,7 +266,7 @@ class Humanoid(object):
|
||||
rotVec = self._pybullet_client.rotateVector(rootOrn, refDir)
|
||||
heading = math.atan2(-rotVec[2], rotVec[0])
|
||||
heading2=eul[1]
|
||||
print("heading=",heading)
|
||||
#print("heading=",heading)
|
||||
headingOrn = self._pybullet_client.getQuaternionFromAxisAngle([0,1,0],-heading)
|
||||
return headingOrn
|
||||
|
||||
@@ -269,21 +282,21 @@ class Humanoid(object):
|
||||
def BuildOriginTrans(self):
|
||||
rootPos,rootOrn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid)
|
||||
|
||||
print("rootPos=",rootPos, " rootOrn=",rootOrn)
|
||||
#print("rootPos=",rootPos, " rootOrn=",rootOrn)
|
||||
invRootPos=[-rootPos[0], 0, -rootPos[2]]
|
||||
#invOrigTransPos, invOrigTransOrn = self._pybullet_client.invertTransform(rootPos,rootOrn)
|
||||
headingOrn = self.BuildHeadingTrans(rootOrn)
|
||||
print("headingOrn=",headingOrn)
|
||||
#print("headingOrn=",headingOrn)
|
||||
headingMat = self._pybullet_client.getMatrixFromQuaternion(headingOrn)
|
||||
print("headingMat=",headingMat)
|
||||
#print("headingMat=",headingMat)
|
||||
#dummy, rootOrnWithoutHeading = self._pybullet_client.multiplyTransforms([0,0,0],headingOrn, [0,0,0], rootOrn)
|
||||
#dummy, invOrigTransOrn = self._pybullet_client.multiplyTransforms([0,0,0],rootOrnWithoutHeading, invOrigTransPos, invOrigTransOrn)
|
||||
|
||||
invOrigTransPos, invOrigTransOrn = self._pybullet_client.multiplyTransforms( [0,0,0],headingOrn, invRootPos,[0,0,0,1])
|
||||
print("invOrigTransPos=",invOrigTransPos)
|
||||
print("invOrigTransOrn=",invOrigTransOrn)
|
||||
#print("invOrigTransPos=",invOrigTransPos)
|
||||
#print("invOrigTransOrn=",invOrigTransOrn)
|
||||
invOrigTransMat = self._pybullet_client.getMatrixFromQuaternion(invOrigTransOrn)
|
||||
print("invOrigTransMat =",invOrigTransMat )
|
||||
#print("invOrigTransMat =",invOrigTransMat )
|
||||
return invOrigTransPos, invOrigTransOrn
|
||||
|
||||
def InitializePoseFromMotionData(self):
|
||||
@@ -360,28 +373,30 @@ class Humanoid(object):
|
||||
pose._leftElbowRot = [angle]
|
||||
|
||||
|
||||
print("index=",index)
|
||||
#print("index=",index)
|
||||
|
||||
initializeBase = False
|
||||
initializeVelocities = False
|
||||
self.ApplyPose(pose, initializeBase, initializeVelocities)
|
||||
self.ApplyPose(pose, initializeBase, initializeVelocities, self._humanoid, self._pybullet_client)
|
||||
|
||||
|
||||
def ApplyPose(self, pose, initializeBase, initializeVelocities):
|
||||
def ApplyPose(self, pose, initializeBase, initializeVelocities, humanoid,bc):
|
||||
#todo: get tunable parametes from a json file or from URDF (kd, maxForce)
|
||||
if (initializeBase):
|
||||
self._pybullet_client.changeVisualShape(self._humanoid, 2 , rgbaColor=[1,0,0,1])
|
||||
bc.changeVisualShape(humanoid, 2 , rgbaColor=[1,0,0,1])
|
||||
basePos=[pose._basePos[0]+self._baseShift[0],pose._basePos[1]+self._baseShift[1],pose._basePos[2]+self._baseShift[2]]
|
||||
|
||||
self._pybullet_client.resetBasePositionAndOrientation(self._humanoid,
|
||||
bc.resetBasePositionAndOrientation(humanoid,
|
||||
basePos, pose._baseOrn)
|
||||
if initializeVelocities:
|
||||
self._pybullet_client.resetBaseVelocity(self._humanoid, pose._baseLinVel, pose._baseAngVel)
|
||||
bc.resetBaseVelocity(humanoid, pose._baseLinVel, pose._baseAngVel)
|
||||
#print("resetBaseVelocity=",pose._baseLinVel)
|
||||
else:
|
||||
self._pybullet_client.changeVisualShape(self._humanoid, 2 , rgbaColor=[1,1,1,1])
|
||||
bc.changeVisualShape(humanoid, 2 , rgbaColor=[1,1,1,1])
|
||||
|
||||
kp=0.01
|
||||
|
||||
|
||||
kp=0.03
|
||||
chest=1
|
||||
neck=2
|
||||
rightShoulder=3
|
||||
@@ -394,63 +409,63 @@ class Humanoid(object):
|
||||
leftHip = 12
|
||||
leftKnee=13
|
||||
leftAnkle=14
|
||||
controlMode = self._pybullet_client.POSITION_CONTROL
|
||||
controlMode = bc.POSITION_CONTROL
|
||||
|
||||
if (initializeBase):
|
||||
if initializeVelocities:
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,chest,pose._chestRot, pose._chestVel)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,neck,pose._neckRot, pose._neckVel)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightHip,pose._rightHipRot, pose._rightHipVel)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightKnee,pose._rightKneeRot, pose._rightKneeVel)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightAnkle,pose._rightAnkleRot, pose._rightAnkleVel)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightShoulder,pose._rightShoulderRot, pose._rightShoulderVel)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightElbow, pose._rightElbowRot, pose._rightElbowVel)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftHip, pose._leftHipRot, pose._leftHipVel)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftKnee, pose._leftKneeRot, pose._leftKneeVel)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftAnkle, pose._leftAnkleRot, pose._leftAnkleVel)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftShoulder, pose._leftShoulderRot, pose._leftShoulderVel)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftElbow, pose._leftElbowRot, pose._leftElbowVel)
|
||||
bc.resetJointStateMultiDof(humanoid,chest,pose._chestRot, pose._chestVel)
|
||||
bc.resetJointStateMultiDof(humanoid,neck,pose._neckRot, pose._neckVel)
|
||||
bc.resetJointStateMultiDof(humanoid,rightHip,pose._rightHipRot, pose._rightHipVel)
|
||||
bc.resetJointStateMultiDof(humanoid,rightKnee,pose._rightKneeRot, pose._rightKneeVel)
|
||||
bc.resetJointStateMultiDof(humanoid,rightAnkle,pose._rightAnkleRot, pose._rightAnkleVel)
|
||||
bc.resetJointStateMultiDof(humanoid,rightShoulder,pose._rightShoulderRot, pose._rightShoulderVel)
|
||||
bc.resetJointStateMultiDof(humanoid,rightElbow, pose._rightElbowRot, pose._rightElbowVel)
|
||||
bc.resetJointStateMultiDof(humanoid,leftHip, pose._leftHipRot, pose._leftHipVel)
|
||||
bc.resetJointStateMultiDof(humanoid,leftKnee, pose._leftKneeRot, pose._leftKneeVel)
|
||||
bc.resetJointStateMultiDof(humanoid,leftAnkle, pose._leftAnkleRot, pose._leftAnkleVel)
|
||||
bc.resetJointStateMultiDof(humanoid,leftShoulder, pose._leftShoulderRot, pose._leftShoulderVel)
|
||||
bc.resetJointStateMultiDof(humanoid,leftElbow, pose._leftElbowRot, pose._leftElbowVel)
|
||||
else:
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,chest,pose._chestRot)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,neck,pose._neckRot)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightHip,pose._rightHipRot)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightKnee,pose._rightKneeRot)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightAnkle,pose._rightAnkleRot)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightShoulder,pose._rightShoulderRot)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightElbow, pose._rightElbowRot)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftHip, pose._leftHipRot)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftKnee, pose._leftKneeRot)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftAnkle, pose._leftAnkleRot)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftShoulder, pose._leftShoulderRot)
|
||||
self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftElbow, pose._leftElbowRot)
|
||||
bc.resetJointStateMultiDof(humanoid,chest,pose._chestRot)
|
||||
bc.resetJointStateMultiDof(humanoid,neck,pose._neckRot)
|
||||
bc.resetJointStateMultiDof(humanoid,rightHip,pose._rightHipRot)
|
||||
bc.resetJointStateMultiDof(humanoid,rightKnee,pose._rightKneeRot)
|
||||
bc.resetJointStateMultiDof(humanoid,rightAnkle,pose._rightAnkleRot)
|
||||
bc.resetJointStateMultiDof(humanoid,rightShoulder,pose._rightShoulderRot)
|
||||
bc.resetJointStateMultiDof(humanoid,rightElbow, pose._rightElbowRot)
|
||||
bc.resetJointStateMultiDof(humanoid,leftHip, pose._leftHipRot)
|
||||
bc.resetJointStateMultiDof(humanoid,leftKnee, pose._leftKneeRot)
|
||||
bc.resetJointStateMultiDof(humanoid,leftAnkle, pose._leftAnkleRot)
|
||||
bc.resetJointStateMultiDof(humanoid,leftShoulder, pose._leftShoulderRot)
|
||||
bc.resetJointStateMultiDof(humanoid,leftElbow, pose._leftElbowRot)
|
||||
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,chest,controlMode, targetPosition=pose._chestRot,positionGain=kp, force=200)
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,neck,controlMode,targetPosition=pose._neckRot,positionGain=kp, force=50)
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightHip,controlMode,targetPosition=pose._rightHipRot,positionGain=kp, force=200)
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightKnee,controlMode,targetPosition=pose._rightKneeRot,positionGain=kp, force=150)
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightAnkle,controlMode,targetPosition=pose._rightAnkleRot,positionGain=kp, force=90)
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightShoulder,controlMode,targetPosition=pose._rightShoulderRot,positionGain=kp, force=100)
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightElbow, controlMode,targetPosition=pose._rightElbowRot,positionGain=kp, force=60)
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftHip, controlMode,targetPosition=pose._leftHipRot,positionGain=kp, force=200)
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftKnee, controlMode,targetPosition=pose._leftKneeRot,positionGain=kp, force=150)
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftAnkle, controlMode,targetPosition=pose._leftAnkleRot,positionGain=kp, force=90)
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftShoulder, controlMode,targetPosition=pose._leftShoulderRot,positionGain=kp, force=100)
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftElbow, controlMode,targetPosition=pose._leftElbowRot,positionGain=kp, force=60)
|
||||
bc.setJointMotorControlMultiDof(humanoid,chest,controlMode, targetPosition=pose._chestRot,positionGain=kp, force=200)
|
||||
bc.setJointMotorControlMultiDof(humanoid,neck,controlMode,targetPosition=pose._neckRot,positionGain=kp, force=50)
|
||||
bc.setJointMotorControlMultiDof(humanoid,rightHip,controlMode,targetPosition=pose._rightHipRot,positionGain=kp, force=200)
|
||||
bc.setJointMotorControlMultiDof(humanoid,rightKnee,controlMode,targetPosition=pose._rightKneeRot,positionGain=kp, force=150)
|
||||
bc.setJointMotorControlMultiDof(humanoid,rightAnkle,controlMode,targetPosition=pose._rightAnkleRot,positionGain=kp, force=90)
|
||||
bc.setJointMotorControlMultiDof(humanoid,rightShoulder,controlMode,targetPosition=pose._rightShoulderRot,positionGain=kp, force=100)
|
||||
bc.setJointMotorControlMultiDof(humanoid,rightElbow, controlMode,targetPosition=pose._rightElbowRot,positionGain=kp, force=60)
|
||||
bc.setJointMotorControlMultiDof(humanoid,leftHip, controlMode,targetPosition=pose._leftHipRot,positionGain=kp, force=200)
|
||||
bc.setJointMotorControlMultiDof(humanoid,leftKnee, controlMode,targetPosition=pose._leftKneeRot,positionGain=kp, force=150)
|
||||
bc.setJointMotorControlMultiDof(humanoid,leftAnkle, controlMode,targetPosition=pose._leftAnkleRot,positionGain=kp, force=90)
|
||||
bc.setJointMotorControlMultiDof(humanoid,leftShoulder, controlMode,targetPosition=pose._leftShoulderRot,positionGain=kp, force=100)
|
||||
bc.setJointMotorControlMultiDof(humanoid,leftElbow, controlMode,targetPosition=pose._leftElbowRot,positionGain=kp, force=60)
|
||||
|
||||
#debug space
|
||||
#if (False):
|
||||
# for j in range (self._pybullet_client.getNumJoints(self._humanoid)):
|
||||
# js = self._pybullet_client.getJointState(self._humanoid, j)
|
||||
# self._pybullet_client.resetJointState(self._humanoidDebug, j,js[0])
|
||||
# jsm = self._pybullet_client.getJointStateMultiDof(self._humanoid, j)
|
||||
# for j in range (bc.getNumJoints(self._humanoid)):
|
||||
# js = bc.getJointState(self._humanoid, j)
|
||||
# bc.resetJointState(self._humanoidDebug, j,js[0])
|
||||
# jsm = bc.getJointStateMultiDof(self._humanoid, j)
|
||||
# if (len(jsm[0])>0):
|
||||
# self._pybullet_client.resetJointStateMultiDof(self._humanoidDebug,j,jsm[0])
|
||||
# bc.resetJointStateMultiDof(self._humanoidDebug,j,jsm[0])
|
||||
|
||||
def GetState(self):
|
||||
|
||||
stateVector = []
|
||||
phase = self.GetPhase()
|
||||
print("phase=",phase)
|
||||
#print("phase=",phase)
|
||||
stateVector.append(phase)
|
||||
|
||||
rootTransPos, rootTransOrn=self.BuildOriginTrans()
|
||||
@@ -459,16 +474,19 @@ class Humanoid(object):
|
||||
rootPosRel, dummy = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn, basePos,[0,0,0,1])
|
||||
#print("!!!rootPosRel =",rootPosRel )
|
||||
#print("rootTransPos=",rootTransPos)
|
||||
print("basePos=",basePos)
|
||||
#print("basePos=",basePos)
|
||||
localPos,localOrn = self._pybullet_client.multiplyTransforms( rootTransPos, rootTransOrn , basePos,baseOrn )
|
||||
|
||||
localPos=[localPos[0]-rootPosRel[0],localPos[1]-rootPosRel[1],localPos[2]-rootPosRel[2]]
|
||||
print("localPos=",localPos)
|
||||
#print("localPos=",localPos)
|
||||
|
||||
stateVector.append(rootPosRel[1])
|
||||
|
||||
self.pb2dmJoints=[0,1,2,9,10,11,3,4,5,12,13,14,6,7,8]
|
||||
|
||||
for j in range (self._pybullet_client.getNumJoints(self._humanoid)):
|
||||
for pbJoint in range (self._pybullet_client.getNumJoints(self._humanoid)):
|
||||
j = self.pb2dmJoints[pbJoint]
|
||||
#print("joint order:",j)
|
||||
ls = self._pybullet_client.getLinkState(self._humanoid, j, computeForwardKinematics=True)
|
||||
linkPos = ls[0]
|
||||
linkOrn = ls[1]
|
||||
@@ -478,11 +496,16 @@ class Humanoid(object):
|
||||
linkPosLocal=[linkPosLocal[0]-rootPosRel[0],linkPosLocal[1]-rootPosRel[1],linkPosLocal[2]-rootPosRel[2]]
|
||||
for l in linkPosLocal:
|
||||
stateVector.append(l)
|
||||
for l in linkOrnLocal:
|
||||
stateVector.append(l)
|
||||
|
||||
#re-order the quaternion, DeepMimic uses w,x,y,z
|
||||
stateVector.append(linkOrnLocal[3])
|
||||
stateVector.append(linkOrnLocal[0])
|
||||
stateVector.append(linkOrnLocal[1])
|
||||
stateVector.append(linkOrnLocal[2])
|
||||
|
||||
|
||||
for j in range (self._pybullet_client.getNumJoints(self._humanoid)):
|
||||
for pbJoint in range (self._pybullet_client.getNumJoints(self._humanoid)):
|
||||
j = self.pb2dmJoints[pbJoint]
|
||||
ls = self._pybullet_client.getLinkState(self._humanoid, j, computeLinkVelocity=True)
|
||||
linkLinVel = ls[6]
|
||||
linkAngVel = ls[7]
|
||||
@@ -491,7 +514,167 @@ class Humanoid(object):
|
||||
for l in linkAngVel:
|
||||
stateVector.append(l)
|
||||
|
||||
print("stateVector len=",len(stateVector))
|
||||
#print("stateVector len=",len(stateVector))
|
||||
#for st in range (len(stateVector)):
|
||||
# print("state[",st,"]=",stateVector[st])
|
||||
return stateVector
|
||||
|
||||
|
||||
|
||||
def GetReward(self):
|
||||
#from DeepMimic double cSceneImitate::CalcRewardImitate
|
||||
pose_w = 0.5
|
||||
vel_w = 0.05
|
||||
end_eff_w = 0 #0.15
|
||||
root_w = 0 #0.2
|
||||
com_w = 0.1
|
||||
|
||||
total_w = pose_w + vel_w + end_eff_w + root_w + com_w
|
||||
pose_w /= total_w
|
||||
vel_w /= total_w
|
||||
end_eff_w /= total_w
|
||||
root_w /= total_w
|
||||
com_w /= total_w
|
||||
|
||||
pose_scale = 2
|
||||
vel_scale = 0.1
|
||||
end_eff_scale = 40
|
||||
root_scale = 5
|
||||
com_scale = 10
|
||||
err_scale = 1
|
||||
|
||||
reward = 0
|
||||
|
||||
pose_err = 0
|
||||
vel_err = 0
|
||||
end_eff_err = 0
|
||||
root_err = 0
|
||||
com_err = 0
|
||||
heading_err = 0
|
||||
|
||||
#create a mimic reward, comparing the dynamics humanoid with a kinematic one
|
||||
|
||||
pose = self.InitializePoseFromMotionData()
|
||||
#print("self._kinematicHumanoid=",self._kinematicHumanoid)
|
||||
#print("kinematicHumanoid #joints=",self.kin_client.getNumJoints(self._kinematicHumanoid))
|
||||
self.ApplyPose(pose, True, True, self._kinematicHumanoid, self.kin_client)
|
||||
|
||||
#const Eigen::VectorXd& pose0 = sim_char.GetPose();
|
||||
#const Eigen::VectorXd& vel0 = sim_char.GetVel();
|
||||
#const Eigen::VectorXd& pose1 = kin_char.GetPose();
|
||||
#const Eigen::VectorXd& vel1 = kin_char.GetVel();
|
||||
#tMatrix origin_trans = sim_char.BuildOriginTrans();
|
||||
#tMatrix kin_origin_trans = kin_char.BuildOriginTrans();
|
||||
#
|
||||
#tVector com0_world = sim_char.CalcCOM();
|
||||
#tVector com_vel0_world = sim_char.CalcCOMVel();
|
||||
#tVector com1_world;
|
||||
#tVector com_vel1_world;
|
||||
#cRBDUtil::CalcCoM(joint_mat, body_defs, pose1, vel1, com1_world, com_vel1_world);
|
||||
#
|
||||
root_id = 0
|
||||
#tVector root_pos0 = cKinTree::GetRootPos(joint_mat, pose0);
|
||||
#tVector root_pos1 = cKinTree::GetRootPos(joint_mat, pose1);
|
||||
#tQuaternion root_rot0 = cKinTree::GetRootRot(joint_mat, pose0);
|
||||
#tQuaternion root_rot1 = cKinTree::GetRootRot(joint_mat, pose1);
|
||||
#tVector root_vel0 = cKinTree::GetRootVel(joint_mat, vel0);
|
||||
#tVector root_vel1 = cKinTree::GetRootVel(joint_mat, vel1);
|
||||
#tVector root_ang_vel0 = cKinTree::GetRootAngVel(joint_mat, vel0);
|
||||
#tVector root_ang_vel1 = cKinTree::GetRootAngVel(joint_mat, vel1);
|
||||
|
||||
mJointWeights = [0.20833,0.10416, 0.0625, 0.10416,
|
||||
0.0625, 0.041666666666666671, 0.0625, 0.0416,
|
||||
0.00, 0.10416, 0.0625, 0.0416, 0.0625, 0.0416, 0.0000]
|
||||
|
||||
num_end_effs = 0
|
||||
num_joints = 15
|
||||
|
||||
root_rot_w = mJointWeights[root_id]
|
||||
#pose_err += root_rot_w * cKinTree::CalcRootRotErr(joint_mat, pose0, pose1)
|
||||
#vel_err += root_rot_w * cKinTree::CalcRootAngVelErr(joint_mat, vel0, vel1)
|
||||
|
||||
for j in range (num_joints):
|
||||
curr_pose_err = 0
|
||||
curr_vel_err = 0
|
||||
w = mJointWeights[j];
|
||||
|
||||
simJointInfo = self._pybullet_client.getJointStateMultiDof(self._humanoid, j)
|
||||
|
||||
#print("simJointInfo.pos=",simJointInfo[0])
|
||||
#print("simJointInfo.vel=",simJointInfo[1])
|
||||
kinJointInfo = self.kin_client.getJointStateMultiDof(self._kinematicHumanoid,j)
|
||||
#print("kinJointInfo.pos=",kinJointInfo[0])
|
||||
#print("kinJointInfo.vel=",kinJointInfo[1])
|
||||
if (len(simJointInfo[0])==1):
|
||||
angle = simJointInfo[0][0]-kinJointInfo[0][0]
|
||||
curr_pose_err = angle*angle
|
||||
velDiff = simJointInfo[1][0]-kinJointInfo[1][0]
|
||||
curr_vel_err = velDiff*velDiff
|
||||
if (len(simJointInfo[0])==4):
|
||||
#print("quaternion diff")
|
||||
diffQuat = self._pybullet_client.getDifferenceQuaternion(simJointInfo[0],kinJointInfo[0])
|
||||
axis,angle = self._pybullet_client.getAxisAngleFromQuaternion(diffQuat)
|
||||
curr_pose_err = angle*angle
|
||||
diffVel = [simJointInfo[1][0]-kinJointInfo[1][0],simJointInfo[1][1]-kinJointInfo[1][1],simJointInfo[1][2]-kinJointInfo[1][2]]
|
||||
curr_vel_err = diffVel[0]*diffVel[0]+diffVel[1]*diffVel[1]+diffVel[2]*diffVel[2]
|
||||
|
||||
|
||||
pose_err += w * curr_pose_err
|
||||
vel_err += w * curr_vel_err
|
||||
|
||||
# bool is_end_eff = sim_char.IsEndEffector(j)
|
||||
# if (is_end_eff)
|
||||
# {
|
||||
# tVector pos0 = sim_char.CalcJointPos(j)
|
||||
# tVector pos1 = cKinTree::CalcJointWorldPos(joint_mat, pose1, j)
|
||||
# double ground_h0 = mGround->SampleHeight(pos0)
|
||||
# double ground_h1 = kin_char.GetOriginPos()[1]
|
||||
#
|
||||
# tVector pos_rel0 = pos0 - root_pos0
|
||||
# tVector pos_rel1 = pos1 - root_pos1
|
||||
# pos_rel0[1] = pos0[1] - ground_h0
|
||||
# pos_rel1[1] = pos1[1] - ground_h1
|
||||
#
|
||||
# pos_rel0 = origin_trans * pos_rel0
|
||||
# pos_rel1 = kin_origin_trans * pos_rel1
|
||||
#
|
||||
# curr_end_err = (pos_rel1 - pos_rel0).squaredNorm()
|
||||
# end_eff_err += curr_end_err;
|
||||
# ++num_end_effs;
|
||||
# }
|
||||
#}
|
||||
#if (num_end_effs > 0):
|
||||
# end_eff_err /= num_end_effs
|
||||
#
|
||||
#double root_ground_h0 = mGround->SampleHeight(sim_char.GetRootPos())
|
||||
#double root_ground_h1 = kin_char.GetOriginPos()[1]
|
||||
#root_pos0[1] -= root_ground_h0
|
||||
#root_pos1[1] -= root_ground_h1
|
||||
#root_pos_err = (root_pos0 - root_pos1).squaredNorm()
|
||||
#
|
||||
#root_rot_err = cMathUtil::QuatDiffTheta(root_rot0, root_rot1)
|
||||
#root_rot_err *= root_rot_err
|
||||
|
||||
#root_vel_err = (root_vel1 - root_vel0).squaredNorm()
|
||||
#root_ang_vel_err = (root_ang_vel1 - root_ang_vel0).squaredNorm()
|
||||
|
||||
#root_err = root_pos_err
|
||||
# + 0.1 * root_rot_err
|
||||
# + 0.01 * root_vel_err
|
||||
# + 0.001 * root_ang_vel_err
|
||||
#com_err = 0.1 * (com_vel1_world - com_vel0_world).squaredNorm()
|
||||
|
||||
#print("pose_err=",pose_err)
|
||||
#print("vel_err=",vel_err)
|
||||
pose_reward = math.exp(-err_scale * pose_scale * pose_err)
|
||||
vel_reward = math.exp(-err_scale * vel_scale * vel_err)
|
||||
end_eff_reward = math.exp(-err_scale * end_eff_scale * end_eff_err)
|
||||
root_reward = math.exp(-err_scale * root_scale * root_err)
|
||||
com_reward = math.exp(-err_scale * com_scale * com_err)
|
||||
|
||||
reward = pose_w * pose_reward + vel_w * vel_reward + end_eff_w * end_eff_reward + root_w * root_reward + com_w * com_reward
|
||||
|
||||
#print("reward = %f (pose_reward=%f, vel_reward=%f, end_eff_reward=%f, root_reward=%f, com_reward=%f)\n", reward,
|
||||
# pose_reward,vel_reward,end_eff_reward, root_reward, com_reward);
|
||||
|
||||
|
||||
return reward
|
||||
@@ -19,19 +19,19 @@ motion=MotionCaptureData()
|
||||
|
||||
motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"#humanoid3d_spinkick.txt"#/motions/humanoid3d_backflip.txt"
|
||||
motion.Load(motionPath)
|
||||
print("numFrames = ", motion.NumFrames())
|
||||
#print("numFrames = ", motion.NumFrames())
|
||||
simTimeId= bc.addUserDebugParameter("simTime",0,motion.NumFrames()-1.1,0)
|
||||
|
||||
y2zOrn = bc.getQuaternionFromEuler([-1.57,0,0])
|
||||
bc.loadURDF("plane.urdf",[0,-0.04,0], y2zOrn)
|
||||
|
||||
humanoid = Humanoid(bc, motion,[0,0,0])
|
||||
humanoid = Humanoid(bc, motion,[0,0,0])#4000,0,5000])
|
||||
|
||||
simTime = 0
|
||||
|
||||
|
||||
keyFrameDuration = motion.KeyFrameDuraction()
|
||||
print("keyFrameDuration=",keyFrameDuration)
|
||||
#print("keyFrameDuration=",keyFrameDuration)
|
||||
#for i in range (50):
|
||||
# bc.stepSimulation()
|
||||
|
||||
@@ -44,25 +44,25 @@ stage = 0
|
||||
def Reset(humanoid):
|
||||
global simTime
|
||||
humanoid.Reset()
|
||||
simTime = random.randint(0,motion.NumFrames()-2)
|
||||
simTime = 0 #random.randint(0,motion.NumFrames()-2)
|
||||
humanoid.SetSimTime(simTime)
|
||||
pose = humanoid.InitializePoseFromMotionData()
|
||||
humanoid.ApplyPose(pose, True, True)
|
||||
humanoid.ApplyPose(pose, True, True, humanoid._humanoid,bc)
|
||||
|
||||
|
||||
Reset(humanoid)
|
||||
bc.stepSimulation()
|
||||
#bc.stepSimulation()
|
||||
|
||||
|
||||
while (1):
|
||||
#simTime = bc.readUserDebugParameter(frameTimeId)
|
||||
#print("keyFrameDuration=",keyFrameDuration)
|
||||
dt = (1./240.)
|
||||
print("------------------------------------------")
|
||||
print("dt=",dt)
|
||||
#print("------------------------------------------")
|
||||
#print("dt=",dt)
|
||||
|
||||
print("simTime=",simTime)
|
||||
print("humanoid.SetSimTime(simTime)")
|
||||
#print("simTime=",simTime)
|
||||
#print("humanoid.SetSimTime(simTime)")
|
||||
humanoid.SetSimTime(simTime)
|
||||
|
||||
#pose = humanoid.InitializePoseFromMotionData()
|
||||
@@ -75,8 +75,10 @@ while (1):
|
||||
action = [0]*36
|
||||
humanoid.ApplyAction(action)
|
||||
for s in range (8):
|
||||
print("step:",s)
|
||||
#print("step:",s)
|
||||
bc.stepSimulation()
|
||||
simTime += dt
|
||||
time.sleep(1./240.)
|
||||
|
||||
reward = humanoid.GetReward()
|
||||
print("reward=",reward)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user