add yapf style and apply yapf to format all Python files
This recreates pull request #2192
This commit is contained in:
Erwin Coumans
@@ -1,6 +1,7 @@
|
||||
from enum import Enum
|
||||
|
||||
|
||||
class ActionSpace(Enum):
|
||||
Null = 0
|
||||
Continuous = 1
|
||||
Discrete = 2
|
||||
Null = 0
|
||||
Continuous = 1
|
||||
Discrete = 2
|
||||
|
||||
@@ -1,19 +1,21 @@
|
||||
from abc import abstractmethod
|
||||
from abc import abstractmethod
|
||||
import sys, abc
|
||||
if sys.version_info >= (3, 4):
|
||||
ABC = abc.ABC
|
||||
ABC = abc.ABC
|
||||
else:
|
||||
ABC = abc.ABCMeta('ABC', (), {})
|
||||
ABC = abc.ABCMeta('ABC', (), {})
|
||||
|
||||
import numpy as np
|
||||
from enum import Enum
|
||||
|
||||
|
||||
class Env(ABC):
|
||||
class Terminate(Enum):
|
||||
Null = 0
|
||||
Fail = 1
|
||||
Succ = 2
|
||||
|
||||
def __init__(self, args, enable_draw):
|
||||
self.enable_draw = enable_draw
|
||||
return
|
||||
|
||||
class Terminate(Enum):
|
||||
Null = 0
|
||||
Fail = 1
|
||||
Succ = 2
|
||||
|
||||
def __init__(self, args, enable_draw):
|
||||
self.enable_draw = enable_draw
|
||||
return
|
||||
|
||||
@@ -1,36 +1,61 @@
|
||||
from pybullet_utils import bullet_client
|
||||
import math
|
||||
|
||||
|
||||
|
||||
class HumanoidPoseInterpolator(object):
|
||||
|
||||
def __init__(self):
|
||||
pass
|
||||
|
||||
def Reset(self,basePos=[0,0,0], baseOrn=[0,0,0,1],chestRot=[0,0,0,1], neckRot=[0,0,0,1],rightHipRot= [0,0,0,1], rightKneeRot=[0],rightAnkleRot = [0,0,0,1],
|
||||
rightShoulderRot = [0,0,0,1],rightElbowRot = [0], leftHipRot = [0,0,0,1], leftKneeRot = [0],leftAnkleRot = [0,0,0,1],
|
||||
leftShoulderRot = [0,0,0,1] ,leftElbowRot = [0],
|
||||
baseLinVel = [0,0,0],baseAngVel = [0,0,0], chestVel = [0,0,0],neckVel = [0,0,0],rightHipVel = [0,0,0],rightKneeVel = [0],
|
||||
rightAnkleVel = [0,0,0],rightShoulderVel = [0,0,0],rightElbowVel = [0],leftHipVel = [0,0,0],leftKneeVel = [0],leftAnkleVel = [0,0,0],leftShoulderVel = [0,0,0],leftElbowVel = [0]
|
||||
):
|
||||
|
||||
|
||||
def Reset(self,
|
||||
basePos=[0, 0, 0],
|
||||
baseOrn=[0, 0, 0, 1],
|
||||
chestRot=[0, 0, 0, 1],
|
||||
neckRot=[0, 0, 0, 1],
|
||||
rightHipRot=[0, 0, 0, 1],
|
||||
rightKneeRot=[0],
|
||||
rightAnkleRot=[0, 0, 0, 1],
|
||||
rightShoulderRot=[0, 0, 0, 1],
|
||||
rightElbowRot=[0],
|
||||
leftHipRot=[0, 0, 0, 1],
|
||||
leftKneeRot=[0],
|
||||
leftAnkleRot=[0, 0, 0, 1],
|
||||
leftShoulderRot=[0, 0, 0, 1],
|
||||
leftElbowRot=[0],
|
||||
baseLinVel=[0, 0, 0],
|
||||
baseAngVel=[0, 0, 0],
|
||||
chestVel=[0, 0, 0],
|
||||
neckVel=[0, 0, 0],
|
||||
rightHipVel=[0, 0, 0],
|
||||
rightKneeVel=[0],
|
||||
rightAnkleVel=[0, 0, 0],
|
||||
rightShoulderVel=[0, 0, 0],
|
||||
rightElbowVel=[0],
|
||||
leftHipVel=[0, 0, 0],
|
||||
leftKneeVel=[0],
|
||||
leftAnkleVel=[0, 0, 0],
|
||||
leftShoulderVel=[0, 0, 0],
|
||||
leftElbowVel=[0]):
|
||||
|
||||
self._basePos = basePos
|
||||
self._baseLinVel = baseLinVel
|
||||
#print("HumanoidPoseInterpolator.Reset: baseLinVel = ", baseLinVel)
|
||||
self._baseOrn =baseOrn
|
||||
self._baseOrn = baseOrn
|
||||
self._baseAngVel = baseAngVel
|
||||
|
||||
|
||||
self._chestRot = chestRot
|
||||
self._chestVel =chestVel
|
||||
self._chestVel = chestVel
|
||||
self._neckRot = neckRot
|
||||
self._neckVel = neckVel
|
||||
|
||||
|
||||
self._rightHipRot = rightHipRot
|
||||
self._rightHipVel = rightHipVel
|
||||
self._rightKneeRot =rightKneeRot
|
||||
self._rightKneeRot = rightKneeRot
|
||||
self._rightKneeVel = rightKneeVel
|
||||
self._rightAnkleRot = rightAnkleRot
|
||||
self._rightAnkleVel = rightAnkleVel
|
||||
|
||||
self._rightShoulderRot =rightShoulderRot
|
||||
|
||||
self._rightShoulderRot = rightShoulderRot
|
||||
self._rightShoulderVel = rightShoulderVel
|
||||
self._rightElbowRot = rightElbowRot
|
||||
self._rightElbowVel = rightElbowVel
|
||||
@@ -39,225 +64,253 @@ class HumanoidPoseInterpolator(object):
|
||||
self._leftHipVel = leftHipVel
|
||||
self._leftKneeRot = leftKneeRot
|
||||
self._leftKneeVel = leftKneeVel
|
||||
self._leftAnkleRot =leftAnkleRot
|
||||
self._leftAnkleRot = leftAnkleRot
|
||||
self._leftAnkleVel = leftAnkleVel
|
||||
|
||||
|
||||
self._leftShoulderRot = leftShoulderRot
|
||||
self._leftShoulderVel = leftShoulderVel
|
||||
self._leftElbowRot =leftElbowRot
|
||||
self._leftElbowRot = leftElbowRot
|
||||
self._leftElbowVel = leftElbowVel
|
||||
|
||||
def ComputeLinVel(self,posStart, posEnd, deltaTime):
|
||||
vel = [(posEnd[0]-posStart[0])/deltaTime,(posEnd[1]-posStart[1])/deltaTime,(posEnd[2]-posStart[2])/deltaTime]
|
||||
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]
|
||||
|
||||
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]
|
||||
|
||||
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 NormalizeVector(self, vec):
|
||||
length2 = orn[0]*orn[0]+orn[1]*orn[1]+orn[2]*orn[2]
|
||||
if (length2>0):
|
||||
length2 = orn[0] * orn[0] + orn[1] * orn[1] + orn[2] * orn[2]
|
||||
if (length2 > 0):
|
||||
length = math.sqrt(length2)
|
||||
|
||||
|
||||
def NormalizeQuaternion(self, orn):
|
||||
length2 = orn[0]*orn[0]+orn[1]*orn[1]+orn[2]*orn[2]+orn[3]*orn[3]
|
||||
if (length2>0):
|
||||
length2 = orn[0] * orn[0] + orn[1] * orn[1] + orn[2] * orn[2] + orn[3] * orn[3]
|
||||
if (length2 > 0):
|
||||
length = math.sqrt(length2)
|
||||
orn[0]/=length
|
||||
orn[1]/=length
|
||||
orn[2]/=length
|
||||
orn[3]/=length
|
||||
orn[0] /= length
|
||||
orn[1] /= length
|
||||
orn[2] /= length
|
||||
orn[3] /= length
|
||||
return orn
|
||||
|
||||
|
||||
#print("Normalize? length=",length)
|
||||
|
||||
|
||||
def PostProcessMotionData(self, frameData):
|
||||
baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
|
||||
|
||||
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 GetPose(self):
|
||||
pose = [ self._basePos[0],self._basePos[1],self._basePos[2],
|
||||
self._baseOrn[0],self._baseOrn[1],self._baseOrn[2],self._baseOrn[3],
|
||||
self._chestRot[0],self._chestRot[1],self._chestRot[2],self._chestRot[3],
|
||||
self._neckRot[0],self._neckRot[1],self._neckRot[2],self._neckRot[3],
|
||||
self._rightHipRot[0],self._rightHipRot[1],self._rightHipRot[2],self._rightHipRot[3],
|
||||
self._rightKneeRot[0],
|
||||
self._rightAnkleRot[0],self._rightAnkleRot[1],self._rightAnkleRot[2],self._rightAnkleRot[3],
|
||||
self._rightShoulderRot[0],self._rightShoulderRot[1],self._rightShoulderRot[2],self._rightShoulderRot[3],
|
||||
self._rightElbowRot[0],
|
||||
self._leftHipRot[0],self._leftHipRot[1],self._leftHipRot[2],self._leftHipRot[3],
|
||||
self._leftKneeRot[0],
|
||||
self._leftAnkleRot[0],self._leftAnkleRot[1],self._leftAnkleRot[2],self._leftAnkleRot[3],
|
||||
self._leftShoulderRot[0],self._leftShoulderRot[1],self._leftShoulderRot[2],self._leftShoulderRot[3],
|
||||
self._leftElbowRot[0] ]
|
||||
return pose
|
||||
baseOrn1Start = [frameData[5], frameData[6], frameData[7], frameData[4]]
|
||||
|
||||
def Slerp(self, frameFraction, frameData, frameDataNext,bullet_client ):
|
||||
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 GetPose(self):
|
||||
pose = [
|
||||
self._basePos[0], self._basePos[1], self._basePos[2], self._baseOrn[0], self._baseOrn[1],
|
||||
self._baseOrn[2], self._baseOrn[3], self._chestRot[0], self._chestRot[1],
|
||||
self._chestRot[2], self._chestRot[3], self._neckRot[0], self._neckRot[1], self._neckRot[2],
|
||||
self._neckRot[3], self._rightHipRot[0], self._rightHipRot[1], self._rightHipRot[2],
|
||||
self._rightHipRot[3], self._rightKneeRot[0], self._rightAnkleRot[0],
|
||||
self._rightAnkleRot[1], self._rightAnkleRot[2], self._rightAnkleRot[3],
|
||||
self._rightShoulderRot[0], self._rightShoulderRot[1], self._rightShoulderRot[2],
|
||||
self._rightShoulderRot[3], self._rightElbowRot[0], self._leftHipRot[0],
|
||||
self._leftHipRot[1], self._leftHipRot[2], self._leftHipRot[3], self._leftKneeRot[0],
|
||||
self._leftAnkleRot[0], self._leftAnkleRot[1], self._leftAnkleRot[2], self._leftAnkleRot[3],
|
||||
self._leftShoulderRot[0], self._leftShoulderRot[1], self._leftShoulderRot[2],
|
||||
self._leftShoulderRot[3], self._leftElbowRot[0]
|
||||
]
|
||||
return pose
|
||||
|
||||
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)
|
||||
|
||||
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.ComputeAngVelRel(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.ComputeAngVelRel(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.ComputeAngVelRel(rightHipRotStart,rightHipRotEnd,keyFrameDuration,bullet_client)
|
||||
|
||||
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.ComputeAngVelRel(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.ComputeAngVelRel(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.ComputeAngVelRel(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.ComputeAngVelRel(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.ComputeAngVelRel(rightShoulderRotStart,rightShoulderRotEnd, keyFrameDuration,bullet_client)
|
||||
|
||||
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.ComputeAngVelRel(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.ComputeAngVelRel(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.ComputeAngVelRel(leftHipRotStart, leftHipRotEnd,keyFrameDuration,bullet_client)
|
||||
|
||||
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.ComputeAngVelRel(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.ComputeAngVelRel(leftAnkleRotStart,leftAnkleRotEnd,keyFrameDuration,bullet_client)
|
||||
self._leftKneeRot = [
|
||||
leftKneeRotStart[0] + frameFraction * (leftKneeRotEnd[0] - leftKneeRotStart[0])
|
||||
]
|
||||
self._leftKneeVel = [(leftKneeRotEnd[0] - leftKneeRotStart[0]) / keyFrameDuration]
|
||||
|
||||
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.ComputeAngVelRel(leftShoulderRotStart,leftShoulderRotEnd,keyFrameDuration,bullet_client)
|
||||
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.ComputeAngVelRel(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.ComputeAngVelRel(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]
|
||||
self._leftElbowRot = [
|
||||
leftElbowRotStart[0] + frameFraction * (leftElbowRotEnd[0] - leftElbowRotStart[0])
|
||||
]
|
||||
self._leftElbowVel = [(leftElbowRotEnd[0] - leftElbowRotStart[0]) / keyFrameDuration]
|
||||
|
||||
pose = self.GetPose()
|
||||
return pose
|
||||
|
||||
|
||||
def ConvertFromAction(self, pybullet_client, action):
|
||||
#turn action into pose
|
||||
|
||||
self.Reset()#?? needed?
|
||||
index=0
|
||||
|
||||
self.Reset() #?? needed?
|
||||
index = 0
|
||||
angle = action[index]
|
||||
axis = [action[index+1],action[index+2],action[index+3]]
|
||||
index+=4
|
||||
self._chestRot = pybullet_client.getQuaternionFromAxisAngle(axis,angle)
|
||||
axis = [action[index + 1], action[index + 2], action[index + 3]]
|
||||
index += 4
|
||||
self._chestRot = 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
|
||||
self._neckRot = pybullet_client.getQuaternionFromAxisAngle(axis,angle)
|
||||
|
||||
axis = [action[index + 1], action[index + 2], action[index + 3]]
|
||||
index += 4
|
||||
self._neckRot = pybullet_client.getQuaternionFromAxisAngle(axis, angle)
|
||||
|
||||
angle = action[index]
|
||||
axis = [action[index+1],action[index+2],action[index+3]]
|
||||
index+=4
|
||||
self._rightHipRot = pybullet_client.getQuaternionFromAxisAngle(axis,angle)
|
||||
|
||||
axis = [action[index + 1], action[index + 2], action[index + 3]]
|
||||
index += 4
|
||||
self._rightHipRot = pybullet_client.getQuaternionFromAxisAngle(axis, angle)
|
||||
|
||||
angle = action[index]
|
||||
index+=1
|
||||
index += 1
|
||||
self._rightKneeRot = [angle]
|
||||
|
||||
|
||||
angle = action[index]
|
||||
axis = [action[index+1],action[index+2],action[index+3]]
|
||||
index+=4
|
||||
self._rightAnkleRot = pybullet_client.getQuaternionFromAxisAngle(axis,angle)
|
||||
|
||||
axis = [action[index + 1], action[index + 2], action[index + 3]]
|
||||
index += 4
|
||||
self._rightAnkleRot = pybullet_client.getQuaternionFromAxisAngle(axis, angle)
|
||||
|
||||
angle = action[index]
|
||||
axis = [action[index+1],action[index+2],action[index+3]]
|
||||
index+=4
|
||||
self._rightShoulderRot = pybullet_client.getQuaternionFromAxisAngle(axis,angle)
|
||||
|
||||
axis = [action[index + 1], action[index + 2], action[index + 3]]
|
||||
index += 4
|
||||
self._rightShoulderRot = pybullet_client.getQuaternionFromAxisAngle(axis, angle)
|
||||
|
||||
angle = action[index]
|
||||
index+=1
|
||||
index += 1
|
||||
self._rightElbowRot = [angle]
|
||||
|
||||
|
||||
angle = action[index]
|
||||
axis = [action[index+1],action[index+2],action[index+3]]
|
||||
index+=4
|
||||
self._leftHipRot = pybullet_client.getQuaternionFromAxisAngle(axis,angle)
|
||||
|
||||
axis = [action[index + 1], action[index + 2], action[index + 3]]
|
||||
index += 4
|
||||
self._leftHipRot = pybullet_client.getQuaternionFromAxisAngle(axis, angle)
|
||||
|
||||
angle = action[index]
|
||||
index+=1
|
||||
index += 1
|
||||
self._leftKneeRot = [angle]
|
||||
|
||||
|
||||
|
||||
angle = action[index]
|
||||
axis = [action[index+1],action[index+2],action[index+3]]
|
||||
index+=4
|
||||
self._leftAnkleRot = pybullet_client.getQuaternionFromAxisAngle(axis,angle)
|
||||
|
||||
axis = [action[index + 1], action[index + 2], action[index + 3]]
|
||||
index += 4
|
||||
self._leftAnkleRot = pybullet_client.getQuaternionFromAxisAngle(axis, angle)
|
||||
|
||||
angle = action[index]
|
||||
axis = [action[index+1],action[index+2],action[index+3]]
|
||||
index+=4
|
||||
self._leftShoulderRot = pybullet_client.getQuaternionFromAxisAngle(axis,angle)
|
||||
|
||||
axis = [action[index + 1], action[index + 2], action[index + 3]]
|
||||
index += 4
|
||||
self._leftShoulderRot = pybullet_client.getQuaternionFromAxisAngle(axis, angle)
|
||||
|
||||
angle = action[index]
|
||||
index+=1
|
||||
index += 1
|
||||
self._leftElbowRot = [angle]
|
||||
|
||||
|
||||
pose = self.GetPose()
|
||||
return pose
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,7 +1,9 @@
|
||||
import json
|
||||
import math
|
||||
|
||||
|
||||
class MotionCaptureData(object):
|
||||
|
||||
def __init__(self):
|
||||
self.Reset()
|
||||
|
||||
@@ -13,30 +15,33 @@ class MotionCaptureData(object):
|
||||
self._motion_data = json.load(f)
|
||||
|
||||
def NumFrames(self):
|
||||
return len(self._motion_data['Frames'])
|
||||
return len(self._motion_data['Frames'])
|
||||
|
||||
def KeyFrameDuraction(self):
|
||||
return self._motion_data['Frames'][0][0]
|
||||
|
||||
return self._motion_data['Frames'][0][0]
|
||||
|
||||
def getCycleTime(self):
|
||||
keyFrameDuration = self.KeyFrameDuraction()
|
||||
cycleTime = keyFrameDuration*(self.NumFrames()-1)
|
||||
cycleTime = keyFrameDuration * (self.NumFrames() - 1)
|
||||
return cycleTime
|
||||
|
||||
|
||||
def calcCycleCount(self, simTime, cycleTime):
|
||||
phases = 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
|
||||
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
|
||||
|
||||
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
|
||||
|
||||
@@ -9,301 +9,313 @@ from pybullet_envs.deep_mimic.env import humanoid_stable_pd
|
||||
import pybullet_data
|
||||
import pybullet as p1
|
||||
import random
|
||||
|
||||
|
||||
|
||||
|
||||
class PyBulletDeepMimicEnv(Env):
|
||||
def __init__(self, arg_parser=None, enable_draw=False, pybullet_client=None):
|
||||
super().__init__(arg_parser, enable_draw)
|
||||
self._num_agents = 1
|
||||
self._pybullet_client = pybullet_client
|
||||
self._isInitialized = False
|
||||
self._useStablePD = True
|
||||
self._arg_parser = arg_parser
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
|
||||
|
||||
if not self._isInitialized:
|
||||
if self.enable_draw:
|
||||
self._pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI)
|
||||
#disable 'GUI' since it slows down a lot on Mac OSX and some other platforms
|
||||
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_GUI,0)
|
||||
else:
|
||||
self._pybullet_client = bullet_client.BulletClient()
|
||||
|
||||
self._pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
z2y = self._pybullet_client.getQuaternionFromEuler([-math.pi*0.5,0,0])
|
||||
self._planeId = self._pybullet_client.loadURDF("plane_implicit.urdf",[0,0,0],z2y, useMaximalCoordinates=True)
|
||||
#print("planeId=",self._planeId)
|
||||
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_Y_AXIS_UP,1)
|
||||
self._pybullet_client.setGravity(0,-9.8,0)
|
||||
|
||||
self._pybullet_client.setPhysicsEngineParameter(numSolverIterations=10)
|
||||
self._pybullet_client.changeDynamics(self._planeId, linkIndex=-1, lateralFriction=0.9)
|
||||
|
||||
self._mocapData = motion_capture_data.MotionCaptureData()
|
||||
|
||||
motion_file = self._arg_parser.parse_strings('motion_file')
|
||||
print("motion_file=",motion_file[0])
|
||||
|
||||
motionPath = pybullet_data.getDataPath()+"/"+motion_file[0]
|
||||
#motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
|
||||
self._mocapData.Load(motionPath)
|
||||
timeStep = 1./600.
|
||||
useFixedBase=False
|
||||
self._humanoid = humanoid_stable_pd.HumanoidStablePD(self._pybullet_client, self._mocapData, timeStep, useFixedBase)
|
||||
self._isInitialized = True
|
||||
|
||||
self._pybullet_client.setTimeStep(timeStep)
|
||||
self._pybullet_client.setPhysicsEngineParameter(numSubSteps=1)
|
||||
|
||||
|
||||
selfCheck = False
|
||||
if (selfCheck):
|
||||
curTime = 0
|
||||
while self._pybullet_client.isConnected():
|
||||
self._humanoid.setSimTime(curTime)
|
||||
state = self._humanoid.getState()
|
||||
#print("state=",state)
|
||||
pose = self._humanoid.computePose(self._humanoid._frameFraction)
|
||||
for i in range (10):
|
||||
curTime+=timeStep
|
||||
#taus = self._humanoid.computePDForces(pose)
|
||||
#self._humanoid.applyPDForces(taus)
|
||||
#self._pybullet_client.stepSimulation()
|
||||
time.sleep(timeStep)
|
||||
#print("numframes = ", self._humanoid._mocap_data.NumFrames())
|
||||
#startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2)
|
||||
rnrange = 1000
|
||||
rn = random.randint(0,rnrange)
|
||||
startTime = float(rn)/rnrange * self._humanoid.getCycleTime()
|
||||
self.t = startTime
|
||||
|
||||
self._humanoid.setSimTime(startTime)
|
||||
|
||||
self._humanoid.resetPose()
|
||||
#this clears the contact points. Todo: add API to explicitly clear all contact points?
|
||||
#self._pybullet_client.stepSimulation()
|
||||
self._humanoid.resetPose()
|
||||
self.needs_update_time = self.t-1#force update
|
||||
|
||||
def get_num_agents(self):
|
||||
return self._num_agents
|
||||
|
||||
def get_action_space(self, agent_id):
|
||||
return ActionSpace(ActionSpace.Continuous)
|
||||
|
||||
def get_reward_min(self, agent_id):
|
||||
return 0
|
||||
|
||||
def get_reward_max(self, agent_id):
|
||||
return 1
|
||||
|
||||
def get_reward_fail(self, agent_id):
|
||||
return self.get_reward_min(agent_id)
|
||||
|
||||
def get_reward_succ(self, agent_id):
|
||||
return self.get_reward_max(agent_id)
|
||||
|
||||
#scene_name == "imitate" -> cDrawSceneImitate
|
||||
def get_state_size(self, agent_id):
|
||||
#cCtController::GetStateSize()
|
||||
#int state_size = cDeepMimicCharController::GetStateSize();
|
||||
# state_size += GetStatePoseSize();#106
|
||||
# state_size += GetStateVelSize(); #(3+3)*numBodyParts=90
|
||||
#state_size += GetStatePhaseSize();#1
|
||||
#197
|
||||
return 197
|
||||
|
||||
def build_state_norm_groups(self, agent_id):
|
||||
#if (mEnablePhaseInput)
|
||||
#{
|
||||
#int phase_group = gNormGroupNone;
|
||||
#int phase_offset = GetStatePhaseOffset();
|
||||
#int phase_size = GetStatePhaseSize();
|
||||
#out_groups.segment(phase_offset, phase_size) = phase_group * Eigen::VectorXi::Ones(phase_size);
|
||||
groups = [0]*self.get_state_size(agent_id)
|
||||
groups[0] = -1
|
||||
return groups
|
||||
|
||||
def build_state_offset(self, agent_id):
|
||||
out_offset = [0]*self.get_state_size(agent_id)
|
||||
phase_offset = -0.5
|
||||
out_offset[0] = phase_offset
|
||||
return np.array(out_offset)
|
||||
|
||||
def build_state_scale(self, agent_id):
|
||||
out_scale = [1]*self.get_state_size(agent_id)
|
||||
phase_scale = 2
|
||||
out_scale[0] = phase_scale
|
||||
return np.array(out_scale)
|
||||
def __init__(self, arg_parser=None, enable_draw=False, pybullet_client=None):
|
||||
super().__init__(arg_parser, enable_draw)
|
||||
self._num_agents = 1
|
||||
self._pybullet_client = pybullet_client
|
||||
self._isInitialized = False
|
||||
self._useStablePD = True
|
||||
self._arg_parser = arg_parser
|
||||
self.reset()
|
||||
|
||||
def get_goal_size(self, agent_id):
|
||||
return 0
|
||||
def reset(self):
|
||||
|
||||
def get_action_size(self, agent_id):
|
||||
ctrl_size = 43 #numDof
|
||||
root_size = 7
|
||||
return ctrl_size - root_size
|
||||
|
||||
def build_goal_norm_groups(self, agent_id):
|
||||
return np.array([])
|
||||
|
||||
def build_goal_offset(self, agent_id):
|
||||
return np.array([])
|
||||
|
||||
def build_goal_scale(self, agent_id):
|
||||
return np.array([])
|
||||
|
||||
def build_action_offset(self, agent_id):
|
||||
out_offset = [0] * self.get_action_size(agent_id)
|
||||
out_offset = [0.0000000000,0.0000000000,0.0000000000,-0.200000000,0.0000000000,0.0000000000,0.0000000000,
|
||||
-0.200000000,0.0000000000,0.0000000000, 0.00000000, -0.2000000, 1.57000000, 0.00000000, 0.00000000,
|
||||
0.00000000, -0.2000000, 0.00000000, 0.00000000, 0.00000000, -0.2000000, -1.5700000, 0.00000000, 0.00000000,
|
||||
0.00000000, -0.2000000, 1.57000000, 0.00000000, 0.00000000, 0.00000000, -0.2000000, 0.00000000, 0.00000000,
|
||||
0.00000000, -0.2000000, -1.5700000]
|
||||
#see cCtCtrlUtil::BuildOffsetScalePDPrismatic and
|
||||
#see cCtCtrlUtil::BuildOffsetScalePDSpherical
|
||||
return np.array(out_offset)
|
||||
|
||||
def build_action_scale(self, agent_id):
|
||||
out_scale = [1] * self.get_action_size(agent_id)
|
||||
#see cCtCtrlUtil::BuildOffsetScalePDPrismatic and
|
||||
#see cCtCtrlUtil::BuildOffsetScalePDSpherical
|
||||
out_scale=[ 0.20833333333333,1.00000000000000,1.00000000000000,1.00000000000000,0.25000000000000,
|
||||
1.00000000000000,1.00000000000000,1.00000000000000,0.12077294685990,1.00000000000000,
|
||||
1.000000000000, 1.000000000000, 0.159235668789, 0.159235668789, 1.000000000000,
|
||||
1.000000000000, 1.000000000000, 0.079617834394, 1.000000000000, 1.000000000000,
|
||||
1.000000000000, 0.159235668789, 0.120772946859, 1.000000000000, 1.000000000000,
|
||||
1.000000000000, 0.159235668789, 0.159235668789, 1.000000000000, 1.000000000000,
|
||||
1.000000000000, 0.107758620689, 1.000000000000, 1.000000000000, 1.000000000000,
|
||||
0.159235668789]
|
||||
return np.array(out_scale)
|
||||
|
||||
def build_action_bound_min(self, agent_id):
|
||||
#see cCtCtrlUtil::BuildBoundsPDSpherical
|
||||
out_scale = [-1] * self.get_action_size(agent_id)
|
||||
out_scale = [-4.79999999999,-1.00000000000,-1.00000000000,-1.00000000000,-4.00000000000,
|
||||
-1.00000000000,-1.00000000000,-1.00000000000,-7.77999999999,-1.00000000000, -1.000000000,
|
||||
-1.000000000, -7.850000000, -6.280000000, -1.000000000, -1.000000000, -1.000000000,
|
||||
-12.56000000, -1.000000000, -1.000000000, -1.000000000, -4.710000000,
|
||||
-7.779999999, -1.000000000, -1.000000000, -1.000000000, -7.850000000,
|
||||
-6.280000000, -1.000000000, -1.000000000, -1.000000000, -8.460000000,
|
||||
-1.000000000, -1.000000000, -1.000000000, -4.710000000]
|
||||
if not self._isInitialized:
|
||||
if self.enable_draw:
|
||||
self._pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI)
|
||||
#disable 'GUI' since it slows down a lot on Mac OSX and some other platforms
|
||||
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_GUI, 0)
|
||||
else:
|
||||
self._pybullet_client = bullet_client.BulletClient()
|
||||
|
||||
self._pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
z2y = self._pybullet_client.getQuaternionFromEuler([-math.pi * 0.5, 0, 0])
|
||||
self._planeId = self._pybullet_client.loadURDF("plane_implicit.urdf", [0, 0, 0],
|
||||
z2y,
|
||||
useMaximalCoordinates=True)
|
||||
#print("planeId=",self._planeId)
|
||||
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_Y_AXIS_UP, 1)
|
||||
self._pybullet_client.setGravity(0, -9.8, 0)
|
||||
|
||||
self._pybullet_client.setPhysicsEngineParameter(numSolverIterations=10)
|
||||
self._pybullet_client.changeDynamics(self._planeId, linkIndex=-1, lateralFriction=0.9)
|
||||
|
||||
self._mocapData = motion_capture_data.MotionCaptureData()
|
||||
|
||||
motion_file = self._arg_parser.parse_strings('motion_file')
|
||||
print("motion_file=", motion_file[0])
|
||||
|
||||
motionPath = pybullet_data.getDataPath() + "/" + motion_file[0]
|
||||
#motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
|
||||
self._mocapData.Load(motionPath)
|
||||
timeStep = 1. / 600.
|
||||
useFixedBase = False
|
||||
self._humanoid = humanoid_stable_pd.HumanoidStablePD(self._pybullet_client, self._mocapData,
|
||||
timeStep, useFixedBase)
|
||||
self._isInitialized = True
|
||||
|
||||
return out_scale
|
||||
|
||||
def build_action_bound_max(self, agent_id):
|
||||
out_scale = [1] * self.get_action_size(agent_id)
|
||||
out_scale=[
|
||||
4.799999999,1.000000000,1.000000000,1.000000000,4.000000000,1.000000000,
|
||||
1.000000000,1.000000000,8.779999999,1.000000000, 1.0000000, 1.0000000,
|
||||
4.7100000, 6.2800000, 1.0000000, 1.0000000, 1.0000000,
|
||||
12.560000, 1.0000000, 1.0000000, 1.0000000, 7.8500000,
|
||||
8.7799999, 1.0000000, 1.0000000, 1.0000000, 4.7100000,
|
||||
6.2800000, 1.0000000, 1.0000000, 1.0000000, 10.100000,
|
||||
1.0000000, 1.0000000, 1.0000000, 7.8500000]
|
||||
return out_scale
|
||||
|
||||
def set_mode(self, mode):
|
||||
self._mode = mode
|
||||
|
||||
def need_new_action(self, agent_id):
|
||||
if self.t>=self.needs_update_time:
|
||||
self.needs_update_time = self.t + 1./30.
|
||||
return True
|
||||
return False
|
||||
|
||||
def record_state(self, agent_id):
|
||||
state = self._humanoid.getState()
|
||||
|
||||
return np.array(state)
|
||||
|
||||
|
||||
def record_goal(self, agent_id):
|
||||
return np.array([])
|
||||
|
||||
def calc_reward(self, agent_id):
|
||||
kinPose = self._humanoid.computePose(self._humanoid._frameFraction)
|
||||
reward = self._humanoid.getReward(kinPose)
|
||||
return reward
|
||||
|
||||
def set_action(self, agent_id, action):
|
||||
#print("action=",)
|
||||
#for a in action:
|
||||
# print(a)
|
||||
np.savetxt("pb_action.csv", action, delimiter=",")
|
||||
self.desiredPose = self._humanoid.convertActionToPose(action)
|
||||
#we need the target root positon and orientation to be zero, to be compatible with deep mimic
|
||||
self.desiredPose[0] = 0
|
||||
self.desiredPose[1] = 0
|
||||
self.desiredPose[2] = 0
|
||||
self.desiredPose[3] = 0
|
||||
self.desiredPose[4] = 0
|
||||
self.desiredPose[5] = 0
|
||||
self.desiredPose[6] = 0
|
||||
target_pose = np.array(self.desiredPose)
|
||||
|
||||
|
||||
np.savetxt("pb_target_pose.csv", target_pose, delimiter=",")
|
||||
|
||||
#print("set_action: desiredPose=", self.desiredPose)
|
||||
|
||||
def log_val(self, agent_id, val):
|
||||
pass
|
||||
|
||||
|
||||
def update(self, timeStep):
|
||||
#print("pybullet_deep_mimic_env:update timeStep=",timeStep," t=",self.t)
|
||||
self._pybullet_client.setTimeStep(timeStep)
|
||||
self._humanoid._timeStep = timeStep
|
||||
|
||||
for i in range(1):
|
||||
self.t += timeStep
|
||||
self._humanoid.setSimTime(self.t)
|
||||
|
||||
if self.desiredPose:
|
||||
kinPose = self._humanoid.computePose(self._humanoid._frameFraction)
|
||||
self._humanoid.initializePose(self._humanoid._poseInterpolator, self._humanoid._kin_model, initBase=True)
|
||||
#pos,orn=self._pybullet_client.getBasePositionAndOrientation(self._humanoid._sim_model)
|
||||
#self._pybullet_client.resetBasePositionAndOrientation(self._humanoid._kin_model, [pos[0]+3,pos[1],pos[2]],orn)
|
||||
#print("desiredPositions=",self.desiredPose)
|
||||
maxForces = [0,0,0,0,0,0,0,200,200,200,200, 50,50,50,50, 200,200,200,200, 150, 90,90,90,90, 100,100,100,100, 60, 200,200,200,200, 150, 90, 90, 90, 90, 100,100,100,100, 60]
|
||||
|
||||
if self._useStablePD:
|
||||
taus = self._humanoid.computePDForces(self.desiredPose, desiredVelocities=None, maxForces=maxForces)
|
||||
self._humanoid.applyPDForces(taus)
|
||||
else:
|
||||
self._humanoid.setJointMotors(self.desiredPose, maxForces=maxForces)
|
||||
|
||||
self._pybullet_client.stepSimulation()
|
||||
|
||||
self._pybullet_client.setPhysicsEngineParameter(numSubSteps=1)
|
||||
|
||||
def set_sample_count(self, count):
|
||||
return
|
||||
|
||||
def check_terminate(self, agent_id):
|
||||
return Env.Terminate(self.is_episode_end())
|
||||
|
||||
def is_episode_end(self):
|
||||
isEnded = self._humanoid.terminates()
|
||||
#also check maximum time, 20 seconds (todo get from file)
|
||||
#print("self.t=",self.t)
|
||||
if (self.t>20):
|
||||
isEnded = True
|
||||
return isEnded
|
||||
|
||||
def check_valid_episode(self):
|
||||
#could check if limbs exceed velocity threshold
|
||||
return true
|
||||
|
||||
def getKeyboardEvents(self):
|
||||
return self._pybullet_client.getKeyboardEvents()
|
||||
|
||||
def isKeyTriggered(self, keys, key):
|
||||
o = ord(key)
|
||||
#print("ord=",o)
|
||||
if o in keys:
|
||||
return keys[ord(key)] & self._pybullet_client.KEY_WAS_TRIGGERED
|
||||
return False
|
||||
selfCheck = False
|
||||
if (selfCheck):
|
||||
curTime = 0
|
||||
while self._pybullet_client.isConnected():
|
||||
self._humanoid.setSimTime(curTime)
|
||||
state = self._humanoid.getState()
|
||||
#print("state=",state)
|
||||
pose = self._humanoid.computePose(self._humanoid._frameFraction)
|
||||
for i in range(10):
|
||||
curTime += timeStep
|
||||
#taus = self._humanoid.computePDForces(pose)
|
||||
#self._humanoid.applyPDForces(taus)
|
||||
#self._pybullet_client.stepSimulation()
|
||||
time.sleep(timeStep)
|
||||
#print("numframes = ", self._humanoid._mocap_data.NumFrames())
|
||||
#startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2)
|
||||
rnrange = 1000
|
||||
rn = random.randint(0, rnrange)
|
||||
startTime = float(rn) / rnrange * self._humanoid.getCycleTime()
|
||||
self.t = startTime
|
||||
|
||||
self._humanoid.setSimTime(startTime)
|
||||
|
||||
self._humanoid.resetPose()
|
||||
#this clears the contact points. Todo: add API to explicitly clear all contact points?
|
||||
#self._pybullet_client.stepSimulation()
|
||||
self._humanoid.resetPose()
|
||||
self.needs_update_time = self.t - 1 #force update
|
||||
|
||||
def get_num_agents(self):
|
||||
return self._num_agents
|
||||
|
||||
def get_action_space(self, agent_id):
|
||||
return ActionSpace(ActionSpace.Continuous)
|
||||
|
||||
def get_reward_min(self, agent_id):
|
||||
return 0
|
||||
|
||||
def get_reward_max(self, agent_id):
|
||||
return 1
|
||||
|
||||
def get_reward_fail(self, agent_id):
|
||||
return self.get_reward_min(agent_id)
|
||||
|
||||
def get_reward_succ(self, agent_id):
|
||||
return self.get_reward_max(agent_id)
|
||||
|
||||
#scene_name == "imitate" -> cDrawSceneImitate
|
||||
def get_state_size(self, agent_id):
|
||||
#cCtController::GetStateSize()
|
||||
#int state_size = cDeepMimicCharController::GetStateSize();
|
||||
# state_size += GetStatePoseSize();#106
|
||||
# state_size += GetStateVelSize(); #(3+3)*numBodyParts=90
|
||||
#state_size += GetStatePhaseSize();#1
|
||||
#197
|
||||
return 197
|
||||
|
||||
def build_state_norm_groups(self, agent_id):
|
||||
#if (mEnablePhaseInput)
|
||||
#{
|
||||
#int phase_group = gNormGroupNone;
|
||||
#int phase_offset = GetStatePhaseOffset();
|
||||
#int phase_size = GetStatePhaseSize();
|
||||
#out_groups.segment(phase_offset, phase_size) = phase_group * Eigen::VectorXi::Ones(phase_size);
|
||||
groups = [0] * self.get_state_size(agent_id)
|
||||
groups[0] = -1
|
||||
return groups
|
||||
|
||||
def build_state_offset(self, agent_id):
|
||||
out_offset = [0] * self.get_state_size(agent_id)
|
||||
phase_offset = -0.5
|
||||
out_offset[0] = phase_offset
|
||||
return np.array(out_offset)
|
||||
|
||||
def build_state_scale(self, agent_id):
|
||||
out_scale = [1] * self.get_state_size(agent_id)
|
||||
phase_scale = 2
|
||||
out_scale[0] = phase_scale
|
||||
return np.array(out_scale)
|
||||
|
||||
def get_goal_size(self, agent_id):
|
||||
return 0
|
||||
|
||||
def get_action_size(self, agent_id):
|
||||
ctrl_size = 43 #numDof
|
||||
root_size = 7
|
||||
return ctrl_size - root_size
|
||||
|
||||
def build_goal_norm_groups(self, agent_id):
|
||||
return np.array([])
|
||||
|
||||
def build_goal_offset(self, agent_id):
|
||||
return np.array([])
|
||||
|
||||
def build_goal_scale(self, agent_id):
|
||||
return np.array([])
|
||||
|
||||
def build_action_offset(self, agent_id):
|
||||
out_offset = [0] * self.get_action_size(agent_id)
|
||||
out_offset = [
|
||||
0.0000000000, 0.0000000000, 0.0000000000, -0.200000000, 0.0000000000, 0.0000000000,
|
||||
0.0000000000, -0.200000000, 0.0000000000, 0.0000000000, 0.00000000, -0.2000000, 1.57000000,
|
||||
0.00000000, 0.00000000, 0.00000000, -0.2000000, 0.00000000, 0.00000000, 0.00000000,
|
||||
-0.2000000, -1.5700000, 0.00000000, 0.00000000, 0.00000000, -0.2000000, 1.57000000,
|
||||
0.00000000, 0.00000000, 0.00000000, -0.2000000, 0.00000000, 0.00000000, 0.00000000,
|
||||
-0.2000000, -1.5700000
|
||||
]
|
||||
#see cCtCtrlUtil::BuildOffsetScalePDPrismatic and
|
||||
#see cCtCtrlUtil::BuildOffsetScalePDSpherical
|
||||
return np.array(out_offset)
|
||||
|
||||
def build_action_scale(self, agent_id):
|
||||
out_scale = [1] * self.get_action_size(agent_id)
|
||||
#see cCtCtrlUtil::BuildOffsetScalePDPrismatic and
|
||||
#see cCtCtrlUtil::BuildOffsetScalePDSpherical
|
||||
out_scale = [
|
||||
0.20833333333333, 1.00000000000000, 1.00000000000000, 1.00000000000000, 0.25000000000000,
|
||||
1.00000000000000, 1.00000000000000, 1.00000000000000, 0.12077294685990, 1.00000000000000,
|
||||
1.000000000000, 1.000000000000, 0.159235668789, 0.159235668789, 1.000000000000,
|
||||
1.000000000000, 1.000000000000, 0.079617834394, 1.000000000000, 1.000000000000,
|
||||
1.000000000000, 0.159235668789, 0.120772946859, 1.000000000000, 1.000000000000,
|
||||
1.000000000000, 0.159235668789, 0.159235668789, 1.000000000000, 1.000000000000,
|
||||
1.000000000000, 0.107758620689, 1.000000000000, 1.000000000000, 1.000000000000,
|
||||
0.159235668789
|
||||
]
|
||||
return np.array(out_scale)
|
||||
|
||||
def build_action_bound_min(self, agent_id):
|
||||
#see cCtCtrlUtil::BuildBoundsPDSpherical
|
||||
out_scale = [-1] * self.get_action_size(agent_id)
|
||||
out_scale = [
|
||||
-4.79999999999, -1.00000000000, -1.00000000000, -1.00000000000, -4.00000000000,
|
||||
-1.00000000000, -1.00000000000, -1.00000000000, -7.77999999999, -1.00000000000,
|
||||
-1.000000000, -1.000000000, -7.850000000, -6.280000000, -1.000000000, -1.000000000,
|
||||
-1.000000000, -12.56000000, -1.000000000, -1.000000000, -1.000000000, -4.710000000,
|
||||
-7.779999999, -1.000000000, -1.000000000, -1.000000000, -7.850000000, -6.280000000,
|
||||
-1.000000000, -1.000000000, -1.000000000, -8.460000000, -1.000000000, -1.000000000,
|
||||
-1.000000000, -4.710000000
|
||||
]
|
||||
|
||||
return out_scale
|
||||
|
||||
def build_action_bound_max(self, agent_id):
|
||||
out_scale = [1] * self.get_action_size(agent_id)
|
||||
out_scale = [
|
||||
4.799999999, 1.000000000, 1.000000000, 1.000000000, 4.000000000, 1.000000000, 1.000000000,
|
||||
1.000000000, 8.779999999, 1.000000000, 1.0000000, 1.0000000, 4.7100000, 6.2800000,
|
||||
1.0000000, 1.0000000, 1.0000000, 12.560000, 1.0000000, 1.0000000, 1.0000000, 7.8500000,
|
||||
8.7799999, 1.0000000, 1.0000000, 1.0000000, 4.7100000, 6.2800000, 1.0000000, 1.0000000,
|
||||
1.0000000, 10.100000, 1.0000000, 1.0000000, 1.0000000, 7.8500000
|
||||
]
|
||||
return out_scale
|
||||
|
||||
def set_mode(self, mode):
|
||||
self._mode = mode
|
||||
|
||||
def need_new_action(self, agent_id):
|
||||
if self.t >= self.needs_update_time:
|
||||
self.needs_update_time = self.t + 1. / 30.
|
||||
return True
|
||||
return False
|
||||
|
||||
def record_state(self, agent_id):
|
||||
state = self._humanoid.getState()
|
||||
|
||||
return np.array(state)
|
||||
|
||||
def record_goal(self, agent_id):
|
||||
return np.array([])
|
||||
|
||||
def calc_reward(self, agent_id):
|
||||
kinPose = self._humanoid.computePose(self._humanoid._frameFraction)
|
||||
reward = self._humanoid.getReward(kinPose)
|
||||
return reward
|
||||
|
||||
def set_action(self, agent_id, action):
|
||||
#print("action=",)
|
||||
#for a in action:
|
||||
# print(a)
|
||||
np.savetxt("pb_action.csv", action, delimiter=",")
|
||||
self.desiredPose = self._humanoid.convertActionToPose(action)
|
||||
#we need the target root positon and orientation to be zero, to be compatible with deep mimic
|
||||
self.desiredPose[0] = 0
|
||||
self.desiredPose[1] = 0
|
||||
self.desiredPose[2] = 0
|
||||
self.desiredPose[3] = 0
|
||||
self.desiredPose[4] = 0
|
||||
self.desiredPose[5] = 0
|
||||
self.desiredPose[6] = 0
|
||||
target_pose = np.array(self.desiredPose)
|
||||
|
||||
np.savetxt("pb_target_pose.csv", target_pose, delimiter=",")
|
||||
|
||||
#print("set_action: desiredPose=", self.desiredPose)
|
||||
|
||||
def log_val(self, agent_id, val):
|
||||
pass
|
||||
|
||||
def update(self, timeStep):
|
||||
#print("pybullet_deep_mimic_env:update timeStep=",timeStep," t=",self.t)
|
||||
self._pybullet_client.setTimeStep(timeStep)
|
||||
self._humanoid._timeStep = timeStep
|
||||
|
||||
for i in range(1):
|
||||
self.t += timeStep
|
||||
self._humanoid.setSimTime(self.t)
|
||||
|
||||
if self.desiredPose:
|
||||
kinPose = self._humanoid.computePose(self._humanoid._frameFraction)
|
||||
self._humanoid.initializePose(self._humanoid._poseInterpolator,
|
||||
self._humanoid._kin_model,
|
||||
initBase=True)
|
||||
#pos,orn=self._pybullet_client.getBasePositionAndOrientation(self._humanoid._sim_model)
|
||||
#self._pybullet_client.resetBasePositionAndOrientation(self._humanoid._kin_model, [pos[0]+3,pos[1],pos[2]],orn)
|
||||
#print("desiredPositions=",self.desiredPose)
|
||||
maxForces = [
|
||||
0, 0, 0, 0, 0, 0, 0, 200, 200, 200, 200, 50, 50, 50, 50, 200, 200, 200, 200, 150, 90,
|
||||
90, 90, 90, 100, 100, 100, 100, 60, 200, 200, 200, 200, 150, 90, 90, 90, 90, 100, 100,
|
||||
100, 100, 60
|
||||
]
|
||||
|
||||
if self._useStablePD:
|
||||
taus = self._humanoid.computePDForces(self.desiredPose,
|
||||
desiredVelocities=None,
|
||||
maxForces=maxForces)
|
||||
self._humanoid.applyPDForces(taus)
|
||||
else:
|
||||
self._humanoid.setJointMotors(self.desiredPose, maxForces=maxForces)
|
||||
|
||||
self._pybullet_client.stepSimulation()
|
||||
|
||||
def set_sample_count(self, count):
|
||||
return
|
||||
|
||||
def check_terminate(self, agent_id):
|
||||
return Env.Terminate(self.is_episode_end())
|
||||
|
||||
def is_episode_end(self):
|
||||
isEnded = self._humanoid.terminates()
|
||||
#also check maximum time, 20 seconds (todo get from file)
|
||||
#print("self.t=",self.t)
|
||||
if (self.t > 20):
|
||||
isEnded = True
|
||||
return isEnded
|
||||
|
||||
def check_valid_episode(self):
|
||||
#could check if limbs exceed velocity threshold
|
||||
return true
|
||||
|
||||
def getKeyboardEvents(self):
|
||||
return self._pybullet_client.getKeyboardEvents()
|
||||
|
||||
def isKeyTriggered(self, keys, key):
|
||||
o = ord(key)
|
||||
#print("ord=",o)
|
||||
if o in keys:
|
||||
return keys[ord(key)] & self._pybullet_client.KEY_WAS_TRIGGERED
|
||||
return False
|
||||
|
||||
@@ -1,54 +1,66 @@
|
||||
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]
|
||||
|
||||
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]
|
||||
|
||||
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]
|
||||
|
||||
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 ):
|
||||
|
||||
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=[self._basePos[0],self._basePos[1],self._basePos[2],
|
||||
self._baseOrn[0],self._baseOrn[1],self._baseOrn[2],self._baseOrn[3]]
|
||||
jointVelocities=[self._baseLinVel[0],self._baseLinVel[1],self._baseLinVel[2],
|
||||
self._baseAngVel[0],self._baseAngVel[1],self._baseAngVel[2]]
|
||||
|
||||
for j in range (12):
|
||||
index=j+8
|
||||
jointPosStart=frameData[index]
|
||||
jointPosEnd=frameDataNext[index]
|
||||
jointPos=jointPosStart+frameFraction*(jointPosEnd-jointPosStart)
|
||||
jointVel=(jointPosEnd-jointPosStart)/keyFrameDuration
|
||||
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 = [
|
||||
self._basePos[0], self._basePos[1], self._basePos[2], self._baseOrn[0], self._baseOrn[1],
|
||||
self._baseOrn[2], self._baseOrn[3]
|
||||
]
|
||||
jointVelocities = [
|
||||
self._baseLinVel[0], self._baseLinVel[1], self._baseLinVel[2], self._baseAngVel[0],
|
||||
self._baseAngVel[1], self._baseAngVel[2]
|
||||
]
|
||||
|
||||
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
|
||||
return jointPositions, jointVelocities
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -8,87 +8,89 @@ import pybullet as p1
|
||||
import humanoid_pose_interpolator
|
||||
import numpy as np
|
||||
|
||||
pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI)
|
||||
pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI)
|
||||
|
||||
pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
z2y = pybullet_client.getQuaternionFromEuler([-math.pi*0.5,0,0])
|
||||
z2y = pybullet_client.getQuaternionFromEuler([-math.pi * 0.5, 0, 0])
|
||||
#planeId = pybullet_client.loadURDF("plane.urdf",[0,0,0],z2y)
|
||||
planeId= pybullet_client.loadURDF("plane_implicit.urdf",[0,0,0],z2y, useMaximalCoordinates=True)
|
||||
pybullet_client.changeDynamics(planeId, linkIndex=-1, lateralFriction=0.9)
|
||||
planeId = pybullet_client.loadURDF("plane_implicit.urdf", [0, 0, 0],
|
||||
z2y,
|
||||
useMaximalCoordinates=True)
|
||||
pybullet_client.changeDynamics(planeId, linkIndex=-1, lateralFriction=0.9)
|
||||
#print("planeId=",planeId)
|
||||
|
||||
pybullet_client.configureDebugVisualizer(pybullet_client.COV_ENABLE_Y_AXIS_UP,1)
|
||||
pybullet_client.setGravity(0,-9.8,0)
|
||||
pybullet_client.configureDebugVisualizer(pybullet_client.COV_ENABLE_Y_AXIS_UP, 1)
|
||||
pybullet_client.setGravity(0, -9.8, 0)
|
||||
|
||||
pybullet_client.setPhysicsEngineParameter(numSolverIterations=10)
|
||||
|
||||
|
||||
mocapData = motion_capture_data.MotionCaptureData()
|
||||
#motionPath = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt"
|
||||
motionPath = pybullet_data.getDataPath()+"/data/motions/humanoid3d_backflip.txt"
|
||||
motionPath = pybullet_data.getDataPath() + "/data/motions/humanoid3d_backflip.txt"
|
||||
mocapData.Load(motionPath)
|
||||
timeStep = 1./600
|
||||
useFixedBase=False
|
||||
timeStep = 1. / 600
|
||||
useFixedBase = False
|
||||
humanoid = humanoid_stable_pd.HumanoidStablePD(pybullet_client, mocapData, timeStep, useFixedBase)
|
||||
isInitialized = True
|
||||
|
||||
pybullet_client.setTimeStep(timeStep)
|
||||
pybullet_client.setPhysicsEngineParameter(numSubSteps=2)
|
||||
timeId = pybullet_client.addUserDebugParameter("time",0,10,0)
|
||||
timeId = pybullet_client.addUserDebugParameter("time", 0, 10, 0)
|
||||
|
||||
|
||||
|
||||
def isKeyTriggered(keys, key):
|
||||
o = ord(key)
|
||||
if o in keys:
|
||||
return keys[ord(key)] & pybullet_client.KEY_WAS_TRIGGERED
|
||||
return False
|
||||
|
||||
|
||||
|
||||
animating = False
|
||||
singleStep = False
|
||||
|
||||
|
||||
t=0
|
||||
t = 0
|
||||
while (1):
|
||||
|
||||
keys = pybullet_client.getKeyboardEvents()
|
||||
#print(keys)
|
||||
if isKeyTriggered(keys, ' '):
|
||||
animating = not animating
|
||||
animating = not animating
|
||||
|
||||
if isKeyTriggered(keys, 'b'):
|
||||
singleStep = True
|
||||
|
||||
singleStep = True
|
||||
|
||||
if animating or singleStep:
|
||||
|
||||
|
||||
|
||||
singleStep = False
|
||||
#t = pybullet_client.readUserDebugParameter(timeId)
|
||||
#print("t=",t)
|
||||
for i in range (1):
|
||||
for i in range(1):
|
||||
|
||||
print("t=",t)
|
||||
print("t=", t)
|
||||
humanoid.setSimTime(t)
|
||||
|
||||
|
||||
humanoid.computePose(humanoid._frameFraction)
|
||||
pose = humanoid._poseInterpolator
|
||||
#humanoid.initializePose(pose=pose, phys_model = humanoid._sim_model, initBase=True, initializeVelocity=True)
|
||||
#humanoid.resetPose()
|
||||
|
||||
|
||||
|
||||
desiredPose = humanoid.computePose(humanoid._frameFraction)
|
||||
#desiredPose = desiredPose.GetPose()
|
||||
#desiredPose = desiredPose.GetPose()
|
||||
#curPose = HumanoidPoseInterpolator()
|
||||
#curPose.reset()
|
||||
s = humanoid.getState()
|
||||
#np.savetxt("pb_record_state_s.csv", s, delimiter=",")
|
||||
maxForces = [0,0,0,0,0,0,0,200,200,200,200, 50,50,50,50, 200,200,200,200, 150, 90,90,90,90, 100,100,100,100, 60, 200,200,200,200, 150, 90, 90, 90, 90, 100,100,100,100, 60]
|
||||
maxForces = [
|
||||
0, 0, 0, 0, 0, 0, 0, 200, 200, 200, 200, 50, 50, 50, 50, 200, 200, 200, 200, 150, 90, 90,
|
||||
90, 90, 100, 100, 100, 100, 60, 200, 200, 200, 200, 150, 90, 90, 90, 90, 100, 100, 100,
|
||||
100, 60
|
||||
]
|
||||
taus = humanoid.computePDForces(desiredPose, desiredVelocities=None, maxForces=maxForces)
|
||||
|
||||
|
||||
#print("taus=",taus)
|
||||
humanoid.applyPDForces(taus)
|
||||
|
||||
|
||||
pybullet_client.stepSimulation()
|
||||
t+=1./600.
|
||||
|
||||
|
||||
time.sleep(1./600.)
|
||||
t += 1. / 600.
|
||||
|
||||
time.sleep(1. / 600.)
|
||||
|
||||
@@ -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)
|
||||
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])
|
||||
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)
|
||||
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]
|
||||
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)
|
||||
for i in range(4):
|
||||
jointOffsets.append(0)
|
||||
jointOffsets.append(-0.7)
|
||||
jointOffsets.append(0.7)
|
||||
|
||||
maxForceId = p.addUserDebugParameter("maxForce",0,100,20)
|
||||
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)
|
||||
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.getCameraImage(480, 320)
|
||||
p.setRealTimeSimulation(0)
|
||||
|
||||
joints=[]
|
||||
|
||||
joints = []
|
||||
|
||||
mocapData = motion_capture_data.MotionCaptureData()
|
||||
|
||||
motionPath = pybullet_data.getDataPath()+"/data/motions/laikago_walk.txt"
|
||||
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)
|
||||
|
||||
Reference in New Issue
Block a user