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
jointPositions.append(jointPos)
jointVelocities.append(jointVel)
self._jointPositions = jointPositions
self._jointVelocities = jointVelocities
return jointPositions, jointVelocities
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
jointPositions.append(jointPos)
jointVelocities.append(jointVel)
self._jointPositions = jointPositions
self._jointVelocities = jointVelocities
return jointPositions, jointVelocities
def Slerp(self, frameFraction, frameData, frameDataNext, bullet_client, base_pos_offset: List[float]):
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]) + base_pos_offset[0],
basePos1Start[1] + frameFraction * (basePos1End[1] - basePos1Start[1]) + base_pos_offset[1],
basePos1Start[2] + frameFraction * (basePos1End[2] - basePos1Start[2]) + base_pos_offset[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)
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)
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)
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)
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]
pose = self.GetPose()
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)
##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)
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)
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)
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)
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]
pose = self.GetPose()
return pose