def getBlendedNextMotion2(nextMotionA, nextMotionB, prevEndPosture, t=None, attachPosition=True, attachOrientation=True):
dA = prevEndPosture - nextMotionA[0]
dB = prevEndPosture - nextMotionB[0]
newNextMotionA = nextMotionA.copy()
newNextMotionB = nextMotionB.copy()
if attachPosition:
p_offset_A = dA.rootPos
p_offset_B = dB.rootPos
# d.disableTranslation()
newNextMotionA.translateByOffset(p_offset_A)
newNextMotionB.translateByOffset(p_offset_B)
if attachOrientation:
R_offset_A = dA.getJointOrientationLocal(0)
R_offset_A = mm.exp(mm.projectionOnVector(mm.logSO3(R_offset_A), mm.v3(0,1,0))) # # project on y axis
R_offset_B = dA.getJointOrientationLocal(0)
R_offset_B = mm.exp(mm.projectionOnVector(mm.logSO3(R_offset_B), mm.v3(0,1,0))) # # project on y axis
# d.disableRotations([0])
newNextMotionA.rotateTrajectory(R_offset_A)
newNextMotionB.rotateTrajectory(R_offset_B)
if t==None:
blendedNextMotion = blendSegmentSmooth(newNextMotionA, newNextMotionB)
else:
blendedNextMotion = blendSegmentFixed(newNextMotionA, newNextMotionB, t)
# del blendedNextMotion[0]
return blendedNextMotion
def blendSegmentFixed(motionSegment0, motionSegment1, t, attachPosition=True, attachOrientation=True):
motionSegment1 = motionSegment1.copy()
if attachPosition:
p_offset = motionSegment0[0].rootPos - motionSegment1[0].rootPos
motionSegment1.translateByOffset(p_offset)
if attachOrientation:
R_offset = np.dot(motionSegment0[0].localRs[0], motionSegment1[0].localRs[0].T)
R_offset = mm.exp(mm.projectionOnVector(mm.logSO3(R_offset), mm.v3(0,1,0))) # # project on y axis
motionSegment1.rotateTrajectory(R_offset)
# newMotion = ym.JointMotion( [None]*(int( (len(motionSegment 0)+len(motionSegment1))/2.) ) )
newMotion = ym.JointMotion( [None]*(int( (1-t)*len(motionSegment0) + t*len(motionSegment1)) ) )
# df0 = float(len(newMotion)) / len(motionSegment0)
# df1 = float(len(newMotion)) / len(motionSegment1)
for frame in range(len(newMotion)):
normalizedFrame = float(frame)/(len(newMotion)-1)
# normalizedFrame2 = yfg.H1(normalizedFrame)
# normalizedFrame2 += df0*yfg.H2(normalizedFrame)
# normalizedFrame2 += df1*yfg.H3(normalizedFrame)
# posture0_at_normalizedFrame = motionSegment0.getPostureAt(normalizedFrame2*(len(motionSegment0)-1))
# posture1_at_normalizedFrame = motionSegment1.getPostureAt(normalizedFrame2*(len(motionSegment1)-1))
# newMotion[frame] = posture0_at_normalizedFrame.blendPosture(posture1_at_normalizedFrame, normalizedFrame2)
# print normalizedFrame*(len(motionSegment0)-1)
posture0_at_normalizedFrame = motionSegment0.getPostureAt(normalizedFrame*(len(motionSegment0)-1))
posture1_at_normalizedFrame = motionSegment1.getPostureAt(normalizedFrame*(len(motionSegment1)-1))
# newMotion[frame] = posture0_at_normalizedFrame.blendPosture(posture1_at_normalizedFrame, normalizedFrame)
newMotion[frame] = posture0_at_normalizedFrame.blendPosture(posture1_at_normalizedFrame, t)
return newMotion
def blendSegmentSmooth(motionSegment0,
motionSegment1,
attachPosition=True,
attachOrientation=True):
motionSegment1 = motionSegment1.copy()
if attachPosition:
p_offset = motionSegment0[0].rootPos - motionSegment1[0].rootPos
motionSegment1.translateByOffset(p_offset)
if attachOrientation:
R_offset = np.dot(motionSegment0[0].localRs[0],
motionSegment1[0].localRs[0].T)
R_offset = mm.exp(
mm.projectionOnVector(mm.logSO3(R_offset),
mm.vec3(0, 1, 0))) # # project on y axis
motionSegment1.rotateTrajectory(R_offset)
newMotion = ym.JointMotion([None] * (int(
(len(motionSegment0) + len(motionSegment1)) / 2.)))
# newMotion = ym.JointMotion( [None]*(int( t*len(motionSegment0) + (1-t)*len(motionSegment1)) ) )
df0 = float(len(newMotion)) / len(motionSegment0)
df1 = float(len(newMotion)) / len(motionSegment1)
for frame in range(len(newMotion)):
normalizedFrame = float(frame) / (len(newMotion) - 1)
normalizedFrame2 = yfg.H1(normalizedFrame)
normalizedFrame2 += df0 * yfg.H2(normalizedFrame)
normalizedFrame2 += df1 * yfg.H3(normalizedFrame)
posture0_at_normalizedFrame = motionSegment0.getPostureAt(
normalizedFrame2 * (len(motionSegment0) - 1))
posture1_at_normalizedFrame = motionSegment1.getPostureAt(
normalizedFrame2 * (len(motionSegment1) - 1))
newMotion[frame] = posture0_at_normalizedFrame.blendPosture(
posture1_at_normalizedFrame, normalizedFrame2)
return newMotion
def getBlendedNextMotion2(nextMotionA,
nextMotionB,
prevEndPosture,
t=None,
attachPosition=True,
attachOrientation=True):
dA = prevEndPosture - nextMotionA[0]
dB = prevEndPosture - nextMotionB[0]
newNextMotionA = nextMotionA.copy()
newNextMotionB = nextMotionB.copy()
if attachPosition:
p_offset_A = dA.rootPos
p_offset_B = dB.rootPos
# d.disableTranslation()
newNextMotionA.translateByOffset(p_offset_A)
newNextMotionB.translateByOffset(p_offset_B)
if attachOrientation:
R_offset_A = dA.getJointOrientationLocal(0)
R_offset_A = mm.exp(
mm.projectionOnVector(mm.logSO3(R_offset_A),
mm.vec3(0, 1, 0))) # # project on y axis
R_offset_B = dA.getJointOrientationLocal(0)
R_offset_B = mm.exp(
mm.projectionOnVector(mm.logSO3(R_offset_B),
mm.vec3(0, 1, 0))) # # project on y axis
# d.disableRotations([0])
newNextMotionA.rotateTrajectory(R_offset_A)
newNextMotionB.rotateTrajectory(R_offset_B)
if t is None:
blendedNextMotion = blendSegmentSmooth(newNextMotionA, newNextMotionB)
else:
blendedNextMotion = blendSegmentFixed(newNextMotionA, newNextMotionB,
t)
# del blendedNextMotion[0]
return blendedNextMotion
def extraDraw():
frame = viewer.getCurrentFrame()
for footName in footNames:
# if footName == RFOOT:
if footName == LFOOT:
# if True:
footPos = motion[frame].getPosition(footName)
Rg = motion[frame].getGlobalRFromParent(footName)
# ygh.drawSO3(Rg, footPos, (0,255,0))
logRg = mm.logSO3(Rg)
logRg_sagittal = mm.projectionOnVector(logRg, sagittalAxis)
logRg_lateral = mm.projectionOnVector(logRg,
lateralAxisMap[footName])
ygh.drawVector(logRg_sagittal, footPos, (255, 255, 0))
ygh.drawVector(logRg_lateral, footPos, (0, 255, 255))
for index, contactStates in list(contactStatesMap.items()):
if contactStates[frame]:
ygh.drawPoint(motion.getPosition(index, frame),
(255, 255, 255), 5.)
sagittalAxis = horizontalRight
lateralAxisMap = {LFOOT: horizontalDirection, RFOOT: -horizontalDirection}
size_sagittals_map = {
LFOOT: [None] * len(motion),
RFOOT: [None] * len(motion)
}
size_laterals_map = {
LFOOT: [None] * len(motion),
RFOOT: [None] * len(motion)
}
for i in range(len(motion)):
for footName in footNames:
Rg = motion[i].getGlobalRFromParent(footName)
logRg = mm.logSO3(Rg)
logRg_sagittal = mm.projectionOnVector(logRg, sagittalAxis)
logRg_lateral = mm.projectionOnVector(logRg,
lateralAxisMap[footName])
size_sagittals_map[footName][i] = mm.componentOnVector(
logRg, sagittalAxis)
size_laterals_map[footName][i] = mm.componentOnVector(
logRg, lateralAxisMap[footName])
lPeakFrames, lPeakTypes = yma.getFootPeakFramesAndTypes(
size_sagittals_map[LFOOT], lFootStates, 5)
rPeakFrames, rPeakTypes = yma.getFootPeakFramesAndTypes(
size_sagittals_map[RFOOT], rFootStates, 5)
showL = True
showR = True
plot = ymp.SmartPlot()