796 lines
34 KiB
Python
796 lines
34 KiB
Python
import os, inspect
|
|
import math
|
|
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
|
|
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"
|
|
]
|
|
|
|
|
|
class HumanoidPose(object):
|
|
|
|
def __init__(self):
|
|
pass
|
|
|
|
def Reset(self):
|
|
|
|
self._basePos = [0, 0, 0]
|
|
self._baseLinVel = [0, 0, 0]
|
|
self._baseOrn = [0, 0, 0, 1]
|
|
self._baseAngVel = [0, 0, 0]
|
|
|
|
self._chestRot = [0, 0, 0, 1]
|
|
self._chestVel = [0, 0, 0]
|
|
self._neckRot = [0, 0, 0, 1]
|
|
self._neckVel = [0, 0, 0]
|
|
|
|
self._rightHipRot = [0, 0, 0, 1]
|
|
self._rightHipVel = [0, 0, 0]
|
|
self._rightKneeRot = [0]
|
|
self._rightKneeVel = [0]
|
|
self._rightAnkleRot = [0, 0, 0, 1]
|
|
self._rightAnkleVel = [0, 0, 0]
|
|
|
|
self._rightShoulderRot = [0, 0, 0, 1]
|
|
self._rightShoulderVel = [0, 0, 0]
|
|
self._rightElbowRot = [0]
|
|
self._rightElbowVel = [0]
|
|
|
|
self._leftHipRot = [0, 0, 0, 1]
|
|
self._leftHipVel = [0, 0, 0]
|
|
self._leftKneeRot = [0]
|
|
self._leftKneeVel = [0]
|
|
self._leftAnkleRot = [0, 0, 0, 1]
|
|
self._leftAnkleVel = [0, 0, 0]
|
|
|
|
self._leftShoulderRot = [0, 0, 0, 1]
|
|
self._leftShoulderVel = [0, 0, 0]
|
|
self._leftElbowRot = [0]
|
|
self._leftElbowVel = [0]
|
|
|
|
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 NormalizeQuaternion(self, orn):
|
|
length2 = orn[0] * orn[0] + orn[1] * orn[1] + orn[2] * orn[2] + orn[3] * orn[3]
|
|
if (length2 > 0):
|
|
length = math.sqrt(length2)
|
|
#print("Normalize? length=",length)
|
|
|
|
def PostProcessMotionData(self, frameData):
|
|
baseOrn1Start = [frameData[5], frameData[6], frameData[7], frameData[4]]
|
|
self.NormalizeQuaternion(baseOrn1Start)
|
|
chestRotStart = [frameData[9], frameData[10], frameData[11], frameData[8]]
|
|
|
|
neckRotStart = [frameData[13], frameData[14], frameData[15], frameData[12]]
|
|
rightHipRotStart = [frameData[17], frameData[18], frameData[19], frameData[16]]
|
|
rightAnkleRotStart = [frameData[22], frameData[23], frameData[24], frameData[21]]
|
|
rightShoulderRotStart = [frameData[26], frameData[27], frameData[28], frameData[25]]
|
|
leftHipRotStart = [frameData[31], frameData[32], frameData[33], frameData[30]]
|
|
leftAnkleRotStart = [frameData[36], frameData[37], frameData[38], frameData[35]]
|
|
leftShoulderRotStart = [frameData[40], frameData[41], frameData[42], frameData[39]]
|
|
|
|
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)
|
|
|
|
##pre-rotate to make z-up
|
|
#y2zPos=[0,0,0.0]
|
|
#y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
|
|
#basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
|
|
|
|
chestRotStart = [frameData[9], frameData[10], frameData[11], frameData[8]]
|
|
chestRotEnd = [frameDataNext[9], frameDataNext[10], frameDataNext[11], frameDataNext[8]]
|
|
self._chestRot = bullet_client.getQuaternionSlerp(chestRotStart, chestRotEnd, frameFraction)
|
|
self._chestVel = self.ComputeAngVel(chestRotStart, chestRotEnd, keyFrameDuration,
|
|
bullet_client)
|
|
|
|
neckRotStart = [frameData[13], frameData[14], frameData[15], frameData[12]]
|
|
neckRotEnd = [frameDataNext[13], frameDataNext[14], frameDataNext[15], frameDataNext[12]]
|
|
self._neckRot = bullet_client.getQuaternionSlerp(neckRotStart, neckRotEnd, frameFraction)
|
|
self._neckVel = self.ComputeAngVel(neckRotStart, neckRotEnd, keyFrameDuration, bullet_client)
|
|
|
|
rightHipRotStart = [frameData[17], frameData[18], frameData[19], frameData[16]]
|
|
rightHipRotEnd = [frameDataNext[17], frameDataNext[18], frameDataNext[19], frameDataNext[16]]
|
|
self._rightHipRot = bullet_client.getQuaternionSlerp(rightHipRotStart, rightHipRotEnd,
|
|
frameFraction)
|
|
self._rightHipVel = self.ComputeAngVel(rightHipRotStart, rightHipRotEnd, keyFrameDuration,
|
|
bullet_client)
|
|
|
|
rightKneeRotStart = [frameData[20]]
|
|
rightKneeRotEnd = [frameDataNext[20]]
|
|
self._rightKneeRot = [
|
|
rightKneeRotStart[0] + frameFraction * (rightKneeRotEnd[0] - rightKneeRotStart[0])
|
|
]
|
|
self._rightKneeVel = [(rightKneeRotEnd[0] - rightKneeRotStart[0]) / keyFrameDuration]
|
|
|
|
rightAnkleRotStart = [frameData[22], frameData[23], frameData[24], frameData[21]]
|
|
rightAnkleRotEnd = [frameDataNext[22], frameDataNext[23], frameDataNext[24], frameDataNext[21]]
|
|
self._rightAnkleRot = bullet_client.getQuaternionSlerp(rightAnkleRotStart, rightAnkleRotEnd,
|
|
frameFraction)
|
|
self._rightAnkleVel = self.ComputeAngVel(rightAnkleRotStart, rightAnkleRotEnd,
|
|
keyFrameDuration, bullet_client)
|
|
|
|
rightShoulderRotStart = [frameData[26], frameData[27], frameData[28], frameData[25]]
|
|
rightShoulderRotEnd = [
|
|
frameDataNext[26], frameDataNext[27], frameDataNext[28], frameDataNext[25]
|
|
]
|
|
self._rightShoulderRot = bullet_client.getQuaternionSlerp(rightShoulderRotStart,
|
|
rightShoulderRotEnd, frameFraction)
|
|
self._rightShoulderVel = self.ComputeAngVel(rightShoulderRotStart, rightShoulderRotEnd,
|
|
keyFrameDuration, bullet_client)
|
|
|
|
rightElbowRotStart = [frameData[29]]
|
|
rightElbowRotEnd = [frameDataNext[29]]
|
|
self._rightElbowRot = [
|
|
rightElbowRotStart[0] + frameFraction * (rightElbowRotEnd[0] - rightElbowRotStart[0])
|
|
]
|
|
self._rightElbowVel = [(rightElbowRotEnd[0] - rightElbowRotStart[0]) / keyFrameDuration]
|
|
|
|
leftHipRotStart = [frameData[31], frameData[32], frameData[33], frameData[30]]
|
|
leftHipRotEnd = [frameDataNext[31], frameDataNext[32], frameDataNext[33], frameDataNext[30]]
|
|
self._leftHipRot = bullet_client.getQuaternionSlerp(leftHipRotStart, leftHipRotEnd,
|
|
frameFraction)
|
|
self._leftHipVel = self.ComputeAngVel(leftHipRotStart, leftHipRotEnd, keyFrameDuration,
|
|
bullet_client)
|
|
|
|
leftKneeRotStart = [frameData[34]]
|
|
leftKneeRotEnd = [frameDataNext[34]]
|
|
self._leftKneeRot = [
|
|
leftKneeRotStart[0] + frameFraction * (leftKneeRotEnd[0] - leftKneeRotStart[0])
|
|
]
|
|
self._leftKneeVel = [(leftKneeRotEnd[0] - leftKneeRotStart[0]) / keyFrameDuration]
|
|
|
|
leftAnkleRotStart = [frameData[36], frameData[37], frameData[38], frameData[35]]
|
|
leftAnkleRotEnd = [frameDataNext[36], frameDataNext[37], frameDataNext[38], frameDataNext[35]]
|
|
self._leftAnkleRot = bullet_client.getQuaternionSlerp(leftAnkleRotStart, leftAnkleRotEnd,
|
|
frameFraction)
|
|
self._leftAnkleVel = self.ComputeAngVel(leftAnkleRotStart, leftAnkleRotEnd, keyFrameDuration,
|
|
bullet_client)
|
|
|
|
leftShoulderRotStart = [frameData[40], frameData[41], frameData[42], frameData[39]]
|
|
leftShoulderRotEnd = [
|
|
frameDataNext[40], frameDataNext[41], frameDataNext[42], frameDataNext[39]
|
|
]
|
|
self._leftShoulderRot = bullet_client.getQuaternionSlerp(leftShoulderRotStart,
|
|
leftShoulderRotEnd, frameFraction)
|
|
self._leftShoulderVel = self.ComputeAngVel(leftShoulderRotStart, leftShoulderRotEnd,
|
|
keyFrameDuration, bullet_client)
|
|
|
|
leftElbowRotStart = [frameData[43]]
|
|
leftElbowRotEnd = [frameDataNext[43]]
|
|
self._leftElbowRot = [
|
|
leftElbowRotStart[0] + frameFraction * (leftElbowRotEnd[0] - leftElbowRotStart[0])
|
|
]
|
|
self._leftElbowVel = [(leftElbowRotEnd[0] - leftElbowRotStart[0]) / keyFrameDuration]
|
|
|
|
|
|
class Humanoid(object):
|
|
|
|
def __init__(self, pybullet_client, motion_data, baseShift):
|
|
"""Constructs a humanoid and reset it to the initial states.
|
|
Args:
|
|
pybullet_client: The instance of BulletClient to manage different
|
|
simulations.
|
|
"""
|
|
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
|
|
print("LOADING humanoid!")
|
|
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)
|
|
|
|
#print("human #joints=", self._pybullet_client.getNumJoints(self._humanoid))
|
|
pose = HumanoidPose()
|
|
|
|
for i in range(self._motion_data.NumFrames() - 1):
|
|
frameData = self._motion_data._motion_data['Frames'][i]
|
|
pose.PostProcessMotionData(frameData)
|
|
|
|
self._pybullet_client.resetBasePositionAndOrientation(self._humanoid, self._baseShift,
|
|
[0, 0, 0, 1])
|
|
self._pybullet_client.changeDynamics(self._humanoid, -1, linearDamping=0, angularDamping=0)
|
|
for j in range(self._pybullet_client.getNumJoints(self._humanoid)):
|
|
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])
|
|
|
|
self._initial_state = self._pybullet_client.saveState()
|
|
self._allowed_body_parts = [11, 14]
|
|
self.Reset()
|
|
|
|
def Reset(self):
|
|
self._pybullet_client.restoreState(self._initial_state)
|
|
self.SetSimTime(0)
|
|
pose = self.InitializePoseFromMotionData()
|
|
self.ApplyPose(pose, True, True, self._humanoid, self._pybullet_client)
|
|
|
|
def RenderReference(self, t):
|
|
self.SetSimTime(t)
|
|
frameData = self._motion_data._motion_data['Frames'][self._frame]
|
|
frameDataNext = self._motion_data._motion_data['Frames'][self._frameNext]
|
|
pose = HumanoidPose()
|
|
pose.Slerp(self._frameFraction, frameData, frameDataNext, self._pybullet_client)
|
|
self.ApplyPose(pose, True, True, self._humanoid, self._pybullet_client)
|
|
|
|
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 SetSimTime(self, t):
|
|
self._simTime = 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())
|
|
#print("cycleTime=",cycleTime)
|
|
cycles = self.CalcCycleCount(t, cycleTime)
|
|
#print("cycles=",cycles)
|
|
frameTime = t - cycles * cycleTime
|
|
if (frameTime < 0):
|
|
frameTime += cycleTime
|
|
|
|
#print("keyFrameDuration=",keyFrameDuration)
|
|
#print("frameTime=",frameTime)
|
|
self._frame = int(frameTime / keyFrameDuration)
|
|
#print("self._frame=",self._frame)
|
|
|
|
self._frameNext = self._frame + 1
|
|
if (self._frameNext >= self._motion_data.NumFrames()):
|
|
self._frameNext = self._frame
|
|
|
|
self._frameFraction = (frameTime - self._frame * keyFrameDuration) / (keyFrameDuration)
|
|
#print("self._frameFraction=",self._frameFraction)
|
|
|
|
def Terminates(self):
|
|
#check if any non-allowed body part hits the ground
|
|
terminates = False
|
|
pts = self._pybullet_client.getContactPoints()
|
|
for p in pts:
|
|
part = -1
|
|
if (p[1] == self._humanoid):
|
|
part = p[3]
|
|
if (p[2] == self._humanoid):
|
|
part = p[4]
|
|
if (part >= 0 and part not in self._allowed_body_parts):
|
|
terminates = True
|
|
|
|
return terminates
|
|
|
|
def BuildHeadingTrans(self, rootOrn):
|
|
#align root transform 'forward' with world-space x axis
|
|
eul = self._pybullet_client.getEulerFromQuaternion(rootOrn)
|
|
refDir = [1, 0, 0]
|
|
rotVec = self._pybullet_client.rotateVector(rootOrn, refDir)
|
|
heading = math.atan2(-rotVec[2], rotVec[0])
|
|
heading2 = eul[1]
|
|
#print("heading=",heading)
|
|
headingOrn = self._pybullet_client.getQuaternionFromAxisAngle([0, 1, 0], -heading)
|
|
return headingOrn
|
|
|
|
def GetPhase(self):
|
|
keyFrameDuration = self._motion_data.KeyFrameDuraction()
|
|
cycleTime = keyFrameDuration * (self._motion_data.NumFrames() - 1)
|
|
phase = self._simTime / cycleTime
|
|
phase = math.fmod(phase, 1.0)
|
|
if (phase < 0):
|
|
phase += 1
|
|
return phase
|
|
|
|
def BuildOriginTrans(self):
|
|
rootPos, rootOrn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid)
|
|
|
|
#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)
|
|
headingMat = self._pybullet_client.getMatrixFromQuaternion(headingOrn)
|
|
#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)
|
|
invOrigTransMat = self._pybullet_client.getMatrixFromQuaternion(invOrigTransOrn)
|
|
#print("invOrigTransMat =",invOrigTransMat )
|
|
return invOrigTransPos, invOrigTransOrn
|
|
|
|
def InitializePoseFromMotionData(self):
|
|
frameData = self._motion_data._motion_data['Frames'][self._frame]
|
|
frameDataNext = self._motion_data._motion_data['Frames'][self._frameNext]
|
|
pose = HumanoidPose()
|
|
pose.Slerp(self._frameFraction, frameData, frameDataNext, self._pybullet_client)
|
|
return pose
|
|
|
|
def ApplyAction(self, action):
|
|
#turn action into pose
|
|
pose = HumanoidPose()
|
|
pose.Reset()
|
|
index = 0
|
|
angle = action[index]
|
|
axis = [action[index + 1], action[index + 2], action[index + 3]]
|
|
index += 4
|
|
pose._chestRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle)
|
|
#print("pose._chestRot=",pose._chestRot)
|
|
|
|
angle = action[index]
|
|
axis = [action[index + 1], action[index + 2], action[index + 3]]
|
|
index += 4
|
|
pose._neckRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle)
|
|
|
|
angle = action[index]
|
|
axis = [action[index + 1], action[index + 2], action[index + 3]]
|
|
index += 4
|
|
pose._rightHipRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle)
|
|
|
|
angle = action[index]
|
|
index += 1
|
|
pose._rightKneeRot = [angle]
|
|
|
|
angle = action[index]
|
|
axis = [action[index + 1], action[index + 2], action[index + 3]]
|
|
index += 4
|
|
pose._rightAnkleRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle)
|
|
|
|
angle = action[index]
|
|
axis = [action[index + 1], action[index + 2], action[index + 3]]
|
|
index += 4
|
|
pose._rightShoulderRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle)
|
|
|
|
angle = action[index]
|
|
index += 1
|
|
pose._rightElbowRot = [angle]
|
|
|
|
angle = action[index]
|
|
axis = [action[index + 1], action[index + 2], action[index + 3]]
|
|
index += 4
|
|
pose._leftHipRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle)
|
|
|
|
angle = action[index]
|
|
index += 1
|
|
pose._leftKneeRot = [angle]
|
|
|
|
angle = action[index]
|
|
axis = [action[index + 1], action[index + 2], action[index + 3]]
|
|
index += 4
|
|
pose._leftAnkleRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle)
|
|
|
|
angle = action[index]
|
|
axis = [action[index + 1], action[index + 2], action[index + 3]]
|
|
index += 4
|
|
pose._leftShoulderRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle)
|
|
|
|
angle = action[index]
|
|
index += 1
|
|
pose._leftElbowRot = [angle]
|
|
|
|
#print("index=",index)
|
|
|
|
initializeBase = False
|
|
initializeVelocities = False
|
|
self.ApplyPose(pose, initializeBase, initializeVelocities, self._humanoid,
|
|
self._pybullet_client)
|
|
|
|
def ApplyPose(self, pose, initializeBase, initializeVelocities, humanoid, bc):
|
|
#todo: get tunable parametes from a json file or from URDF (kd, maxForce)
|
|
if (initializeBase):
|
|
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]
|
|
]
|
|
|
|
bc.resetBasePositionAndOrientation(humanoid, basePos, pose._baseOrn)
|
|
if initializeVelocities:
|
|
bc.resetBaseVelocity(humanoid, pose._baseLinVel, pose._baseAngVel)
|
|
#print("resetBaseVelocity=",pose._baseLinVel)
|
|
else:
|
|
bc.changeVisualShape(humanoid, 2, rgbaColor=[1, 1, 1, 1])
|
|
|
|
kp = 0.03
|
|
chest = 1
|
|
neck = 2
|
|
rightShoulder = 3
|
|
rightElbow = 4
|
|
leftShoulder = 6
|
|
leftElbow = 7
|
|
rightHip = 9
|
|
rightKnee = 10
|
|
rightAnkle = 11
|
|
leftHip = 12
|
|
leftKnee = 13
|
|
leftAnkle = 14
|
|
controlMode = bc.POSITION_CONTROL
|
|
|
|
if (initializeBase):
|
|
if initializeVelocities:
|
|
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:
|
|
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)
|
|
|
|
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 (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):
|
|
# bc.resetJointStateMultiDof(self._humanoidDebug,j,jsm[0])
|
|
|
|
def GetState(self):
|
|
|
|
stateVector = []
|
|
phase = self.GetPhase()
|
|
#print("phase=",phase)
|
|
stateVector.append(phase)
|
|
|
|
rootTransPos, rootTransOrn = self.BuildOriginTrans()
|
|
basePos, baseOrn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid)
|
|
|
|
rootPosRel, dummy = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn,
|
|
basePos, [0, 0, 0, 1])
|
|
#print("!!!rootPosRel =",rootPosRel )
|
|
#print("rootTransPos=",rootTransPos)
|
|
#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)
|
|
|
|
stateVector.append(rootPosRel[1])
|
|
|
|
self.pb2dmJoints = [0, 1, 2, 9, 10, 11, 3, 4, 5, 12, 13, 14, 6, 7, 8]
|
|
|
|
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]
|
|
linkPosLocal, linkOrnLocal = self._pybullet_client.multiplyTransforms(
|
|
rootTransPos, rootTransOrn, linkPos, linkOrn)
|
|
if (linkOrnLocal[3] < 0):
|
|
linkOrnLocal = [-linkOrnLocal[0], -linkOrnLocal[1], -linkOrnLocal[2], -linkOrnLocal[3]]
|
|
linkPosLocal = [
|
|
linkPosLocal[0] - rootPosRel[0], linkPosLocal[1] - rootPosRel[1],
|
|
linkPosLocal[2] - rootPosRel[2]
|
|
]
|
|
for l in linkPosLocal:
|
|
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 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]
|
|
for l in linkLinVel:
|
|
stateVector.append(l)
|
|
for l in linkAngVel:
|
|
stateVector.append(l)
|
|
|
|
#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
|
|
|
|
def GetBasePosition(self):
|
|
pos, orn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid)
|
|
return pos
|