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 PDControl3(frame):
# Kp = 100.*70;
# Kd = 10.*3;
Kp = 1000.
Kd = 10.
desiredOri1 = mm.exp(axis1, deg1[0] * M_RADIAN)
desiredOriX = mm.exp(axisX, deg1[0] * M_RADIAN)
desiredOriY = mm.exp(axisY, deg1[0] * M_RADIAN)
desiredOriZ = mm.exp(axisZ, deg1[0] * M_RADIAN)
calcPDTorque(mm.I_SO3(), mm.exp(axisX, -90 * M_RADIAN),
base.getRotation(), body1.getRotation(),
base.getAngularVel(), body1.getAngularVel(), Kp, Kd,
M1)
def extendByIntegration_constant(motion, extendLength, preserveJoints=[], finiteDiff=1):
lastFrame = len(motion)-1
p = motion.getJointPositionGlobal(0, lastFrame)
v = motion.getJointVelocityGlobal(0, lastFrame-finiteDiff, lastFrame)
a = motion.getJointAccelerationGlobal(0, lastFrame-finiteDiff, lastFrame)
ap = motion.getJointOrientationsLocal(lastFrame)
av = motion.getJointAngVelocitiesLocal(lastFrame-finiteDiff, lastFrame)
aa = motion.getJointAngAccelerationsLocal(lastFrame-finiteDiff, lastFrame)
t = 1/motion.fps
# integration
extended = ym.JointMotion([motion[0].getTPose() for i in range(extendLength)])
for i in range(extendLength):
p += v * t
# v += a * t
ap = map(lambda R0, dR: np.dot(R0, mm.exp(t*dR)), ap, av)
# av = map(lambda V0, dV: V0 + t*dV, av, aa)
extended[i].rootPos = p.copy()
extended.setJointOrientationsLocal(i, ap)
# preserve joint orientations
preserveJointOrientations = [motion[-1].getJointOrientationGlobal(footJoint) for footJoint in preserveJoints]
for extendedPosture in extended:
for i in range(len(preserveJoints)):
extendedPosture.setJointOrientationGlobal(preserveJoints[i], preserveJointOrientations[i])
return extended
def extendByIntegrationAttenuation(motion, extendLength, preserveJoints=[], finiteDiff=1, k=1.):
lastFrame = len(motion)-1
p = motion.getJointPositionGlobal(0, lastFrame)
v = motion.getJointVelocityGlobal(0, lastFrame-finiteDiff, lastFrame)
a = motion.getJointAccelerationGlobal(0, lastFrame-finiteDiff, lastFrame)
ap = motion.getJointOrientationsLocal(lastFrame)
av = motion.getJointAngVelocitiesLocal(lastFrame-finiteDiff, lastFrame)
aa = motion.getJointAngAccelerationsLocal(lastFrame-finiteDiff, lastFrame)
t = 1/motion.fps
# integration
extended = ym.JointMotion([motion[0].getTPose() for i in range(extendLength)])
for i in range(extendLength):
p += v * t * k
v += a * t * k
ap = list(map(lambda R0, dR: np.dot(R0, mm.exp(t*k*dR)), ap, av))
av = list(map(lambda V0, dV: V0 + t*k*dV, av, aa))
extended[i].rootPos = p.copy()
extended.setJointOrientationsLocal(i, ap)
# preserve joint orientations
preserveJointOrientations = [motion[-1].getJointOrientationGlobal(footJoint) for footJoint in preserveJoints]
for extendedPosture in extended:
for i in range(len(preserveJoints)):
extendedPosture.setJointOrientationGlobal(preserveJoints[i], preserveJointOrientations[i])
return extended
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 test_calcPointCloudMetric():
pointsA = [(1,0,0), (0,0,0), (0,0,1)]
R = mmMath.exp(mmMath.v3(0,1,0), math.pi/4)
pointsB = []
for p in pointsA:
pointsB.append(np.dot(R, p)+(0,1,0))
T = mmMath.I_SE3()
distance = cmt.calcPointCloudMetric(pointsA, pointsB, T)
print distance
print T
pointsC = []
for p in pointsB:
pointsC.append(mmMath.T2p(np.dot(T, mmMath.p2T(p))))
viewer = ysv.SimpleViewer()
def extraDrawCallback():
for p in pointsA:
ygh.drawPoint(p, (255,0,0))
for p in pointsB:
ygh.drawPoint(p, (0,255,0))
for p in pointsC:
ygh.drawPoint(p, (0,255,255))
viewer.setExtraDrawCallback(extraDrawCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def _readOgreSkeletonAnimations(dom, jointSkeleton, initialRs, scale=1.0):
jointMotions = []
animationEles = dom.getElementsByTagName('animation')
for animationEle in animationEles:
jointMotion = ym.Motion()
# jointMotion.resourceName = animationEle.getAttribute('name').encode()
trackEles = animationEle.getElementsByTagName('track')
first_keyframes = trackEles[0].getElementsByTagName('keyframe')
len_keyframes = len(first_keyframes)
time2frameMap = {}
for i in range(len_keyframes):
jointPosture = ym.JointPosture(jointSkeleton)
# jointPosture.initLocalRMap(initialRMap)
jointPosture.initLocalRs(initialRs)
jointMotion.append(jointPosture)
# because each bone may not have same number of keyframes
time2frameMap[first_keyframes[i].getAttribute('time')] = i
for trackEle in trackEles:
# print i, trackEle.getAttribute('bone'), len(trackEle.getElementsByTagName('keyframe'))
keyframeEles = trackEle.getElementsByTagName('keyframe')
for keyframeEle in keyframeEles:
keyframeTime = keyframeEle.getAttribute('time')
# because each bone may not have same number of keyframes
frame = time2frameMap[keyframeTime]
jointPosture = jointMotion[frame]
boneName = trackEle.getAttribute('bone').encode()
if boneName == jointSkeleton.root.name:
transEle = keyframeEle.getElementsByTagName('translate')[0]
jointPosture.rootPos[0] = float(
transEle.getAttribute('x')) * scale
jointPosture.rootPos[1] = float(
transEle.getAttribute('y')) * scale
jointPosture.rootPos[2] = float(
transEle.getAttribute('z')) * scale
rotEle = keyframeEle.getElementsByTagName('rotate')[0]
angle = float(rotEle.getAttribute('angle'))
axisEle = rotEle.getElementsByTagName('axis')[0]
axis = mmMath.v3(float(axisEle.getAttribute('x')),
float(axisEle.getAttribute('y')),
float(axisEle.getAttribute('z')))
R = mmMath.exp(axis, angle)
# jointPosture.mulLocalR(boneName, R)
jointPosture.mulLocalR(jointSkeleton.getElementIndex(boneName),
R)
jointPosture.updateGlobalT()
jointMotions.append(jointMotion)
return jointMotions
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 strs2vec3(strs):
floats = []
for w in strs:
if w == '':
floats.append(0.0)
elif w == '\r\n':
pass
else:
floats.append(float(w) / 200.)
out = []
for fidx in range(0, len(floats), 3):
vec = np.array((floats[fidx], floats[fidx + 1], floats[fidx + 2]))
R = mm.exp((1, 0, 0), -math.pi / 2)
vec = np.dot(R, vec)
out.append(vec)
return out
def test_inertia_matrix():
bvhFilePath = '../samples/chain_1.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 100.
mcfg.defaultBoneRatio = 1.
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
wcfg.gravity = (0,0,0)
stepsPerFrame = 30
wcfg.timeStep = (1/30.)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
controlModel2 = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel2.translateByOffset((0,0,1))
controlModel2.rotate(mm.exp(mm.v3(0,1,0), math.pi/2))
print 'model local'
print controlModel.getBodyInertiasLocal()
print
print 'model global'
print controlModel.getBodyInertiasGlobal()
print
print 'model2 global'
print controlModel2.getBodyInertiasGlobal()
viewer = ysv.SimpleViewer()
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
# viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('model', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
viewer.doc.addRenderer('model2', cvr.VpModelRenderer(controlModel2, (255,240,255), yr.POLYGON_LINE))
viewer.setMaxFrame(100)
viewer.show()
Fl.run()
def test_FramesRenderer_OrientationsRenderer():
frame0 = mm.I_SE3()
frame1 = mm.Rp2T(mm.exp(mm.v3(0, 1, 0), math.pi / 8.), (1, 0, 0))
viewer = ysv.SimpleViewer()
viewer.doc.addRenderer('frame0', FramesRenderer([frame0], (255, 0, 0)))
viewer.doc.addRenderer('frame1', FramesRenderer([frame1], (255, 0, 0)))
viewer.doc.addRenderer(
'orientation0',
OrientationsRenderer([mm.T2R(frame0)], [mm.T2p(frame0)],
(0, 255, 0)))
viewer.doc.addRenderer(
'orientation1',
OrientationsRenderer([mm.T2R(frame1)], [mm.T2p(frame1)],
(0, 255, 0)))
viewer.show()
Fl.run()
def test_FramesRenderer_OrientationsRenderer():
frame0 = mm.i_se3()
frame1 = mm.r_p_to_t(mm.exp(mm.vec3(0, 1, 0), math.pi / 8.), (1, 0, 0))
viewer = ysv.SimpleViewer()
viewer.doc.addRenderer('frame0', FramesRenderer([frame0], (255, 0, 0)))
viewer.doc.addRenderer('frame1', FramesRenderer([frame1], (255, 0, 0)))
viewer.doc.addRenderer(
'orientation0',
OrientationsRenderer([mm.t_to_r(frame0)], [mm.t_to_p(frame0)],
(0, 255, 0)))
viewer.doc.addRenderer(
'orientation1',
OrientationsRenderer([mm.t_to_r(frame1)], [mm.t_to_p(frame1)],
(0, 255, 0)))
viewer.show()
Fl.run()
def readTrcFile(trcFilePath, scale=1.0):
f = open(trcFilePath)
fileLines = f.readlines()
pointMotion = ym.Motion()
i = 0
while i != len(fileLines):
splited = fileLines[i].split()
boneNames = []
if i == 2:
dataRate = float(splited[0])
numFrames = int(splited[2])
numMarkers = int(splited[3])
# print numFrames, numMarkers
elif i == 3:
markerNames = [name.split(':')[1] for name in splited[2:]]
skeleton = ym.PointSkeleton()
for name in markerNames:
skeleton.addElement(None, name)
# print markerNames
elif i > 5:
markerPositions = splited[2:]
# print markerPositions
# print 'i', i
pointPosture = ym.PointPosture(skeleton)
for m in range(numMarkers):
point = numpy.array([
float(markerPositions[m * 3]),
float(markerPositions[m * 3 + 1]),
float(markerPositions[m * 3 + 2])
])
point = numpy.dot(
mm.exp(numpy.array([1, 0, 0]), -math.pi / 2.),
point) * scale
# pointPosture.addPoint(markerNames[m], point)
pointPosture.setPosition(m, point)
# print 'm', m
# print markerNames[m], (markerPositions[m*3],markerPositions[m*3+1],markerPositions[m*3+2])
pointMotion.append(pointPosture)
i += 1
f.close()
pointMotion.fps = dataRate
return pointMotion
def _readOgreSkeleton(dom, scale=1.0):
bones = dom.getElementsByTagName('bone')
jointMap = {}
# initialRMap = {}
initialRs = []
for bone in bones:
joint = ym.Joint(bone.getAttribute('name').encode(), None)
jointMap[joint.name] = joint
posEle = bone.getElementsByTagName('position')[0]
joint.offset[0] = float(posEle.getAttribute('x')) * scale
joint.offset[1] = float(posEle.getAttribute('y')) * scale
joint.offset[2] = float(posEle.getAttribute('z')) * scale
rotEle = bone.getElementsByTagName('rotation')[0]
angle = float(rotEle.getAttribute('angle'))
axisEle = rotEle.getElementsByTagName('axis')[0]
axis = mmMath.s2v((float(axisEle.getAttribute('x')),
float(axisEle.getAttribute('y')),
float(axisEle.getAttribute('z'))))
R = mmMath.exp(axis, angle)
# initialRMap[joint.name] = R
initialRs.append(R)
rootJoint = jointMap[bones[0].getAttribute('name').encode()]
boneParents = dom.getElementsByTagName('boneparent')
for bp in boneParents:
joint = jointMap[bp.getAttribute('bone').encode()]
parentJoint = jointMap[bp.getAttribute('parent').encode()]
joint.parent = parentJoint
parentJoint.addChild(joint)
jointSkeleton = ym.JointSkeleton(rootJoint)
for bone in bones:
# jointSkeleton.jointsAr.append(jointMap[bone.getAttribute('name')])
boneName = bone.getAttribute('name').encode()
jointSkeleton.addElement(jointMap[boneName], boneName)
# return jointSkeleton, initialRMap
return jointSkeleton, initialRs
def extendByIntegrationIK(motion, extendLength, effectorJoint, preserveJoints=[], finiteDiff=1):
lastFrame = len(motion)-1
p = motion.getJointPositionGlobal(0, lastFrame)
v = motion.getJointVelocityGlobal(0, lastFrame-finiteDiff, lastFrame)
a = motion.getJointAccelerationGlobal(0, lastFrame-finiteDiff, lastFrame)
ap = motion.getJointOrientationsLocal(lastFrame)
av = motion.getJointAngVelocitiesLocal(lastFrame-finiteDiff, lastFrame)
aa = motion.getJointAngAccelerationsLocal(lastFrame-finiteDiff, lastFrame)
p_effector = motion.getJointPositionGlobal(effectorJoint, lastFrame)
v_effector = motion.getJointVelocityGlobal(effectorJoint, lastFrame-finiteDiff, lastFrame)
a_effector = motion.getJointAccelerationGlobal(effectorJoint, lastFrame-finiteDiff, lastFrame)
t = 1/motion.fps
# integration
extended = ym.JointMotion([motion[0].getTPose() for i in range(extendLength)])
for i in range(extendLength):
p += v * t
v += a * t
ap = map(lambda R0, dR: np.dot(R0, mm.exp(t*dR)), ap, av)
av = map(lambda V0, dV: V0 + t*dV, av, aa)
extended[i].rootPos = p.copy()
extended.setJointOrientationsLocal(i, ap)
# preserve joint orientations
preserveJointOrientations = [motion[-1].getJointOrientationGlobal(footJoint) for footJoint in preserveJoints]
for extendedPosture in extended:
for i in range(len(preserveJoints)):
extendedPosture.setJointOrientationGlobal(preserveJoints[i], preserveJointOrientations[i])
# integration with IK
for i in range(extendLength):
p_effector += v_effector * t
v_effector += a_effector * t
aik.ik_analytic(extended[i], effectorJoint, p_effector)
return extended
def readTrcFile(trcFilePath, scale = 1.0):
f = open(trcFilePath)
fileLines = f.readlines()
pointMotion = ym.Motion()
i = 0
while i != len(fileLines):
splited = fileLines[i].split()
boneNames = []
if i == 2:
dataRate = float(splited[0])
numFrames = int(splited[2])
numMarkers = int(splited[3])
# print numFrames, numMarkers
elif i == 3:
markerNames = [name.split(':')[1] for name in splited[2:]]
skeleton = ym.PointSkeleton()
for name in markerNames:
skeleton.addElement(None, name)
# print markerNames
elif i > 5:
markerPositions = splited[2:]
# print markerPositions
# print 'i', i
pointPosture = ym.PointPosture(skeleton)
for m in range(numMarkers):
point = numpy.array([float(markerPositions[m*3]),float(markerPositions[m*3+1]),float(markerPositions[m*3+2])])
point = numpy.dot(mm.exp(numpy.array([1,0,0]), -math.pi/2.), point)*scale
# pointPosture.addPoint(markerNames[m], point)
pointPosture.setPosition(m, point)
# print 'm', m
# print markerNames[m], (markerPositions[m*3],markerPositions[m*3+1],markerPositions[m*3+2])
pointMotion.append(pointPosture)
i += 1
f.close()
pointMotion.fps = dataRate
return pointMotion
def ik_analytic(posture, joint_name_or_index, new_position):
if isinstance(joint_name_or_index, int):
joint = joint_name_or_index
else:
joint = posture.skeleton.getJointIndex(joint_name_or_index)
# joint_parent = posture.body.joint_parent[joint]
# joint_parent_parent = posture.body.joint_parent[joint_parent]
joint_parent = posture.skeleton.getParentJointIndex(joint)
joint_parent_parent = posture.skeleton.getParentJointIndex(joint_parent)
# B = posture.get_position(joint)
# C = posture.get_position(joint_parent)
# A = posture.get_position(joint_parent_parent)
B = posture.getJointPositionGlobal(joint)
C = posture.getJointPositionGlobal(joint_parent)
A = posture.getJointPositionGlobal(joint_parent_parent)
L = B - A
N = B - C
M = C - A
# l = mathlib.length(L);
# n = mathlib.length(N);
# m = mathlib.length(M);
l = mm.length(L);
n = mm.length(N);
m = mm.length(M);
# a = mathlib.ACOS((l*l + n*n - m*m) / (2*l*n))
# b = mathlib.ACOS((l*l + m*m - n*n) / (2*l*m))
a = mm.ACOS((l*l + n*n - m*m) / (2*l*n))
b = mm.ACOS((l*l + m*m - n*n) / (2*l*m))
B_new = new_position;
L_new = B_new - A;
# l_ = mathlib.length(L_new)
l_ = mm.length(L_new)
# a_ = mathlib.ACOS((l_*l_ + n*n - m*m) / (2*l_*n))
# b_ = mathlib.ACOS((l_*l_ + m*m - n*n) / (2*l_*m))
a_ = mm.ACOS((l_*l_ + n*n - m*m) / (2*l_*n))
b_ = mm.ACOS((l_*l_ + m*m - n*n) / (2*l_*m))
# rotate joint in plane
# rotV = mathlib.normalize(numpy.cross(M, L))
rotV = mm.normalize2(np.cross(M, L))
rotb = b - b_;
rota = a_ - a - rotb;
# posture.rotate_global_orientation(joint_parent_parent, mathlib.exp(rotV, rotb))
# posture.rotate_global_orientation(joint_parent, mathlib.exp(rotV * rota))
posture.mulJointOrientationGlobal(joint_parent_parent, mm.exp(rotV, rotb))
posture.mulJointOrientationGlobal(joint_parent, mm.exp(rotV * rota))
# rotate plane
# rotV2 = mathlib.normalize(numpy.cross(L, L_new))
# l_new = mathlib.length(L_new)
# l_diff = mathlib.length(L_new - L)
# rot2 = mathlib.ACOS((l_new * l_new + l * l - l_diff * l_diff) / (2 * l_new * l))
# posture.rotate_global_orientation(joint_parent_parent, mathlib.exp(rotV2, rot2))
rotV2 = mm.normalize2(np.cross(L, L_new))
l_new = mm.length(L_new)
l_diff = mm.length(L_new - L)
rot2 = mm.ACOS((l_new * l_new + l * l - l_diff * l_diff) / (2 * l_new * l))
posture.mulJointOrientationGlobal(joint_parent_parent, mm.exp(rotV2, rot2))
return posture
def PDControl(frame):
# global deg1[0], J1, J2, M1, M2
# scalar Kp = 1000.;
# scalar Kd = 100.;
Kp = 100.
Kd = 2.
# SE3 desiredOri1 = Exp(Axis(axis1), scalar(deg1[0] * M_RADIAN));
# SE3 desiredOri2 = Exp(Axis(axis2), scalar(deg2 * M_RADIAN));
desiredOri1 = mm.exp(axis1, deg1[0] * M_RADIAN)
# desiredOri2 = mm.exp(axis2, deg2 * M_RADIAN)
# se3 log1= Log(J1.GetOrientation() % desiredOri1);
# se3 log2= Log(J2.GetOrientation() % desiredOri2);
parent1 = J1.getBody(0)
child1 = J1.getBody(1)
parent1_desired_SO3 = mm.exp((0, 0, 0), 0)
child1_desired_SO3 = desiredOri1
# child1_desired_SO3 = parent1_desired_SO3
parent1_body_SO3 = mm.odeSO3ToSO3(parent1.getRotation())
child1_body_SO3 = mm.odeSO3ToSO3(child1.getRotation())
# init_ori = (mm.exp((1,0,0),math.pi/2))
# child1_body_SO3 = numpy.dot(mm.odeSO3ToSO3(child1.getRotation()), init_ori.transpose())
align_SO3 = numpy.dot(parent1_body_SO3,
parent1_desired_SO3.transpose())
child1_desired_SO3 = numpy.dot(align_SO3, child1_desired_SO3)
diff_rot = mm.logSO3(
numpy.dot(child1_desired_SO3, child1_body_SO3.transpose()))
# print diff_rot
parent_angleRate = parent1.getAngularVel()
child_angleRate = child1.getAngularVel()
# print child_angleRate
angleRate = numpy.array([
-parent_angleRate[0] + child_angleRate[0],
-parent_angleRate[1] + child_angleRate[1],
-parent_angleRate[2] + child_angleRate[2]
])
# torque1 = Kp*diff_rot - Kd*angleRate
# print torque1
# J1_ori =
# log1 = mm.logSO3_tuple(numpy.dot(desiredOri1 ,J1.GetOrientation().transpose()))
# log2 = mm.logSO3_tuple(numpy.dot(desiredOri1 ,J1.GetOrientation().transpose()))
# Vec3 torque1 = Kp*(Vec3(log1[0],log1[1],log1[2])) - Kd*J1.GetVelocity();
# Vec3 torque2 = Kp*(Vec3(log2[0],log2[1],log2[2])) - Kd*J2.GetVelocity();
M1.setAxis(0, 0, diff_rot)
M1.setAxis(1, 0, angleRate)
# M2.setAxis(0,0,torque1)
## J1.SetTorque(torque1);
## J2.SetTorque(torque2);
M1.addTorques(-Kp * mm.length(diff_rot), 0, 0)
M1.addTorques(0, Kd * mm.length(angleRate), 0)
def applyThetas(self, deltaThetas, axes, inoutPosture):
for i in range(self.skeleton.getElementNum()-1):
inoutPosture.mulGlobalR(i, mm.exp(axes[i], deltaThetas[i]))
inoutPosture.updateGlobalT()
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
motionName = 'wd2_stand.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
#motionName = 'ww13_41_V001.bvh'
motion = yf.readBvhFile(motionName, .01)
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
if FOOT_PART_NUM == 1:
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(2.5, -0.0, .3), -.5),
False)
yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(2.5, 0.0, -.3), -.5),
False)
if MOTION == FORWARD_JUMP:
yme.rotateJointLocal(motion, LEFT_UP_LEG,
mm.exp(mm.v3(0.0, .0, 1.), .08), False)
yme.rotateJointLocal(motion, LEFT_LEG, mm.exp(mm.v3(0.0, 1.0, 0.),
-.2), False)
if FOOT_PART_NUM > 1:
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TARSUS)
yme.addJoint(motion, RIGHT_TARSUS, 'RIGHT_Dummy1')
yme.addJoint(motion, LEFT_FOOT, LEFT_TARSUS)
yme.addJoint(motion, LEFT_TARSUS, 'LEFT_Dummy1')
yme.rotateJointLocal(motion, LEFT_TOES, mm.exp(mm.v3(1., 0.0, 0.0),
.45), False)
yme.rotateJointLocal(motion, RIGHT_TOES,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_TARSUS,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
yme.rotateJointLocal(motion, RIGHT_TARSUS,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
if FOOT_PART_NUM == 5:
yme.addJoint(motion, LEFT_FOOT, LEFT_FOOT_SIDE_L)
yme.addJoint(motion, LEFT_FOOT_SIDE_L, 'LEFT_Dummy2')
yme.addJoint(motion, LEFT_FOOT, LEFT_FOOT_SIDE_R)
yme.addJoint(motion, LEFT_FOOT_SIDE_R, 'LEFT_Dummy2')
yme.addJoint(motion, RIGHT_FOOT, RIGHT_FOOT_SIDE_L)
yme.addJoint(motion, RIGHT_FOOT_SIDE_L, 'RIGHT_Dummy2')
yme.addJoint(motion, RIGHT_FOOT, RIGHT_FOOT_SIDE_R)
yme.addJoint(motion, RIGHT_FOOT_SIDE_R, 'RIGHT_Dummy2')
yme.rotateJointLocal(motion, LEFT_FOOT_SIDE_L,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_FOOT_SIDE_R,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, RIGHT_FOOT_SIDE_L,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, RIGHT_FOOT_SIDE_R,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
## Taekwondo turning-kick
#motion = motion[108:-1]
#motion = motion[108:109]
motion[0:0] = [motion[0]] * 100
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
#node.length = .2 ####
if FOOT_PART_NUM == 1:
length1 = .25
width1 = .2
mass1 = 4.
elif FOOT_PART_NUM == 3:
length1 = .1
width1 = .2
mass1 = 1.5
length2 = .1
width2 = .2
mass2 = 1.5
elif FOOT_PART_NUM == 5:
length1 = .1
width1 = .065
mass1 = .5
length2 = .1
width2 = .2
mass2 = 1.5
node = mcfg.getNode(RIGHT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
if FOOT_PART_NUM == 5:
node = mcfg.getNode(LEFT_FOOT_SIDE_L)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0.07, 0.0, 0.015)
node = mcfg.getNode(LEFT_FOOT_SIDE_R)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (-0.07, 0.0, 0.015)
node = mcfg.getNode(RIGHT_FOOT_SIDE_L)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0.07, 0.0, 0.015)
node = mcfg.getNode(RIGHT_FOOT_SIDE_R)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (-0.07, 0.0, 0.015)
if FOOT_PART_NUM > 1:
node = mcfg.getNode(LEFT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.02)
node = mcfg.getNode(RIGHT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.02)
node = mcfg.getNode(LEFT_TARSUS)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.08)
node = mcfg.getNode(RIGHT_TARSUS)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.08)
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
if FOOT_PART_NUM == 1:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 1.,
LEFT_FOOT: 1.,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.
}
elif FOOT_PART_NUM == 3:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .3,
RIGHT_LEG: .3,
LEFT_UP_LEG: .3,
LEFT_LEG: .3,
LEFT_TOES: .3,
RIGHT_TOES: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.5,
LEFT_FOOT: 2.5,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_TOES: .3,
RIGHT_TOES: .3
}
elif FOOT_PART_NUM == 5:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3,
LEFT_TOES: .3,
RIGHT_TOES: .3,
LEFT_TARSUS: .3,
RIGHT_TARSUS: .3,
LEFT_FOOT_SIDE_L: .3,
LEFT_FOOT_SIDE_R: .3,
RIGHT_FOOT_SIDE_L: .3,
RIGHT_FOOT_SIDE_R: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.5,
LEFT_FOOT: 2.5,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_TOES: .3,
RIGHT_TOES: .3,
LEFT_TARSUS: .3,
RIGHT_TARSUS: .3,
LEFT_FOOT_SIDE_L: .3,
LEFT_FOOT_SIDE_R: .3,
RIGHT_FOOT_SIDE_L: .3,
RIGHT_FOOT_SIDE_R: .3
}
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
#config['end'] = 'HIP'
config['end'] = SPINE1
config['const'] = HIP
config['root'] = HIP
config['FootPartNum'] = FOOT_PART_NUM
config['FootLPart'] = [
LEFT_FOOT, LEFT_TOES, LEFT_TARSUS, LEFT_FOOT_SIDE_L, LEFT_FOOT_SIDE_R
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_FOOT_SIDE_L,
RIGHT_FOOT_SIDE_R
]
return motion, mcfg, wcfg, stepsPerFrame, config
node = mcfg.getNode('Spine')
node.width = .22
node = mcfg.getNode('RightFoot')
node.length = .25
node = mcfg.getNode('LeftFoot')
node.length = .25
return mcfg
g_motionDirConfigMap = {}
g_motionDirConfigMap['../../../Data/woody2/Motion/Physics2/'] = \
{'footRot': mm.exp(mm.vec3(1,0,0), -.4), 'yOffset': .0, 'scale':1.,'rootRot': mm.i_so3()}
g_motionDirConfigMap['../../../Data/woody2/Motion/Balancing/'] = \
{'footRot': mm.exp(mm.vec3(1,-.5,0), -.6), 'yOffset': .0, 'scale':1.,'rootRot': mm.exp(mm.vec3(1,0,0), .01)}
g_motionDirConfigMap['../../../Data/woody2/Motion/Picking/'] = \
{'footRot': mm.exp(mm.vec3(1,0,0), -.5), 'yOffset': .0, 'scale':1.,'rootRot': mm.i_so3()}
g_motionDirConfigMap['../../../Data/woody2/Motion/Samsung/'] = \
{'footRot': mm.rotX(-.5), 'yOffset': .0, 'scale':2.53999905501,'rootRot': mm.i_so3()}
g_motionDirConfigMap['../../../Data/woody2/Motion/VideoMotion/'] = \
{'footRot': mm.exp(mm.vec3(1,0,0), -.05), 'yOffset': .01, 'scale':2.53999905501,'rootRot': mm.exp(mm.vec3(1,0,0), .0)}
if __name__=='__main__':
#===============================================================================
# initialize motion
#===============================================================================
motionFiles = []
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
motionName = 'wd2_stand.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
#motionName = 'ww13_41_V001.bvh'
motion = yf.readBvhFile(motionName, .01)
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
#yme.removeJoint(motion, RIGHT_TOES_END, False)
#yme.removeJoint(motion, LEFT_TOES_END, False)
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(2.5, -0.0, .3), -.5),
False)
yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(2.5, 0.0, -.3), -.5),
False)
if MOTION == FORWARD_JUMP:
yme.rotateJointLocal(motion, LEFT_UP_LEG,
mm.exp(mm.v3(0.0, .0, 1.), .08), False)
yme.rotateJointLocal(motion, LEFT_LEG, mm.exp(mm.v3(0.0, 1.0, 0.),
-.2), False)
#yme.offsetJointLocal(motion, LEFT_TOES, (.03,-.05,0), False)
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCA)
yme.addJoint(motion, RIGHT_CALCA, 'RIGHT_Dummy')
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCA)
yme.addJoint(motion, LEFT_CALCA, 'LEFT_Dummy')
if 0:
yme.rotateJointLocal(motion, LEFT_TOES, mm.exp(mm.v3(1., 0.0, 0.0),
.2), False)
yme.rotateJointLocal(motion, RIGHT_TOES,
mm.exp(mm.v3(1., 0.0, 0.0), .2), False)
yme.rotateJointLocal(motion, LEFT_CALCA,
mm.exp(mm.v3(1., 0.0, 0.0), .7), False)
yme.rotateJointLocal(motion, RIGHT_CALCA,
mm.exp(mm.v3(1., 0.0, 0.0), .7), False)
else:
yme.rotateJointLocal(motion, LEFT_TOES, mm.exp(mm.v3(1., 0.0, 0.0),
.45), False)
yme.rotateJointLocal(motion, RIGHT_TOES,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_CALCA,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
yme.rotateJointLocal(motion, RIGHT_CALCA,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
## Taekwondo turning-kick
#motion = motion[100:-1]
motion[0:0] = [motion[0]] * 100
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
#node.length = .2 ####
node = mcfg.getNode(RIGHT_FOOT)
node.length = .1
node.width = .2
node.mass = 1.5
node = mcfg.getNode(LEFT_FOOT)
node.length = .1
node.width = .2
node.mass = 1.5
node = mcfg.getNode(LEFT_TOES)
node.length = .1
node.width = .2
node.mass = 1.5
node.offset = (0, 0.0, -0.02)
node = mcfg.getNode(RIGHT_TOES)
node.length = .1
node.width = .2
node.mass = 1.5
node.offset = (0, 0.0, -0.02)
node = mcfg.getNode(LEFT_CALCA)
node.length = .1
node.width = .2
node.mass = 1.5
node.offset = (0, 0.0, -0.08)
node = mcfg.getNode(RIGHT_CALCA)
node.length = .1
node.width = .2
node.mass = 1.5
node.offset = (0, 0.0, -0.08)
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
'''
config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain
config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain
config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain
config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 2.5
config['Bh'] = 1.
'''
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2,\
SPINE:.3, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5,\
RIGHT_UP_LEG:.1, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3, LEFT_TOES:.1, RIGHT_TOES:.1, LEFT_CALCA:.3, RIGHT_CALCA:.3}
config['IKweightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2,\
SPINE:.3, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5,\
RIGHT_UP_LEG:.1, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3}
'''
config['IKweightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2,\
SPINE:0.5, SPINE1:0.5, RIGHT_FOOT:1.2, LEFT_FOOT:1.2, HIP:1.2,\
RIGHT_UP_LEG:.9, RIGHT_LEG:.9, LEFT_UP_LEG:.9, LEFT_LEG:.9}
'''
config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2,\
SPINE:1.5, LEFT_FOOT:1., HIP:1.5,\
RIGHT_UP_LEG:1., RIGHT_LEG:1., LEFT_UP_LEG:1., LEFT_LEG:1., LEFT_TOES:.1, RIGHT_TOES:.1, LEFT_CALCA:.1, RIGHT_CALCA:.1}
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
#config['end'] = 'HIP'
config['end'] = SPINE1
config['const'] = HIP
config['root'] = HIP
config['leftForeFoot'] = LEFT_TOES
config['rightForeFoot'] = RIGHT_TOES
config['leftRearFoot'] = LEFT_CALCA
config['rightRearFoot'] = RIGHT_CALCA
return motion, mcfg, wcfg, stepsPerFrame, config
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_FOOT_SIDE_L,
RIGHT_FOOT_SIDE_R
]
return motion, mcfg, wcfg, stepsPerFrame, config
#===============================================================================
# biped config
#===============================================================================
# motion, mesh config
g_motionDirConfigMap = {}
g_motionDirConfigMap['../Data/woody2/Motion/Physics2/'] = \
{'footRot': mm.exp(mm.v3(1,0,0), .05), 'yOffset': .0, 'scale':1.,\
'rootRot': mm.I_SO3()}
g_motionDirConfigMap['../Data/woody2/Motion/Balancing/'] = \
{'footRot': mm.exp(mm.v3(1,-.5,0), -.6), 'yOffset': .0, 'scale':1.,\
'rootRot': mm.exp(mm.v3(1,0,0), .01)}
g_motionDirConfigMap['../Data/woody2/Motion/VideoMotion/'] = \
{'footRot': mm.exp(mm.v3(1,0,0), -.05), 'yOffset': .01, 'scale':2.53999905501,\
'rootRot': mm.exp(mm.v3(1,0,0), .0)}
g_motionDirConfigMap['../Data/woody2/Motion/Samsung/'] = \
{'footRot': mm.exp(mm.v3(1,0,0), -.03), 'yOffset': .0, 'scale':2.53999905501,\
'rootRot': mm.exp(mm.v3(1,0,0), .03)}
#===============================================================================
# # reloadable config
#===============================================================================
def simulateCallback(frame):
# seginfo
segIndex = seg_index[0]
curState = seginfo[segIndex]['state']
curInterval = yma.offsetInterval(acc_offset[0], seginfo[segIndex]['interval'])
stanceLegs = seginfo[segIndex]['stanceHips']
swingLegs = seginfo[segIndex]['swingHips']
stanceFoots = seginfo[segIndex]['stanceFoots']
swingFoots = seginfo[segIndex]['swingFoots']
swingKnees = seginfo[segIndex]['swingKnees']
groundHeight = seginfo[segIndex]['ground_height']
# maxStfPushFrame = seginfo[segIndex]['max_stf_push_frame']
prev_frame = frame-1 if frame>0 else 0
# prev_frame = frame
# information
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, upperMass, uppers)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, upperMass, uppers)
## dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, totalMass)
## CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, totalMass)
# stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], frame)
# CMr_tar = CM_tar - stf_tar
dCM_tar = motion_seg.getJointVelocityGlobal(0, prev_frame)
CM_tar = motion_seg.getJointPositionGlobal(0, prev_frame)
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, upperMass, uppers)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, upperMass, uppers)
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, totalMass)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, totalMass)
stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], prev_frame)
CMr_tar = CM_tar - stf_tar
dCM = avg_dCM[0]
CM = controlModel.getJointPositionGlobal(0)
# CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, upperMass, uppers)
# CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass)
CMreal = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass)
stf = controlModel.getJointPositionGlobal(stanceFoots[0])
CMr = CM - stf
diff_dCM = mm.projectionOnPlane(dCM-dCM_tar, (1,0,0), (0,0,1))
diff_dCM_axis = np.cross((0,1,0), diff_dCM)
rd_vec1[0] = diff_dCM; rd_vecori1[0] = CM_tar
diff_CMr = mm.projectionOnPlane(CMr-CMr_tar, (1,0,0), (0,0,1))
# rd_vec1[0] = diff_CMr; rd_vecori1[0] = stf_tar
diff_CMr_axis = np.cross((0,1,0), diff_CMr)
direction = mm.normalize2(mm.projectionOnPlane(dCM_tar, (1,0,0), (0,0,1)))
# direction = mm.normalize2(mm.projectionOnPlane(dCM, (1,0,0), (0,0,1)))
directionAxis = np.cross((0,1,0), direction)
diff_dCM_sag, diff_dCM_cor = mm.projectionOnVector2(diff_dCM, direction)
# rd_vec1[0] = diff_dCM_sag; rd_vecori1[0] = CM_tar
diff_dCM_sag_axis = np.cross((0,1,0), diff_dCM_sag)
diff_dCM_cor_axis = np.cross((0,1,0), diff_dCM_cor)
diff_CMr_sag, diff_CMr_cor = mm.projectionOnVector2(diff_CMr, direction)
diff_CMr_sag_axis = np.cross((0,1,0), diff_CMr_sag)
diff_CMr_cor_axis = np.cross((0,1,0), diff_CMr_cor)
t = (frame-curInterval[0])/float(curInterval[1]-curInterval[0])
t_raw = t
if t>1.: t=1.
p_root = motion_stitch[frame].getJointPositionGlobal(0)
R_root = motion_stitch[frame].getJointOrientationGlobal(0)
motion_seg_orig.goToFrame(frame)
motion_seg.goToFrame(frame)
motion_stitch.goToFrame(frame)
motion_debug1.append(motion_stitch[frame].copy())
motion_debug1.goToFrame(frame)
motion_debug2.append(motion_stitch[frame].copy())
motion_debug2.goToFrame(frame)
motion_debug3.append(motion_stitch[frame].copy())
motion_debug3.goToFrame(frame)
# paper implementation
M_tc.append(motion_stitch[prev_frame])
M_tc.goToFrame(frame)
P_hat.append(M_tc[frame].copy())
P_hat.goToFrame(frame)
p_temp = ym.JointPosture(skeleton)
p_temp.rootPos = controlModel.getJointPositionGlobal(0)
p_temp.setJointOrientationsLocal(controlModel.getJointOrientationsLocal())
P.append(p_temp)
P.goToFrame(frame)
# stance foot stabilize
motion_stf_stabilize.append(motion_stitch[frame].copy())
motion_stf_stabilize.goToFrame(frame)
if STANCE_FOOT_STABILIZE:
for stanceFoot in stanceFoots:
R_target_foot = motion_seg[frame].getJointOrientationGlobal(stanceFoot)
R_current_foot = motion_stf_stabilize[frame].getJointOrientationGlobal(stanceFoot)
motion_stf_stabilize[frame].setJointOrientationGlobal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t)))
# R_target_foot = motion_seg[frame].getJointOrientationLocal(stanceFoot)
# R_current_foot = motion_stf_stabilize[frame].getJointOrientationLocal(stanceFoot)
# motion_stf_stabilize[frame].setJointOrientationLocal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t)))
# match stance leg
motion_match_stl.append(motion_stf_stabilize[frame].copy())
motion_match_stl.goToFrame(frame)
if MATCH_STANCE_LEG:
if curState!=yba.GaitState.STOP:
for i in range(len(stanceLegs)):
stanceLeg = stanceLegs[i]
stanceFoot = stanceFoots[i]
# # motion stance leg -> character stance leg as time goes
R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg)
R_character = controlModel.getJointOrientationGlobal(stanceLeg)
motion_match_stl[frame].setJointOrientationGlobal(stanceLeg, cm.slerp(R_motion, R_character, match_stl_func(t)))
# t_y = match_stl_func_y(t)
# t_xz = match_stl_func(t)
#
# R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg)
# R_character = controlModel.getJointOrientationGlobal(stanceLeg)
# R = np.dot(R_character, R_motion.T)
# R_y, R_xz = mm.projectRotation((0,1,0), R)
# motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_xz, t_xz))
# motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_y, t_y))
# swing foot placement
motion_swf_placement.append(motion_match_stl[frame].copy())
motion_swf_placement.goToFrame(frame)
if SWING_FOOT_PLACEMENT:
t_swing_foot_placement = swf_placement_func(t);
if extended[0]:
R_swp_sag = prev_R_swp[0][0]
R_swp_cor = prev_R_swp[0][1]
else:
R_swp_sag = mm.I_SO3(); R_swp_cor = mm.I_SO3()
R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_dCM_cor_axis * K_swp_vel_cor * -t_swing_foot_placement))
if np.dot(direction, diff_CMr_sag) < 0:
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag * -t_swing_foot_placement))
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag * -t_swing_foot_placement))
else:
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag_faster * -t_swing_foot_placement))
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag_faster * -t_swing_foot_placement))
R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_CMr_cor_axis * K_swp_pos_cor * -t_swing_foot_placement))
for i in range(len(swingLegs)):
swingLeg = swingLegs[i]
swingFoot = swingFoots[i]
# save swing foot global orientation
# R_swf = motion_swf_placement[frame].getJointOrientationGlobal(swingFoot)
# rotate swing leg
motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_sag)
motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_cor)
# restore swing foot global orientation
# motion_swf_placement[frame].setJointOrientationGlobal(swingFoot, R_swf)
prev_R_swp[0] = (R_swp_sag, R_swp_cor)
# swing foot height
motion_swf_height.append(motion_swf_placement[frame].copy())
motion_swf_height.goToFrame(frame)
if SWING_FOOT_HEIGHT:
for swingFoot in swingFoots:
stanceFoot = stanceFoots[0]
# save foot global orientation
R_foot = motion_swf_height[frame].getJointOrientationGlobal(swingFoot)
R_stance_foot = motion_swf_height[frame].getJointOrientationGlobal(stanceFoot)
if OLD_SWING_HEIGHT:
height_tar = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1]
else:
height_tar = motion_swf_height[prev_frame].getJointPositionGlobal(swingFoot)[1] - groundHeight
d_height_tar = motion_swf_height.getJointVelocityGlobal(swingFoot, prev_frame)[1]
# height_tar += c_swf_mid_offset * swf_height_sine_func(t)
# motion_debug1[frame] = motion_swf_height[frame].copy()
# rotate
motion_swf_height[frame].rotateByTarget(controlModel.getJointOrientationGlobal(0))
# motion_debug2[frame] = motion_swf_height[frame].copy()
# motion_debug2[frame].translateByTarget(controlModel.getJointPositionGlobal(0))
if OLD_SWING_HEIGHT:
height_cur = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1]
else:
height_cur = controlModel.getJointPositionGlobal(swingFoot)[1] - halfFootHeight - c_swf_offset
d_height_cur = controlModel.getJointVelocityGlobal(swingFoot)[1]
if OLD_SWING_HEIGHT:
offset_height = (height_tar - height_cur) * swf_height_func(t) * c5
else:
offset_height = ((height_tar - height_cur) * c5
+ (d_height_tar - d_height_cur) * c6) * swf_height_func(t)
offset_sine = c_swf_mid_offset * swf_height_sine_func(t)
# offset_sine = 0.
offset = 0.
offset += offset_height
offset += offset_sine
if offset > 0.:
newPosition = motion_swf_height[frame].getJointPositionGlobal(swingFoot)
newPosition[1] += offset
aik.ik_analytic(motion_swf_height[frame], swingFoot, newPosition)
else:
if HIGHER_OFFSET:
newPosition = motion_swf_height[frame].getJointPositionGlobal(stanceFoot)
newPosition[1] -= offset
aik.ik_analytic(motion_swf_height[frame], stanceFoot, newPosition)
# return
# motion_debug3[frame] = motion_swf_height[frame].copy()
# motion_debug3[frame].translateByTarget(controlModel.getJointPositionGlobal(0))
motion_swf_height[frame].rotateByTarget(R_root)
# restore foot global orientation
motion_swf_height[frame].setJointOrientationGlobal(swingFoot, R_foot)
motion_swf_height[frame].setJointOrientationGlobal(stanceFoot, R_stance_foot)
if plot!=None:
plot.addDataPoint('debug1', frame, offset_height)
plot.addDataPoint('debug2', frame, height_tar - height_cur)
# plot.addDataPoint('diff', frame, diff)
# swing foot orientation
motion_swf_orientation.append(motion_swf_height[frame].copy())
motion_swf_orientation.goToFrame(frame)
if SWING_FOOT_ORIENTATION:
swf_orientation_func = yfg.concatenate([yfg.zero, yfg.hermite2nd, yfg.one], [.25, .75])
for swingFoot in swingFoots:
R_target_foot = motion_seg[curInterval[1]].getJointOrientationGlobal(swingFoot)
R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot)
motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot, swf_orientation_func(t)))
# swf_stabilize_func = yfg.concatenate([yfg.hermite2nd, yfg.one], [c_taking_duration])
# push orientation
# for swingFoot in swingFoots:
# R_target_foot = motion_seg[frame].getJointOrientationGlobal(swingFoot)
# R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot)
# motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot , swf_stabilize_func(t)))
# stance foot push
motion_stf_push.append(motion_swf_orientation[frame].copy())
motion_stf_push.goToFrame(frame)
if STANCE_FOOT_PUSH:
for swingFoot in swingFoots:
# max_t = (maxStfPushFrame)/float(curInterval[1]-curInterval[0])
# stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [max_t*2])
stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [c_taking_duration*2])
R_swp_sag = mm.I_SO3()
# R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_stp_vel * -stf_push_func(t)))
# if step_length_cur[0] < step_length_tar[0]:
# ratio = step_length_cur[0] / step_length_tar[0]
# R_max = maxmaxStfPushFrame
# R_zero =
R_swp_sag = np.dot(R_swp_sag, mm.exp((step_length_tar[0] - step_length_cur[0])*step_axis[0] * K_stp_pos * -stf_push_func(t)))
motion_stf_push[frame].mulJointOrientationGlobal(swingFoot, R_swp_sag)
# stance foot balancing
motion_stf_balancing.append(motion_stf_push[frame].copy())
motion_stf_balancing.goToFrame(frame)
if STANCE_FOOT_BALANCING:
R_stb = mm.exp(diff_dCM_axis * K_stb_vel * stf_balancing_func(t))
R_stb = np.dot(R_stb, mm.exp(diff_CMr_axis * K_stb_pos * stf_balancing_func(t)))
for stanceFoot in stanceFoots:
if frame < 5: continue
motion_stf_balancing[frame].mulJointOrientationGlobal(stanceFoot, R_stb)
# control trajectory
motion_control.append(motion_stf_balancing[frame].copy())
motion_control.goToFrame(frame)
#=======================================================================
# tracking with inverse dynamics
#=======================================================================
th_r = motion_control.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion_control.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion_control.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
#=======================================================================
# simulation
#=======================================================================
CP = mm.v3(0.,0.,0.)
F = mm.v3(0.,0.,0.)
avg_dCM[0] = mm.v3(0.,0.,0.)
# external force rendering info
del rd_forces[:]; del rd_force_points[:]
for fi in forceInfos:
if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime):
rd_forces.append(fi.force)
rd_force_points.append(controlModel.getBodyPositionGlobal(fi.targetBody) + -mm.normalize2(fi.force)*.2)
for i in range(stepsPerFrame):
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces)
# apply external force
for fi in forceInfos:
if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime):
controlModel.applyBodyForceGlobal(fi.targetBody, fi.force)
controlModel.setDOFAccelerations(ddth_des)
controlModel.solveHybridDynamics()
# # apply external force
# for fi in forceInfos:
# if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime):
# controlModel.applyBodyForceGlobal(fi.targetBody, fi.force)
vpWorld.step()
# yvu.align2D(controlModel)
if len(contactForces) > 0:
CP += yrp.getCP(contactPositions, contactForces)
F += sum(contactForces)
avg_dCM[0] += controlModel.getJointVelocityGlobal(0)
# avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, upperMass, uppers)
# avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, totalMass)
# if len(stanceFoots)>0:
# avg_stf_v[0] += controlModel.getJointVelocityGlobal(stanceFoots[0])
# avg_stf_av[0] += controlModel.getJointAngVelocityGlobal(stanceFoots[0])
CP /= stepsPerFrame
F /= stepsPerFrame
avg_dCM[0] /= stepsPerFrame
# if len(stanceFoots)>0:
# avg_stf_v[0] /= stepsPerFrame
# avg_stf_av[0] /= stepsPerFrame
# rd_vec1[0] = avg_stf_av[0]; rd_vec1[0][0] = 0.; rd_vec1[0][2] = 0.
# rd_vecori1[0]= controlModel.getJointPositionGlobal(stanceFoots[0])
#=======================================================================
# segment editing
#=======================================================================
lastFrame = False
if SEGMENT_EDITING:
if curState==yba.GaitState.STOP:
if frame == len(motion_seg)-1:
lastFrame = True
elif (curState==yba.GaitState.LSWING or curState==yba.GaitState.RSWING) and t>c_min_contact_time:
swingID = lID if curState==yba.GaitState.LSWING else rID
contact = False
if swingID in bodyIDs:
minContactVel = 1000.
for i in range(len(bodyIDs)):
if bodyIDs[i]==swingID:
vel = controlModel.getBodyVelocityGlobal(swingID, contactPositionLocals[i])
vel[1] = 0
contactVel = mm.length(vel)
if contactVel < minContactVel: minContactVel = contactVel
if minContactVel < c_min_contact_vel: contact = True
extended[0] = False
if contact:
# print frame, 'foot touch'
lastFrame = True
acc_offset[0] += frame - curInterval[1]
elif frame == len(motion_seg)-1:
print frame, 'extend frame', frame+1
preserveJoints = []
# preserveJoints = [lFoot, rFoot]
# preserveJoints = [lFoot, rFoot, lKnee, rKnee]
# preserveJoints = [lFoot, rFoot, lKnee, rKnee, lUpLeg, rUpLeg]
stanceKnees = [rKnee] if curState==yba.GaitState.LSWING else [lKnee]
preserveJoints = [stanceFoots[0], stanceKnees[0], stanceLegs[0]]
diff = 3
motion_seg_orig.extend([motion_seg_orig[-1]])
motion_seg.extend(ymt.extendByIntegration_root(motion_seg, 1, diff))
motion_stitch.extend(ymt.extendByIntegration_constant(motion_stitch, 1, preserveJoints, diff))
# # extend for swing foot ground speed matching & swing foot height lower
## extendedPostures = ymt.extendByIntegration(motion_stitch, 1, preserveJoints, diff)
## extendedPostures = [motion_stitch[-1]]
##
# extendFrameNum = frame - curInterval[1] + 1
# k = 1.-extendFrameNum/5.
# if k<0.: k=0.
# extendedPostures = ymt.extendByIntegrationAttenuation(motion_stitch, 1, preserveJoints, diff, k)
#
## if len(swingFoots)>0 and np.inner(dCM_tar, dCM)>0.:
## print frame, 'speed matching'
## R_swf = motion_stitch[-1].getJointOrientationGlobal(swingFoots[0])
##
## p_swf = motion_stitch[-1].getJointPositionGlobal(swingFoots[0])
## v_swf = motion_stitch.getJointVelocityGlobal(swingFoots[0], frame-diff, frame)
## a_swf = motion_stitch.getJointAccelerationGlobal(swingFoots[0], frame-diff, frame)
## p_swf += v_swf * (frameTime) + a_swf * (frameTime)*(frameTime)
## aik.ik_analytic(extendedPostures[0], swingFoots[0], p_swf)
##
## extendedPostures[0].setJointOrientationGlobal(swingFoots[0], R_swf)
#
# motion_stitch.extend(extendedPostures)
extended[0] = True
else:
if frame == len(motion_seg)-1: lastFrame = True
if lastFrame:
if segIndex < len(segments)-1:
print '%d (%d): end of %dth seg (%s, %s)'%(frame, frame-curInterval[1],segIndex, yba.GaitState.text[curState], curInterval)
if plot!=None: plot.addDataPoint('diff', frame, (frame-curInterval[1])*.01)
if len(stanceFoots)>0 and len(swingFoots)>0:
# step_cur = controlModel.getJointPositionGlobal(swingFoots[0]) - controlModel.getJointPositionGlobal(stanceFoots[0])
# step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(swingFoots[0]) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
step_cur = controlModel.getJointPositionGlobal(0) - controlModel.getJointPositionGlobal(stanceFoots[0])
step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(0) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
step_cur = mm.projectionOnPlane(step_cur, (1,0,0), (0,0,1))
step_tar = mm.projectionOnPlane(step_tar, (1,0,0), (0,0,1))
step_cur_sag, step_cur_cor = mm.projectionOnVector2(step_cur, direction)
step_tar_sag, step_tar_cor = mm.projectionOnVector2(step_tar, direction)
step_length_tar[0] = mm.length(step_tar_sag)
if np.inner(step_tar_sag, step_cur_sag) > 0:
step_length_cur[0] = mm.length(step_cur_sag)
else:
step_length_cur[0] = -mm.length(step_cur_sag)
step_axis[0] = directionAxis
# rd_vec1[0] = step_tar_sag
# rd_vecori1[0] = motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
# rd_vec2[0] = step_cur_sag
# rd_vecori2[0] = controlModel.getJointPositionGlobal(stanceFoots[0])
seg_index[0] += 1
curSeg = segments[seg_index[0]]
stl_y_limit_num[0] = 0
stl_xz_limit_num[0] = 0
del motion_seg_orig[frame+1:]
motion_seg_orig.extend(ymb.getAttachedNextMotion(curSeg, motion_seg_orig[-1], False, False))
del motion_seg[frame+1:]
del motion_stitch[frame+1:]
transitionLength = len(curSeg)-1
# motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, False))
# motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, False))
d = motion_seg[-1] - curSeg[0]
d.rootPos[1] = 0.
motion_seg.extend(ymb.getAttachedNextMotion(curSeg, d, True, False))
d = motion_control[-1] - curSeg[0]
d.rootPos[1] = 0.
motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, d, transitionLength, stitch_func, True, False))
# motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, True))
# motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, True))
else:
motion_seg_orig.append(motion_seg_orig[-1])
motion_seg.append(motion_seg[-1])
motion_stitch.append(motion_control[-1])
# rendering
motionModel.update(motion_ori[frame])
# motionModel.update(motion_seg[frame])
rd_CP[0] = CP
rd_CMP[0] = (CMreal[0] - (F[0]/F[1])*CMreal[1], 0, CMreal[2] - (F[2]/F[1])*CMreal[1])
if plot!=None:
plot.addDataPoint('zero', frame, 0)
plot.updatePoints()
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
motionName = 'wd2_stand.bvh'
elif MOTION == STAND2:
motionName = 'ww13_41_V001.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == TAEKWONDO2:
motionName = './MotionFile/wd2_098_V002.bvh'
#motionName = 'ww13_41_V001.bvh'
motion = yf.readBvhFile(motionName, .01)
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
if FOOT_PART_NUM == 1 or FOOT_PART_NUM == 5 or FOOT_PART_NUM == 7:
yme.removeJoint(motion, RIGHT_TOES, False)
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False)
#yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(2.5,-0.0,.3), -.5), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(2.5,0.0,-.3), -.5), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(2.5, -0.0, .3), -.5),
False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(2.5,0.0,-.3), -.5), False)
if MOTION == FORWARD_JUMP:
yme.rotateJointLocal(motion, LEFT_UP_LEG,
mm.exp(mm.v3(0.0, .0, 1.), .08), False)
yme.rotateJointLocal(motion, LEFT_LEG, mm.exp(mm.v3(0.0, 1.0, 0.),
-.2), False)
if FOOT_PART_NUM > 1 and FOOT_PART_NUM < 5:
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TARSUS)
yme.addJoint(motion, RIGHT_TARSUS, 'RIGHT_Dummy1')
yme.addJoint(motion, LEFT_FOOT, LEFT_TARSUS)
yme.addJoint(motion, LEFT_TARSUS, 'LEFT_Dummy1')
yme.rotateJointLocal(motion, LEFT_TOES, mm.exp(mm.v3(1., 0.0, 0.0),
.45), False)
yme.rotateJointLocal(motion, RIGHT_TOES,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_TARSUS,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
yme.rotateJointLocal(motion, RIGHT_TARSUS,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
if FOOT_PART_NUM == 4:
yme.addJoint(motion, LEFT_FOOT, LEFT_TOES_SIDE_R)
yme.addJoint(motion, LEFT_TOES_SIDE_R, 'LEFT_Dummy2')
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TOES_SIDE_R)
yme.addJoint(motion, RIGHT_TOES_SIDE_R, 'RIGHT_Dummy2')
yme.rotateJointLocal(motion, LEFT_TOES_SIDE_R,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, RIGHT_TOES_SIDE_R,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
elif FOOT_PART_NUM == 5:
sibIndex = motion[0].skeleton.getElementIndex(RIGHT_FOOT)
skeleton = motion[0].skeleton
sibJoint = skeleton.getElement(sibIndex)
newOffset = sibJoint.offset.copy()
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS, (0.0, 0., -.1))
yme.addJoint(motion, RIGHT_CALCANEUS, 'RIGHT_Dummy3')
yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_3, (-0.1, 0., 0.0))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_PHALANGE_1, (0.1, 0., .2))
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_Dummy4', (0., 0., .1))
yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3)
#yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_Dummy6')
sibIndex = motion[0].skeleton.getElementIndex(LEFT_FOOT)
skeleton = motion[0].skeleton
sibJoint = skeleton.getElement(sibIndex)
newOffset = sibJoint.offset.copy()
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS)
yme.addJoint(motion, LEFT_CALCANEUS, 'LEFT_Dummy3')
yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_3)
yme.addJoint(motion, LEFT_FOOT, LEFT_PHALANGE_1)
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_Dummy4')
yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3)
yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_Dummy6')
'''
yme.addJoint(motion, RIGHT_LEG, RIGHT_CALCANEUS, (0.0025, -0.4209, -0.0146))
yme.addJoint(motion, RIGHT_CALCANEUS, 'RIGHT_Dummy3')
yme.addJoint(motion, RIGHT_LEG, RIGHT_METATARSAL_3, (0.0025, -0.4209, -0.0146))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_PHALANGE_1)
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_Dummy4')
yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3)
yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_Dummy6')
sibIndex = motion[0].skeleton.getElementIndex(LEFT_FOOT)
skeleton = motion[0].skeleton
sibJoint = skeleton.getElement(sibIndex)
newOffset = sibJoint.offset.copy()
yme.addJoint(motion, LEFT_LEG, LEFT_CALCANEUS, (-0.0025, -0.4209, -0.0146))
yme.addJoint(motion, LEFT_CALCANEUS, 'LEFT_Dummy3')
yme.addJoint(motion, LEFT_LEG, LEFT_METATARSAL_3, (-0.0025, -0.4209, -0.0146))
yme.addJoint(motion, LEFT_FOOT, LEFT_PHALANGE_1)
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_Dummy4')
yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3)
yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_Dummy6')
'''
'''
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(1.,0.0,0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_3, mm.exp(mm.v3(1.,0.0,0.0), .45), False)
yme.rotateJointLocal(motion, RIGHT_PHALANGE_1, mm.exp(mm.v3(1.,0.0,0.0), .0), False)
yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(1.,0.0,0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS, mm.exp(mm.v3(1.,0.0,0.0), .45), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS, mm.exp(mm.v3(1.,0.0,0.0), .45), False)
'''
elif FOOT_PART_NUM == 7:
sibIndex = motion[0].skeleton.getElementIndex(RIGHT_FOOT)
skeleton = motion[0].skeleton
sibJoint = skeleton.getElement(sibIndex)
newOffset = sibJoint.offset.copy()
print(newOffset)
yme.addJoint(motion, RIGHT_LEG, RIGHT_CALCANEUS,
(0.0025, -0.4209, -0.0146))
yme.addJoint(motion, RIGHT_CALCANEUS, 'RIGHT_Dummy3')
yme.addJoint(motion, RIGHT_LEG, RIGHT_METATARSAL_2,
(0.0025, -0.4209, -0.0146))
yme.addJoint(motion, RIGHT_LEG, RIGHT_METATARSAL_3,
(0.0025, -0.4209, -0.0146))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_PHALANGE_1)
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_Dummy4')
yme.addJoint(motion, RIGHT_METATARSAL_2, RIGHT_PHALANGE_2)
yme.addJoint(motion, RIGHT_PHALANGE_2, 'RIGHT_Dummy5')
yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3)
yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_Dummy6')
sibIndex = motion[0].skeleton.getElementIndex(LEFT_FOOT)
skeleton = motion[0].skeleton
sibJoint = skeleton.getElement(sibIndex)
newOffset = sibJoint.offset.copy()
yme.addJoint(motion, LEFT_LEG, LEFT_CALCANEUS,
(-0.0025, -0.4209, -0.0146))
yme.addJoint(motion, LEFT_CALCANEUS, 'LEFT_Dummy3')
yme.addJoint(motion, LEFT_LEG, LEFT_METATARSAL_2,
(-0.0025, -0.4209, -0.0146))
yme.addJoint(motion, LEFT_LEG, LEFT_METATARSAL_3,
(-0.0025, -0.4209, -0.0146))
yme.addJoint(motion, LEFT_FOOT, LEFT_PHALANGE_1)
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_Dummy4')
yme.addJoint(motion, LEFT_METATARSAL_2, LEFT_PHALANGE_2)
yme.addJoint(motion, LEFT_PHALANGE_2, 'LEFT_Dummy5')
yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3)
yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_Dummy6')
#yme.offsetJointLocal(motion, RIGHT_CALCANEUS, (0.0,-1.55,0), False)
#yme.offsetJointLocal(motion, RIGHT_METATARSAL_2, (.0,-1.55,0), False)
#yme.offsetJointLocal(motion, RIGHT_METATARSAL_3, (.0,-1.55,0), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_2,
mm.exp(mm.v3(-1., 0.0, 0.0), .3), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_2,
mm.exp(mm.v3(-1., 0.0, 0.0), .3), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3,
mm.exp(mm.v3(-1., 0.0, 0.0), .3), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_3,
mm.exp(mm.v3(-1., 0.0, 0.0), .3), False)
yme.rotateJointLocal(motion, RIGHT_PHALANGE_1,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_PHALANGE_1,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS,
mm.exp(mm.v3(-1., 0.0, 0.0), .20), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS,
mm.exp(mm.v3(-1., 0.0, 0.0), .20), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
elif MOTION == TAEKWONDO2:
## Taekwondo base-step
motion = motion[0:31 + 40]
## Taekwondo turning-kick
#motion = motion[108:-1]
#motion = motion[108:109]
motion[0:0] = [motion[0]] * 100
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
#node.length = .2 ####
if FOOT_PART_NUM == 1:
length1 = .25
width1 = .2
mass1 = 4.
elif FOOT_PART_NUM == 3:
length1 = .1
width1 = .2
mass1 = 1.5
length2 = .1
width2 = .2
mass2 = 1.5
elif FOOT_PART_NUM == 4:
length1 = .1
width1 = .2
mass1 = 1.5
length2 = .1
width2 = .095
mass2 = .75
elif FOOT_PART_NUM == 5:
mass0 = .4
width0 = 0.028
#Metatarsal1
length1 = .18
width1 = width0 * 3
mass1 = mass0 * 2.8
#Metatarsal3
length3 = .17
width3 = width0 * 3
mass3 = mass0 * 2.7
#Calcaneus
length4 = .08
width4 = 0.15
mass4 = mass0 * 2.3
#Phalange1
length5 = .06
width5 = width1
mass5 = mass0 * 0.9
#Phalange3
length7 = .05
width7 = width3
mass7 = mass0 * 0.7
elif FOOT_PART_NUM == 7:
mass0 = .4
width0 = 0.028
#Metatarsal1
length1 = .2
width1 = width0 * 2
mass1 = mass0 * 2
#Metatarsal2
length2 = .2
width2 = width0
mass2 = mass0
#Metatarsal3
length3 = .19
width3 = width0 * 3
mass3 = mass0 * 2.8
#Calcaneus
length4 = .08
width4 = 0.15
mass4 = mass0 * 2
#Phalange1
length5 = .06
width5 = width1
mass5 = mass0 / 2.
#Phalange2
length6 = .06
width6 = width2
mass6 = mass0 / 4.
#Phalange3
length7 = .05
width7 = width3
mass7 = mass6 * 2.8
if FOOT_PART_NUM == 7:
node = mcfg.getNode(RIGHT_FOOT)
node.offset = (0.055, -0.03, 0.04)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_METATARSAL_2)
node.offset = (0.01, -0.03, 0.065)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(RIGHT_METATARSAL_3)
node.offset = (-.05, -0.03, 0.055)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_CALCANEUS)
node.offset = (.0, -0.03, -0.07)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.offset = (0.055, -0.03, 0.19)
node.length = length5
node.width = width5
node.mass = mass5
node = mcfg.getNode(RIGHT_PHALANGE_2)
node.offset = (0.01, -0.03, 0.19)
node.length = length6
node.width = width6
node.mass = mass6
node = mcfg.getNode(RIGHT_PHALANGE_3)
node.offset = (-.05, -0.03, 0.18)
node.length = length7
node.width = width7
node.mass = mass7
node = mcfg.getNode(LEFT_FOOT)
node.offset = (-0.055, -0.03, 0.04)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_METATARSAL_2)
node.offset = (-0.01, -0.03, 0.065)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(LEFT_METATARSAL_3)
node.offset = (.05, -0.03, 0.055)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_CALCANEUS)
node.offset = (.0, -0.03, -0.07)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_PHALANGE_1)
node.offset = (-0.055, -0.03, 0.19)
node.length = length5
node.width = width5
node.mass = mass5
node = mcfg.getNode(LEFT_PHALANGE_2)
node.offset = (-0.01, -0.03, 0.19)
node.length = length6
node.width = width6
node.mass = mass6
node = mcfg.getNode(LEFT_PHALANGE_3)
node.offset = (.05, -0.03, 0.18)
node.length = length7
node.width = width7
node.mass = mass7
elif FOOT_PART_NUM == 5:
'''
node = mcfg.getNode(RIGHT_FOOT)
node.offset = (0.05,-0.03,0.06)
node.length = length1
node.width = width1
node.mass = mass1
'''
node = mcfg.getNode(RIGHT_METATARSAL_3)
node.offset = (-.04, -0.03, 0.075)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_CALCANEUS)
node.offset = (.0, -0.03, -0.05)
node.length = length4
node.width = width4
node.mass = mass4
#node = mcfg.getNode(RIGHT_PHALANGE_1)
#node.offset = (0.05,-0.03,0.19)
#node.length = length5
#node.width = width5
#node.mass = mass5
node = mcfg.getNode(RIGHT_PHALANGE_3)
node.offset = (-.04, -0.03, 0.18)
node.length = length7
node.width = width7
node.mass = mass7
node = mcfg.getNode(LEFT_FOOT)
node.offset = (-0.05, -0.03, 0.06)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_METATARSAL_3)
node.offset = (.04, -0.03, 0.075)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_CALCANEUS)
node.offset = (.0, -0.03, -0.05)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_PHALANGE_1)
node.offset = (-0.05, -0.03, 0.19)
node.length = length5
node.width = width5
node.mass = mass5
node = mcfg.getNode(LEFT_PHALANGE_3)
node.offset = (.04, -0.03, 0.18)
node.length = length7
node.width = width7
node.mass = mass7
elif FOOT_PART_NUM == 4:
node = mcfg.getNode(LEFT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (-0.05, 0.0, -0.03)
node = mcfg.getNode(LEFT_TOES_SIDE_R)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0.08, 0.0, 0.1)
node = mcfg.getNode(RIGHT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0.05, 0.0, -0.03)
node = mcfg.getNode(RIGHT_TOES_SIDE_R)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (-0.05, 0.0, 0.12)
elif FOOT_PART_NUM == 3:
node = mcfg.getNode(LEFT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.02)
node = mcfg.getNode(RIGHT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.02)
'''
elif FOOT_PART_NUM == 5:
node = mcfg.getNode(LEFT_FOOT_SIDE_L)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0.07,0.0,0.015)
node = mcfg.getNode(LEFT_FOOT_SIDE_R)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (-0.07,0.0,0.015)
node = mcfg.getNode(RIGHT_FOOT_SIDE_L)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0.07,0.0,0.015)
node = mcfg.getNode(RIGHT_FOOT_SIDE_R)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (-0.07,0.0,0.015)
'''
if FOOT_PART_NUM > 1 and FOOT_PART_NUM < 5:
node = mcfg.getNode(LEFT_TARSUS)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0, 0.0, -0.08)
node = mcfg.getNode(RIGHT_TARSUS)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0, 0.0, -0.08)
if FOOT_PART_NUM < 5:
node = mcfg.getNode(RIGHT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
if FOOT_PART_NUM == 1:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 1.,
LEFT_FOOT: 1.,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.
}
elif FOOT_PART_NUM == 3:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3,
LEFT_TOES: .3,
RIGHT_TOES: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.5,
LEFT_FOOT: 2.5,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_TOES: .3,
RIGHT_TOES: .3
}
elif FOOT_PART_NUM == 4:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3,
LEFT_TOES: .3,
RIGHT_TOES: .3,
LEFT_TOES_SIDE_R: .3,
RIGHT_TOES_SIDE_R: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.5,
LEFT_FOOT: 2.5,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_TOES: .3,
RIGHT_TOES: .3,
LEFT_TOES_SIDE_R: .3,
RIGHT_TOES_SIDE_R: .3
}
elif FOOT_PART_NUM == 5:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3,
LEFT_METATARSAL_3: .2,
RIGHT_METATARSAL_3: .2,
LEFT_PHALANGE_1: .2,
LEFT_PHALANGE_3: .1,
RIGHT_PHALANGE_1: .2,
RIGHT_PHALANGE_3: .2
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.5,
LEFT_FOOT: 2.5,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_METATARSAL_3: .3,
RIGHT_METATARSAL_3: .3,
LEFT_PHALANGE_1: .3,
LEFT_PHALANGE_3: .3,
RIGHT_PHALANGE_1: .3,
RIGHT_PHALANGE_3: .3
}
elif FOOT_PART_NUM == 7:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3,
LEFT_METATARSAL_2: .2,
LEFT_METATARSAL_3: .2,
RIGHT_METATARSAL_2: .2,
RIGHT_METATARSAL_3: .2,
LEFT_PHALANGE_1: .2,
LEFT_PHALANGE_2: .2,
LEFT_PHALANGE_3: .3,
RIGHT_PHALANGE_1: .2,
RIGHT_PHALANGE_2: .2,
RIGHT_PHALANGE_3: .2
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.,
LEFT_FOOT: 2.,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_METATARSAL_2: .3,
LEFT_METATARSAL_3: .3,
RIGHT_METATARSAL_2: .3,
RIGHT_METATARSAL_3: .3,
LEFT_PHALANGE_1: .3,
LEFT_PHALANGE_2: .3,
LEFT_PHALANGE_3: .3,
RIGHT_PHALANGE_1: .3,
RIGHT_PHALANGE_2: .3,
RIGHT_PHALANGE_3: .3
}
'''
elif FOOT_PART_NUM == 5:
config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5,
RIGHT_UP_LEG:.1, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3, LEFT_TOES:.3, RIGHT_TOES:.3, LEFT_TARSUS:.3, RIGHT_TARSUS:.3,
LEFT_FOOT_SIDE_L:.3, LEFT_FOOT_SIDE_R:.3, RIGHT_FOOT_SIDE_L:.3, RIGHT_FOOT_SIDE_R:.3}
config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:1., SPINE1:.3, RIGHT_FOOT:2.5, LEFT_FOOT:2.5, HIP:1.,
RIGHT_UP_LEG:1., RIGHT_LEG:1., LEFT_UP_LEG:1., LEFT_LEG:1., LEFT_TOES:.3, RIGHT_TOES:.3, LEFT_TARSUS:.3, RIGHT_TARSUS:.3,
LEFT_FOOT_SIDE_L:.3, LEFT_FOOT_SIDE_R:.3, RIGHT_FOOT_SIDE_L:.3, RIGHT_FOOT_SIDE_R:.3}
'''
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
#config['end'] = 'HIP'
config['end'] = SPINE1
config['const'] = HIP
config['root'] = HIP
config['FootPartNum'] = FOOT_PART_NUM
if FOOT_PART_NUM == 7:
config['FootLPart'] = [
LEFT_FOOT, LEFT_METATARSAL_2, LEFT_METATARSAL_3, LEFT_PHALANGE_1,
LEFT_PHALANGE_2, LEFT_PHALANGE_3, LEFT_CALCANEUS
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_METATARSAL_2, RIGHT_METATARSAL_3,
RIGHT_PHALANGE_1, RIGHT_PHALANGE_2, RIGHT_PHALANGE_3,
RIGHT_CALCANEUS
]
elif FOOT_PART_NUM == 5:
config['FootLPart'] = [
LEFT_FOOT, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_3,
LEFT_CALCANEUS
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1, RIGHT_PHALANGE_3,
RIGHT_CALCANEUS
]
else:
config['FootLPart'] = [
LEFT_FOOT, LEFT_TOES, LEFT_TARSUS, LEFT_TOES_SIDE_R,
LEFT_FOOT_SIDE_L, LEFT_FOOT_SIDE_R
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_TOES_SIDE_R,
RIGHT_FOOT_SIDE_L, RIGHT_FOOT_SIDE_R
]
return motion, mcfg, wcfg, stepsPerFrame, config
def test_joint_pos_vel_acc_funcs_and_tracking():
def getDesiredAngAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt):
ddth_des = [None]*len(th_r)
for i in range(len(th_r)):
ddth_des[i] = Kt*(mm.logSO3(np.dot(th[i].transpose(), th_r[i]))) + Dt*(dth_r[i] - dth[i]) + ddth_r[i]
return ddth_des
def getDesiredAcceleration(p_r, p, v_r, v, a_r, Kt, Dt):
return Kt*(p_r - p) + Dt*(v_r - v) + a_r
# bvhFilePath = '../samples/chain_1.bvh'
# bvhFilePath = '../samples/block_tree_rotate.bvh'
bvhFilePath = '../samples/chain_3_rotate_freely_move.bvh'
# bvhFilePath = '../samples/chain_3_rotate_freely.bvh'
# bvhFilePath = '../samples/chain_3_rotate_freely_expt_root.bvh'
# bvhFilePath = '../samples/chain_3_rotate.bvh'
# bvhFilePath = '../samples/chain_3_rotate_expt_root.bvh'
# bvhFilePath = '../samples/chain_6_rotate_expt_root.bvh'
# bvhFilePath = '../samples/chain_2_rotate_2axes.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .8
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = -1.
wcfg.gravity = (0,0,0)
stepsPerFrame = 30
wcfg.timeStep = (1/30.)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics(False)
# controlModel.fixBody(0)
controlModel.rotate(mm.exp(mm.v3(0,1,0)))
print controlModel
motion_p = []; motion_v = []; motion_a = []
motion_ap = []; motion_av = []; motion_aa = []
motion_ap_local = []; motion_av_local = []; motion_aa_local = []
model_p = []; model_v = []; model_a = []
model_ap = []; model_av = []; model_aa = []
model_ap_local = []; model_av_local = []; model_aa_local = []
model_body_p = []; model_body_a = []
prev_model_v = [(0.,0.,0.)]*controlModel.getJointNum()
prev_model_av = [(0.,0.,0.)]*controlModel.getJointNum()
prev_model_av_local = [(0.,0.,0.)]*controlModel.getJointNum()
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('model', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
# viewer.doc.addRenderer('motion_p', yr.PointsRenderer(motion_p, (255,0,0)))
# viewer.doc.addRenderer('model_p', yr.PointsRenderer(model_p, (0,255,0)))
#
# viewer.doc.addRenderer('motion_v', yr.VectorsRenderer(motion_v, motion_p, (255,0,0)))
# viewer.doc.addRenderer('model_v', yr.VectorsRenderer(model_v, model_p, (0,255,0)))
viewer.doc.addRenderer('motion_a', yr.VectorsRenderer(motion_a, motion_p, (255,0,0)))
viewer.doc.addRenderer('model_a', yr.VectorsRenderer(model_a, model_p, (0,255,0)))
# viewer.doc.addRenderer('model_body_a', yr.VectorsRenderer(model_body_a, model_body_p, (255,255,0)))
# viewer.doc.addRenderer('motion_ap', yr.OrientationsRenderer(motion_ap, motion_p, (255,0,0)))
# viewer.doc.addRenderer('model_ap', yr.OrientationsRenderer(model_ap, model_p, (0,255,0)))
# viewer.doc.addRenderer('motion_ap_local', yr.OrientationsRenderer(motion_ap_local, motion_p, (255,100,100)))
# viewer.doc.addRenderer('model_ap_local', yr.OrientationsRenderer(model_ap_local, model_p, (100,255,100)))
# viewer.doc.addRenderer('motion_av', yr.VectorsRenderer(motion_av, motion_p, (255,0,0)))
# viewer.doc.addRenderer('model_av', yr.VectorsRenderer(model_av, model_p, (0,255,0)))
# viewer.doc.addRenderer('motion_av_local', yr.VectorsRenderer(motion_av_local, motion_p, (255,100,100)))
# viewer.doc.addRenderer('model_av_local', yr.VectorsRenderer(model_av_local, model_p, (100,255,100)))
# viewer.doc.addRenderer('motion_aa', yr.VectorsRenderer(motion_aa, motion_p, (255,0,0)))
# viewer.doc.addRenderer('model_aa', yr.VectorsRenderer(model_aa, model_p, (0,255,0)))
# viewer.doc.addRenderer('motion_aa_local', yr.VectorsRenderer(motion_aa_local, motion_p, (255,100,100)))
# viewer.doc.addRenderer('model_aa_local', yr.VectorsRenderer(model_aa_local, model_p, (100,255,100)))
Kt = 200; Dt = 2*(Kt**.5)
def simulateCallback(frame):
th_r = motion.getJointOrientationsLocal(frame)
th = controlModel.getJointOrientationsLocal()
dth_r = motion.getJointAngVelocitiesLocal(frame)
dth = controlModel.getJointAngVelocitiesLocal()
ddth_r = motion.getJointAngAccelerationsLocal(frame)
ddth_des = getDesiredAngAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
p_r = motion.getJointPositionGlobal(0, frame)
p = controlModel.getJointPositionGlobal(0)
v_r = motion.getJointVelocityGlobal(0, frame)
v = controlModel.getJointVelocityGlobal(0)
a_r = motion.getJointAccelerationGlobal(0, frame)
a_des = getDesiredAcceleration(p_r, p, v_r, v, a_r, Kt, Dt)
th_r0 = motion.getJointOrientationGlobal(0, frame)
th0 = controlModel.getJointOrientationGlobal(0)
dth_r0 = motion.getJointAngVelocityGlobal(0, frame)
dth0 = controlModel.getJointAngVelocityGlobal(0)
ddth_r0 = motion.getJointAngAccelerationGlobal(0, frame)
ddth_des0 = getDesiredAngAccelerations([th_r0], [th0], [dth_r0], [dth0], [ddth_r0], Kt, Dt)[0]
for i in range(stepsPerFrame):
# controlModel.setBodyAccelerationGlobal(0, a_des)
# controlModel.setJointAngAccelerationsLocal(ddth_des)
controlModel.setJointAccelerationGlobal(0, a_des)
controlModel.setJointAngAccelerationGlobal(0, ddth_des0)
controlModel.setInternalJointAngAccelerationsLocal(ddth_des[1:])
controlModel.solveHybridDynamics()
vpWorld.step()
motion_p[:] = motion.getJointPositionsGlobal(frame)
motion_v[:] = motion.getJointVelocitiesGlobal(frame)
motion_a[:] = motion.getJointAccelerationsGlobal(frame)
motion_ap[:] = motion.getJointOrientationsGlobal(frame)
motion_av[:] = motion.getJointAngVelocitiesGlobal(frame)
motion_aa[:] = motion.getJointAngAccelerationsGlobal(frame)
motion_ap_local[:] = motion.getJointOrientationsLocal(frame)
motion_av_local[:] = motion.getJointAngVelocitiesLocal(frame)
motion_aa_local[:] = motion.getJointAngAccelerationsLocal(frame)
model_p[:] = controlModel.getJointPositionsGlobal()
model_v[:] = controlModel.getJointVelocitiesGlobal()
# model_a[:] = controlModel.getJointAccelerationsGlobal()
model_a[:] = map(lambda v1,v0: (v1-v0)/(1/30.), model_v, prev_model_v)
prev_model_v[:] = model_v
model_ap[:] = controlModel.getJointOrientationsGlobal()
model_av[:] = controlModel.getJointAngVelocitiesGlobal()
# model_aa[:] = controlModel.getJointAngAccelerationsGlobal()
model_aa[:] = map(lambda v1,v0: (v1-v0)/(1/30.), model_av, prev_model_av)
prev_model_av[:] = model_av
model_ap_local[:] = controlModel.getJointOrientationsLocal()
model_av_local[:] = controlModel.getJointAngVelocitiesLocal()
# model_aa_local[:] = controlModel.getJointAngAccelerationsLocal()
model_aa_local[:] = map(lambda v1,v0: (v1-v0)/(1/30.), model_av_local, prev_model_av_local)
prev_model_av_local[:] = model_av_local
model_body_p[:] = controlModel.getBodyPositionsGlobal()
model_body_a[:] = controlModel.getBodyAccelerationsGlobal()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def simulateCallback(frame):
# seginfo
segIndex = seg_index[0]
curState = seginfo[segIndex]['state']
curInterval = yma.offsetInterval(acc_offset[0], seginfo[segIndex]['interval'])
stanceLegs = seginfo[segIndex]['stanceHips']
swingLegs = seginfo[segIndex]['swingHips']
stanceFoots = seginfo[segIndex]['stanceFoots']
swingFoots = seginfo[segIndex]['swingFoots']
swingKnees = seginfo[segIndex]['swingKnees']
groundHeight = seginfo[segIndex]['ground_height']
maxStfPushFrame = seginfo[segIndex]['max_stf_push_frame']
prev_frame = frame-1 if frame>0 else 0
# prev_frame = frame
# information
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, upperMass, uppers)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, upperMass, uppers)
## dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, totalMass)
## CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, totalMass)
# stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], frame)
# CMr_tar = CM_tar - stf_tar
dCM_tar = motion_seg.getJointVelocityGlobal(0, prev_frame)
CM_tar = motion_seg.getJointPositionGlobal(0, prev_frame)
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, upperMass, uppers)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, upperMass, uppers)
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, totalMass)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, totalMass)
stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], prev_frame)
CMr_tar = CM_tar - stf_tar
dCM = avg_dCM[0]
CM = controlModel.getJointPositionGlobal(0)
# CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, upperMass, uppers)
# CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass)
CMreal = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass)
stf = controlModel.getJointPositionGlobal(stanceFoots[0])
CMr = CM - stf
diff_dCM = mm.projectionOnPlane(dCM-dCM_tar, (1,0,0), (0,0,1))
diff_dCM_axis = np.cross((0,1,0), diff_dCM)
rd_vec1[0] = diff_dCM; rd_vecori1[0] = CM_tar
diff_CMr = mm.projectionOnPlane(CMr-CMr_tar, (1,0,0), (0,0,1))
# rd_vec1[0] = diff_CMr; rd_vecori1[0] = stf_tar
diff_CMr_axis = np.cross((0,1,0), diff_CMr)
direction = mm.normalize2(mm.projectionOnPlane(dCM_tar, (1,0,0), (0,0,1)))
# direction = mm.normalize2(mm.projectionOnPlane(dCM, (1,0,0), (0,0,1)))
directionAxis = np.cross((0,1,0), direction)
diff_dCM_sag, diff_dCM_cor = mm.projectionOnVector2(diff_dCM, direction)
# rd_vec1[0] = diff_dCM_sag; rd_vecori1[0] = CM_tar
diff_dCM_sag_axis = np.cross((0,1,0), diff_dCM_sag)
diff_dCM_cor_axis = np.cross((0,1,0), diff_dCM_cor)
diff_CMr_sag, diff_CMr_cor = mm.projectionOnVector2(diff_CMr, direction)
diff_CMr_sag_axis = np.cross((0,1,0), diff_CMr_sag)
diff_CMr_cor_axis = np.cross((0,1,0), diff_CMr_cor)
t = (frame-curInterval[0])/float(curInterval[1]-curInterval[0])
t_raw = t
if t>1.: t=1.
p_root = motion_stitch[frame].getJointPositionGlobal(0)
R_root = motion_stitch[frame].getJointOrientationGlobal(0)
motion_seg_orig.goToFrame(frame)
motion_seg.goToFrame(frame)
motion_stitch.goToFrame(frame)
motion_debug1.append(motion_stitch[frame].copy())
motion_debug1.goToFrame(frame)
motion_debug2.append(motion_stitch[frame].copy())
motion_debug2.goToFrame(frame)
motion_debug3.append(motion_stitch[frame].copy())
motion_debug3.goToFrame(frame)
# paper implementation
M_tc.append(motion_stitch[prev_frame])
M_tc.goToFrame(frame)
P_hat.append(M_tc[frame].copy())
P_hat.goToFrame(frame)
p_temp = ym.JointPosture(skeleton)
p_temp.rootPos = controlModel.getJointPositionGlobal(0)
p_temp.setJointOrientationsLocal(controlModel.getJointOrientationsLocal())
P.append(p_temp)
P.goToFrame(frame)
# stance foot stabilize
motion_stf_stabilize.append(motion_stitch[frame].copy())
motion_stf_stabilize.goToFrame(frame)
if STANCE_FOOT_STABILIZE:
for stanceFoot in stanceFoots:
R_target_foot = motion_seg[frame].getJointOrientationGlobal(stanceFoot)
R_current_foot = motion_stf_stabilize[frame].getJointOrientationGlobal(stanceFoot)
motion_stf_stabilize[frame].setJointOrientationGlobal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t)))
# R_target_foot = motion_seg[frame].getJointOrientationLocal(stanceFoot)
# R_current_foot = motion_stf_stabilize[frame].getJointOrientationLocal(stanceFoot)
# motion_stf_stabilize[frame].setJointOrientationLocal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t)))
# match stance leg
motion_match_stl.append(motion_stf_stabilize[frame].copy())
motion_match_stl.goToFrame(frame)
if MATCH_STANCE_LEG:
if curState!=yba.GaitState.STOP:
for i in range(len(stanceLegs)):
stanceLeg = stanceLegs[i]
stanceFoot = stanceFoots[i]
# # motion stance leg -> character stance leg as time goes
R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg)
R_character = controlModel.getJointOrientationGlobal(stanceLeg)
motion_match_stl[frame].setJointOrientationGlobal(stanceLeg, cm.slerp(R_motion, R_character, match_stl_func(t)))
# t_y = match_stl_func_y(t)
# t_xz = match_stl_func(t)
#
# R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg)
# R_character = controlModel.getJointOrientationGlobal(stanceLeg)
# R = np.dot(R_character, R_motion.T)
# R_y, R_xz = mm.projectRotation((0,1,0), R)
# motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_xz, t_xz))
# motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_y, t_y))
# swing foot placement
motion_swf_placement.append(motion_match_stl[frame].copy())
motion_swf_placement.goToFrame(frame)
if SWING_FOOT_PLACEMENT:
t_swing_foot_placement = swf_placement_func(t);
if extended[0]:
R_swp_sag = prev_R_swp[0][0]
R_swp_cor = prev_R_swp[0][1]
else:
R_swp_sag = mm.I_SO3(); R_swp_cor = mm.I_SO3()
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag * -t_swing_foot_placement))
R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_dCM_cor_axis * K_swp_vel_cor * -t_swing_foot_placement))
if np.dot(direction, diff_CMr_sag) < 0:
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag * -t_swing_foot_placement))
R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_CMr_cor_axis * K_swp_pos_cor * -t_swing_foot_placement))
for i in range(len(swingLegs)):
swingLeg = swingLegs[i]
swingFoot = swingFoots[i]
# save swing foot global orientation
# R_swf = motion_swf_placement[frame].getJointOrientationGlobal(swingFoot)
# rotate swing leg
motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_sag)
motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_cor)
# restore swing foot global orientation
# motion_swf_placement[frame].setJointOrientationGlobal(swingFoot, R_swf)
prev_R_swp[0] = (R_swp_sag, R_swp_cor)
# swing foot height
motion_swf_height.append(motion_swf_placement[frame].copy())
motion_swf_height.goToFrame(frame)
if SWING_FOOT_HEIGHT:
for swingFoot in swingFoots:
stanceFoot = stanceFoots[0]
# save foot global orientation
R_foot = motion_swf_height[frame].getJointOrientationGlobal(swingFoot)
R_stance_foot = motion_swf_height[frame].getJointOrientationGlobal(stanceFoot)
height_tar = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1]
# motion_debug1[frame] = motion_swf_height[frame].copy()
# rotate
motion_swf_height[frame].rotateByTarget(controlModel.getJointOrientationGlobal(0))
# motion_debug2[frame] = motion_swf_height[frame].copy()
# motion_debug2[frame].translateByTarget(controlModel.getJointPositionGlobal(0))
height_cur = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1]
offset_height = (height_tar - height_cur) * swf_height_func(t) * c_swf_stability
offset_sine = c_swf_mid_offset * swf_height_sine_func(t)
offset = 0.
offset += offset_height
offset += offset_sine
if offset > 0.:
newPosition = motion_swf_height[frame].getJointPositionGlobal(swingFoot)
newPosition[1] += offset
aik.ik_analytic(motion_swf_height[frame], swingFoot, newPosition)
else:
newPosition = motion_swf_height[frame].getJointPositionGlobal(stanceFoot)
newPosition[1] -= offset
aik.ik_analytic(motion_swf_height[frame], stanceFoot, newPosition)
# return
# motion_debug3[frame] = motion_swf_height[frame].copy()
# motion_debug3[frame].translateByTarget(controlModel.getJointPositionGlobal(0))
motion_swf_height[frame].rotateByTarget(R_root)
# restore foot global orientation
motion_swf_height[frame].setJointOrientationGlobal(swingFoot, R_foot)
motion_swf_height[frame].setJointOrientationGlobal(stanceFoot, R_stance_foot)
if plot!=None:
plot.addDataPoint('debug1', frame, height_tar)
plot.addDataPoint('debug2', frame, height_cur)
# plot.addDataPoint('diff', frame, diff)
# swing foot orientation
motion_swf_orientation.append(motion_swf_height[frame].copy())
motion_swf_orientation.goToFrame(frame)
if SWING_FOOT_ORIENTATION:
# swf_orientation_func = yfg.concatenate([yfg.zero, yfg.hermite2nd, yfg.one], [.25, .75])
for swingFoot in swingFoots:
R_target_foot = motion_seg[curInterval[1]].getJointOrientationGlobal(swingFoot)
R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot)
motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot, swf_orientation_func(t)))
# swf_stabilize_func = yfg.concatenate([yfg.hermite2nd, yfg.one], [c_taking_duration])
# push orientation
# for swingFoot in swingFoots:
# R_target_foot = motion_seg[frame].getJointOrientationGlobal(swingFoot)
# R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot)
# motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot , swf_stabilize_func(t)))
# stance foot push
motion_stf_push.append(motion_swf_orientation[frame].copy())
motion_stf_push.goToFrame(frame)
if STANCE_FOOT_PUSH:
for swingFoot in swingFoots:
# max_t = (maxStfPushFrame)/float(curInterval[1]-curInterval[0])
# stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [max_t*2])
stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [c_taking_duration*2])
R_swp_sag = mm.I_SO3()
# R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_stp_vel * -stf_push_func(t)))
# if step_length_cur[0] < step_length_tar[0]:
# ratio = step_length_cur[0] / step_length_tar[0]
# R_max = maxmaxStfPushFrame
# R_zero =
R_swp_sag = np.dot(R_swp_sag, mm.exp((step_length_tar[0] - step_length_cur[0])*step_axis[0] * K_stp_pos * -stf_push_func(t)))
motion_stf_push[frame].mulJointOrientationGlobal(swingFoot, R_swp_sag)
# stance foot balancing
motion_stf_balancing.append(motion_stf_push[frame].copy())
motion_stf_balancing.goToFrame(frame)
if STANCE_FOOT_BALANCING:
R_stb = mm.exp(diff_dCM_axis * K_stb_vel * stf_balancing_func(t))
for stanceFoot in stanceFoots:
if frame < 5: continue
motion_stf_balancing[frame].mulJointOrientationGlobal(stanceFoot, R_stb)
# control trajectory
motion_control.append(motion_stf_balancing[frame].copy())
motion_control.goToFrame(frame)
#=======================================================================
# tracking with inverse dynamics
#=======================================================================
th_r = motion_control.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion_control.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion_control.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
#=======================================================================
# simulation
#=======================================================================
CP = mm.v3(0.,0.,0.)
F = mm.v3(0.,0.,0.)
avg_dCM[0] = mm.v3(0.,0.,0.)
# external force rendering info
del rd_forces[:]; del rd_force_points[:]
for fi in forceInfos:
if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime):
rd_forces.append(fi.force)
rd_force_points.append(controlModel.getBodyPositionGlobal(fi.targetBody))
for i in range(stepsPerFrame):
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces)
# apply external force
for fi in forceInfos:
if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime):
controlModel.applyBodyForceGlobal(fi.targetBody, fi.force)
controlModel.setDOFAccelerations(ddth_des)
controlModel.solveHybridDynamics()
if TORQUE_PLOT:
rhip_torques[frame] += mm.length(controlModel.getJointTorqueLocal(rUpLeg))
rknee_torques[frame] += mm.length(controlModel.getJointTorqueLocal(rKnee))
rankle_torques[frame] += mm.length(controlModel.getJointTorqueLocal(rFoot))
rd_torques[:] = [controlModel.getJointTorqueLocal(i)/100. for i in range(skeleton.getJointNum())]
rd_joint_positions[:] = controlModel.getJointPositionsGlobal()
vpWorld.step()
# yvu.align2D(controlModel)
if len(contactForces) > 0:
CP += yrp.getCP(contactPositions, contactForces)
F += sum(contactForces)
avg_dCM[0] += controlModel.getJointVelocityGlobal(0)
# avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, upperMass, uppers)
# avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, totalMass)
# if len(stanceFoots)>0:
# avg_stf_v[0] += controlModel.getJointVelocityGlobal(stanceFoots[0])
# avg_stf_av[0] += controlModel.getJointAngVelocityGlobal(stanceFoots[0])
CP /= stepsPerFrame
F /= stepsPerFrame
avg_dCM[0] /= stepsPerFrame
# if len(stanceFoots)>0:
# avg_stf_v[0] /= stepsPerFrame
# avg_stf_av[0] /= stepsPerFrame
# rd_vec1[0] = avg_stf_av[0]; rd_vec1[0][0] = 0.; rd_vec1[0][2] = 0.
# rd_vecori1[0]= controlModel.getJointPositionGlobal(stanceFoots[0])
#=======================================================================
# segment editing
#=======================================================================
lastFrame = False
if SEGMENT_EDITING:
if curState==yba.GaitState.STOP:
if frame == len(motion_seg)-1:
lastFrame = True
elif (curState==yba.GaitState.LSWING or curState==yba.GaitState.RSWING) and t>c_min_contact_time:
swingID = lID if curState==yba.GaitState.LSWING else rID
contact = False
if swingID in bodyIDs:
minContactVel = 1000.
for i in range(len(bodyIDs)):
if bodyIDs[i]==swingID:
vel = controlModel.getBodyVelocityGlobal(swingID, contactPositionLocals[i])
vel[1] = 0
contactVel = mm.length(vel)
if contactVel < minContactVel: minContactVel = contactVel
if minContactVel < c_min_contact_vel: contact = True
extended[0] = False
if contact:
# print frame, 'foot touch'
lastFrame = True
acc_offset[0] += frame - curInterval[1]
elif frame == len(motion_seg)-1:
print frame, 'extend frame', frame+1
preserveJoints = []
# preserveJoints = [lFoot, rFoot]
# preserveJoints = [lFoot, rFoot, lKnee, rKnee]
# preserveJoints = [lFoot, rFoot, lKnee, rKnee, lUpLeg, rUpLeg]
stanceKnees = [rKnee] if curState==yba.GaitState.LSWING else [lKnee]
preserveJoints = [stanceFoots[0], stanceKnees[0], stanceLegs[0]]
diff = 3
motion_seg_orig.extend([motion_seg_orig[-1]])
motion_seg.extend(ymt.extendByIntegration_root(motion_seg, 1, diff))
motion_stitch.extend(ymt.extendByIntegration_constant(motion_stitch, 1, preserveJoints, diff))
# # extend for swing foot ground speed matching & swing foot height lower
## extendedPostures = ymt.extendByIntegration(motion_stitch, 1, preserveJoints, diff)
## extendedPostures = [motion_stitch[-1]]
##
# extendFrameNum = frame - curInterval[1] + 1
# k = 1.-extendFrameNum/5.
# if k<0.: k=0.
# extendedPostures = ymt.extendByIntegrationAttenuation(motion_stitch, 1, preserveJoints, diff, k)
#
## if len(swingFoots)>0 and np.inner(dCM_tar, dCM)>0.:
## print frame, 'speed matching'
## R_swf = motion_stitch[-1].getJointOrientationGlobal(swingFoots[0])
##
## p_swf = motion_stitch[-1].getJointPositionGlobal(swingFoots[0])
## v_swf = motion_stitch.getJointVelocityGlobal(swingFoots[0], frame-diff, frame)
## a_swf = motion_stitch.getJointAccelerationGlobal(swingFoots[0], frame-diff, frame)
## p_swf += v_swf * (frameTime) + a_swf * (frameTime)*(frameTime)
## aik.ik_analytic(extendedPostures[0], swingFoots[0], p_swf)
##
## extendedPostures[0].setJointOrientationGlobal(swingFoots[0], R_swf)
#
# motion_stitch.extend(extendedPostures)
extended[0] = True
else:
if frame == len(motion_seg)-1: lastFrame = True
if lastFrame:
if segIndex < len(segments)-1:
print '%d (%d): end of %dth seg (%s, %s)'%(frame, frame-curInterval[1],segIndex, yba.GaitState.text[curState], curInterval)
if plot!=None: plot.addDataPoint('diff', frame, (frame-curInterval[1])*.01)
if len(stanceFoots)>0 and len(swingFoots)>0:
# step_cur = controlModel.getJointPositionGlobal(swingFoots[0]) - controlModel.getJointPositionGlobal(stanceFoots[0])
# step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(swingFoots[0]) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
step_cur = controlModel.getJointPositionGlobal(0) - controlModel.getJointPositionGlobal(stanceFoots[0])
step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(0) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
step_cur = mm.projectionOnPlane(step_cur, (1,0,0), (0,0,1))
step_tar = mm.projectionOnPlane(step_tar, (1,0,0), (0,0,1))
step_cur_sag, step_cur_cor = mm.projectionOnVector2(step_cur, direction)
step_tar_sag, step_tar_cor = mm.projectionOnVector2(step_tar, direction)
step_length_tar[0] = mm.length(step_tar_sag)
if np.inner(step_tar_sag, step_cur_sag) > 0:
step_length_cur[0] = mm.length(step_cur_sag)
else:
step_length_cur[0] = -mm.length(step_cur_sag)
step_axis[0] = directionAxis
# rd_vec1[0] = step_tar_sag
# rd_vecori1[0] = motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
# rd_vec2[0] = step_cur_sag
# rd_vecori2[0] = controlModel.getJointPositionGlobal(stanceFoots[0])
seg_index[0] += 1
curSeg = segments[seg_index[0]]
stl_y_limit_num[0] = 0
stl_xz_limit_num[0] = 0
del motion_seg_orig[frame+1:]
motion_seg_orig.extend(ymb.getAttachedNextMotion(curSeg, motion_seg_orig[-1], False, False))
del motion_seg[frame+1:]
del motion_stitch[frame+1:]
transitionLength = len(curSeg)-1
# motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, False))
# motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, False))
d = motion_seg[-1] - curSeg[0]
d.rootPos[1] = 0.
motion_seg.extend(ymb.getAttachedNextMotion(curSeg, d, True, False))
if NO_FOOT_SLIDING:
if segIndex == len(segments)-2:
Rl = motion_control[-1].getJointOrientationLocal(lUpLeg)
Rr = motion_control[-1].getJointOrientationLocal(rUpLeg)
Rlk = motion_control[-1].getJointOrientationLocal(lKnee)
Rrk = motion_control[-1].getJointOrientationLocal(rKnee)
Rlf = motion_control[-1].getJointOrientationLocal(lFoot)
Rrf = motion_control[-1].getJointOrientationLocal(rFoot)
for p in curSeg:
p.setJointOrientationLocal(lUpLeg, Rl, False)
p.setJointOrientationLocal(rUpLeg, Rr, False)
p.setJointOrientationLocal(lKnee, Rlk, False)
p.setJointOrientationLocal(rKnee, Rrk, False)
p.setJointOrientationLocal(lFoot, Rlf, False)
p.setJointOrientationLocal(rFoot, Rrf, False)
p.updateGlobalT()
d = motion_control[-1] - curSeg[0]
d.rootPos[1] = 0.
motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, d, transitionLength, stitch_func, True, False))
# motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, True))
# motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, True))
else:
motion_seg_orig.append(motion_seg_orig[-1])
motion_seg.append(motion_seg[-1])
motion_stitch.append(motion_control[-1])
# rendering
motionModel.update(motion_ori[frame])
# motionModel.update(motion_seg[frame])
if SAVE_SIMULATION:
p_temp = ym.JointPosture(skeleton)
p_temp.initLocalRs()
p_temp.rootPos = controlModel.getJointPositionGlobal(0)
p_temp.setJointOrientationsLocal(controlModel.getJointOrientationsLocal())
motion_simulation.append(p_temp)
if frame == viewer.getMaxFrame():
saveFilePath = SAVE_DIR+'simulated_'+filename
yf.writeBvhFile(saveFilePath, motion_simulation)
print saveFilePath, 'saved'
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
#motionName = 'wd2_stand.bvh'
motionName = 'wd2_stand2.bvh'
elif MOTION == STAND2:
motionName = 'ww13_41_V001.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == TAEKWONDO2:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == KICK:
motionName = 'wd2_n_kick.bvh'
elif MOTION == WALK:
motionName = 'wd2_WalkForwardNormal00.bvh'
elif MOTION == TIPTOE:
motionName = './MotionFile/cmu/15_07_15_07.bvh'
#motionName = 'ww13_41_V001.bvh'
scale = 0.01
if MOTION == WALK:
scale = 1.0
elif MOTION == TIPTOE:
scale = 0.01
motion = yf.readBvhFile(motionName, scale)
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
yme.removeJoint(motion, RIGHT_TOES, False)
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(0, 0, 1), -.01), False)
#addFootSegment
yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_1, (-0.045, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_2, (0.0, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_3,
(0.045, -0.06, -0.05)) #-0.0037
yme.addJoint(motion, LEFT_TALUS_1, LEFT_METATARSAL_1, (0.0, 0.0, 0.1))
yme.addJoint(motion, LEFT_TALUS_3, LEFT_METATARSAL_3,
(0.0, 0.0, 0.1)) #-0.0037
yme.addJoint(motion, LEFT_TALUS_2, LEFT_METATARSAL_2, (0.0, 0.0, 0.1))
yme.addJoint(motion, LEFT_METATARSAL_1, LEFT_PHALANGE_1, (0.0, 0.0, 0.07))
yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3, (0.0, 0.0, 0.07))
yme.addJoint(motion, LEFT_METATARSAL_2, LEFT_PHALANGE_2, (0.0, 0.0, 0.07))
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_PHALANGE_Effector1',
(0.0, 0.0, 0.06))
yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_PHALANGE_Effector3',
(0.0, 0.0, 0.06))
yme.addJoint(motion, LEFT_PHALANGE_2, 'LEFT_PHALANGE_Effector2',
(0.0, 0.0, 0.06))
#yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (-0.045, -0.06, -0.04))
#yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_2, (0.0, -0.06, -0.04))
#yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_3, (0.045, -0.06, -0.04))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (-0.045, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_2, (0.0, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_3, (0.045, -0.06, -0.05))
yme.addJoint(motion, LEFT_CALCANEUS_1, 'LEFT_CALCANEUS_Effector1',
(0., 0.0, -0.07))
yme.addJoint(motion, LEFT_CALCANEUS_2, 'LEFT_CALCANEUS_Effector2',
(0., 0.0, -0.07))
yme.addJoint(motion, LEFT_CALCANEUS_3, 'LEFT_CALCANEUS_Effector3',
(0., 0.0, -0.07))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_1, (0.045, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_2, (0.0, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_3, (-0.045, -0.06, -0.05))
yme.addJoint(motion, RIGHT_TALUS_1, RIGHT_METATARSAL_1, (0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_TALUS_2, RIGHT_METATARSAL_2, (0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_TALUS_3, RIGHT_METATARSAL_3, (0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_METATARSAL_1, RIGHT_PHALANGE_1,
(0.0, 0.0, 0.07))
yme.addJoint(motion, RIGHT_METATARSAL_2, RIGHT_PHALANGE_2,
(0.0, 0.0, 0.07))
yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3,
(0.0, 0.0, 0.07))
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_PHALANGE_Effector1',
(0.0, 0.0, 0.06))
yme.addJoint(motion, RIGHT_PHALANGE_2, 'RIGHT_PHALANGE_Effector2',
(0.0, 0.0, 0.06))
yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_PHALANGE_Effector3',
(0.0, 0.0, 0.06))
#yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1, (0.045, -0.06, -0.04))
#yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_2, (0.0, -0.06, -0.04))
#yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_3, (-0.045, -0.06, -0.04))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1, (0.045, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_2, (0.0, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_3, (-0.045, -0.06, -0.05))
yme.addJoint(motion, RIGHT_CALCANEUS_1, 'RIGHT_CALCANEUS_Effector1',
(0.0, 0.0, -0.07))
yme.addJoint(motion, RIGHT_CALCANEUS_2, 'RIGHT_CALCANEUS_Effector2',
(0.0, 0.0, -0.07))
yme.addJoint(motion, RIGHT_CALCANEUS_3, 'RIGHT_CALCANEUS_Effector3',
(0.0, 0.0, -0.07))
yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5),
False)
yme.rotateJointLocal(motion, RIGHT_TALUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, RIGHT_TALUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, RIGHT_TALUS_3, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_3,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_3,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5),
False)
yme.rotateJointLocal(motion, LEFT_TALUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, LEFT_TALUS_3, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, LEFT_TALUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_2,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_3,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_2,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_3,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
#yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(0.0,-1.0,0.0), 1.57), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
elif MOTION == TAEKWONDO2:
## Taekwondo base-step
#motion = motion[0:31+40]
## Taekwondo turning-kick
motion = motion[108:-1]
#motion = motion[108:109]
elif MOTION == KICK:
#motion = motion[141:-1]
#motion = motion[100:-1]
#motion = motion[58:-1]
motion = motion[82:-1]
#motion = motion[0:-1]
elif MOTION == STAND2:
motion = motion[1:-1]
elif MOTION == TIPTOE:
#motion = motion[183:440]
#motion = motion[350:410]
motion = motion[350:550]
motion[0:0] = [motion[0]] * 40
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
#node.length = .2 ####
node = mcfg.getNode('RightFoot')
node.length = .1
node.width = .1
node = mcfg.getNode('LeftFoot')
node.length = .1
node.width = .1
#return mcfg
#
#mass0 = .4
#width0 = 0.028
#length0 = 0.1
#
##Metatarsal1
#length1 = .12
#width1 = 0.03
#mass1 = 0.4
#
#length2 = .1
#width2 = 0.026
#mass2 = 0.4
#
##Metatarsal3
#length3 = .08
#width3 = 0.024
#mass3 = 0.4
#
##Calcaneus1
#length4 = .1
#width4 = 0.032
#mass4 = 0.4
#
##Phalange1
#length5 = .08
#width5 = 0.01
#mass5 = 0.4
##Phalange3
#length7 = length5
#width7 = width5
#mass7 = mass5
#
##Talus
##length8 = .13
##width8 = width0*3
##mass8 = mass0*2.
#
#length8 = .1
#width8 = width0*3
#mass8 = mass0*1.5
width0 = 0.028
length0 = 0.1
mass0 = .4
#Metatarsal1
length1 = .1
width1 = 0.03
mass1 = 0.4
length2 = length1
width2 = width1
mass2 = 0.4
#Metatarsal3
length3 = length1
width3 = width1
mass3 = 0.4
#Calcaneus1
length4 = length1
width4 = width1
mass4 = 0.4
#Phalange1
length5 = length1
width5 = width1
mass5 = 0.4
#Phalange3
length7 = length1
width7 = width1
mass7 = 0.4
#Talus
#length8 = .13
#width8 = width0*3
#mass8 = mass0*2.
length8 = .1
width8 = width0 * 3
mass8 = mass0 * 1.5
node = mcfg.getNode(RIGHT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(RIGHT_TALUS_1)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_TALUS_3)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_TALUS_2)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_METATARSAL_1)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(RIGHT_METATARSAL_3)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(RIGHT_METATARSAL_2)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_PHALANGE_2)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_PHALANGE_3)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_CALCANEUS_1)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(RIGHT_CALCANEUS_2)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(RIGHT_CALCANEUS_3)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(LEFT_TALUS_1)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_TALUS_3)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_TALUS_2)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_METATARSAL_1)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(LEFT_METATARSAL_3)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(LEFT_METATARSAL_2)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(LEFT_PHALANGE_1)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_PHALANGE_2)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_PHALANGE_3)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_CALCANEUS_1)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_CALCANEUS_2)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_CALCANEUS_3)
node.length = length4
node.width = width4
node.mass = mass4
#node.offset = (0.0, -0.025, 0.0)
node = mcfg.getNode('LeftFoot')
node = mcfg.getNode(RIGHT_TALUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_TALUS_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_TALUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_TALUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_TALUS_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_TALUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_METATARSAL_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_METATARSAL_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_METATARSAL_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_METATARSAL_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_METATARSAL_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_METATARSAL_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_PHALANGE_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_PHALANGE_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_PHALANGE_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_PHALANGE_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_PHALANGE_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_CALCANEUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_CALCANEUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_CALCANEUS_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_CALCANEUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_CALCANEUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_CALCANEUS_3)
node.geom = 'MyFoot3'
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 60
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .5,
SPINE1: .3,
RIGHT_FOOT: .7,
LEFT_FOOT: .7,
HIP: .5,
RIGHT_UP_LEG: .7,
RIGHT_LEG: .7,
LEFT_UP_LEG: .7,
LEFT_LEG: .7,
LEFT_TALUS_1: .7,
RIGHT_TALUS_1: .7,
LEFT_TALUS_2: .7,
RIGHT_TALUS_2: .7,
LEFT_TALUS_3: .7,
RIGHT_TALUS_3: .7,
LEFT_METATARSAL_1: .7,
RIGHT_METATARSAL_1: .7,
LEFT_METATARSAL_2: .7,
RIGHT_METATARSAL_2: .7,
LEFT_METATARSAL_3: .7,
RIGHT_METATARSAL_3: .7,
RIGHT_CALCANEUS_1: .7,
LEFT_CALCANEUS_1: .7,
RIGHT_CALCANEUS_2: .7,
LEFT_CALCANEUS_2: .7,
RIGHT_CALCANEUS_3: .7,
LEFT_CALCANEUS_3: .7,
LEFT_PHALANGE_1: .4,
LEFT_PHALANGE_2: .4,
LEFT_PHALANGE_3: .4,
RIGHT_PHALANGE_1: .4,
RIGHT_PHALANGE_2: .4,
RIGHT_PHALANGE_3: .4
}
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .2,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .3,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .2,
LEFT_UP_LEG: .1,
LEFT_LEG: .2,
LEFT_TALUS_1: .1,
RIGHT_TALUS_1: .1,
LEFT_TALUS_2: .1,
RIGHT_TALUS_2: .1,
LEFT_TALUS_3: .1,
RIGHT_TALUS_3: .1,
LEFT_METATARSAL_1: .1,
RIGHT_METATARSAL_1: .1,
LEFT_METATARSAL_2: .1,
RIGHT_METATARSAL_2: .1,
LEFT_METATARSAL_3: .1,
RIGHT_METATARSAL_3: .1,
RIGHT_CALCANEUS_1: .2,
LEFT_CALCANEUS_1: .2,
RIGHT_CALCANEUS_2: .2,
LEFT_CALCANEUS_2: .2,
RIGHT_CALCANEUS_3: .2,
LEFT_CALCANEUS_3: .2,
LEFT_PHALANGE_1: .1,
LEFT_PHALANGE_2: .1,
LEFT_PHALANGE_3: .1,
RIGHT_PHALANGE_1: .1,
RIGHT_PHALANGE_2: .1,
RIGHT_PHALANGE_3: .1
}
'''
config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.2, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.3,
RIGHT_UP_LEG:.1, RIGHT_LEG:.2, LEFT_UP_LEG:.1, LEFT_LEG:.2,
LEFT_TALUS_1:.01, RIGHT_TALUS_1:.01, LEFT_TALUS_2:.01, RIGHT_TALUS_2:.01, LEFT_TALUS_3:.01, RIGHT_TALUS_3:.01,
LEFT_METATARSAL_1:.01, RIGHT_METATARSAL_1:.01, LEFT_METATARSAL_2:.01, RIGHT_METATARSAL_2:.01, LEFT_METATARSAL_3:.01, RIGHT_METATARSAL_3:.01,
RIGHT_CALCANEUS_1:.01, LEFT_CALCANEUS_1:.01, RIGHT_CALCANEUS_2:.01, LEFT_CALCANEUS_2:.01, RIGHT_CALCANEUS_3:.01, LEFT_CALCANEUS_3:.01,
LEFT_PHALANGE_1:.01, LEFT_PHALANGE_2:.01, LEFT_PHALANGE_3:.01, RIGHT_PHALANGE_1:.01, RIGHT_PHALANGE_2:.01, RIGHT_PHALANGE_3:.01}
'''
#config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.2, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.3,
# RIGHT_UP_LEG:.1, RIGHT_LEG:.2, LEFT_UP_LEG:.1, LEFT_LEG:.2,
# LEFT_TALUS_1:.1, RIGHT_TALUS_1:.1, LEFT_TALUS_3:.1, RIGHT_TALUS_3:.1,
# LEFT_METATARSAL_1:.1, RIGHT_METATARSAL_1:.1, LEFT_METATARSAL_3:.1, RIGHT_METATARSAL_3:.1,
# RIGHT_CALCANEUS_1:.2, LEFT_CALCANEUS_1:.2, RIGHT_CALCANEUS_3:.2, LEFT_CALCANEUS_3:.2,
# LEFT_PHALANGE_1:.1, LEFT_PHALANGE_3:.1, RIGHT_PHALANGE_1:.1, RIGHT_PHALANGE_3:.1}
#
#config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.5, SPINE1:.3, RIGHT_FOOT:.7, LEFT_FOOT:.7, HIP:.5,
# RIGHT_UP_LEG:.7, RIGHT_LEG:.7, LEFT_UP_LEG:.7, LEFT_LEG:.7,
# LEFT_TALUS_1:.7, RIGHT_TALUS_1:.7, LEFT_TALUS_3:.7, RIGHT_TALUS_3:.7,
# LEFT_METATARSAL_1:.7, RIGHT_METATARSAL_1:.7, LEFT_METATARSAL_3:.7, RIGHT_METATARSAL_3:.7,
# RIGHT_CALCANEUS_1:.7, LEFT_CALCANEUS_1:.7, RIGHT_CALCANEUS_3:.7, LEFT_CALCANEUS_3:.7,
# LEFT_PHALANGE_1:.4, LEFT_PHALANGE_3:.4, RIGHT_PHALANGE_1:.4, RIGHT_PHALANGE_3:.4}
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
#config['end'] = HIP
config['end'] = SPINE1
config['const'] = HIP
config['root'] = HIP
config['FootPartNum'] = FOOT_PART_NUM
config['FootLPart'] = [
LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_CALCANEUS_2, LEFT_CALCANEUS_3,
LEFT_TALUS_1, LEFT_TALUS_2, LEFT_TALUS_3, LEFT_METATARSAL_1,
LEFT_METATARSAL_2, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_2,
LEFT_PHALANGE_3
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_CALCANEUS_2, RIGHT_CALCANEUS_3,
RIGHT_TALUS_1, RIGHT_TALUS_2, RIGHT_TALUS_3, RIGHT_METATARSAL_1,
RIGHT_METATARSAL_2, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1,
RIGHT_PHALANGE_2, RIGHT_PHALANGE_3
]
#config['FootLPart'] = [LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_METATARSAL_1, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_3]
#config['FootRPart'] = [RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_METATARSAL_1, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1, RIGHT_PHALANGE_3]
return motion, mcfg, wcfg, stepsPerFrame, config
def ik_analytic(posture, joint_name_or_index, new_position):
if isinstance(joint_name_or_index, int):
joint = joint_name_or_index
else:
joint = posture.skeleton.getJointIndex(joint_name_or_index)
# joint_parent = posture.body.joint_parent[joint]
# joint_parent_parent = posture.body.joint_parent[joint_parent]
joint_parent = posture.skeleton.getParentJointIndex(joint)
joint_parent_parent = posture.skeleton.getParentJointIndex(joint_parent)
# B = posture.get_position(joint)
# C = posture.get_position(joint_parent)
# A = posture.get_position(joint_parent_parent)
B = posture.getJointPositionGlobal(joint)
C = posture.getJointPositionGlobal(joint_parent)
A = posture.getJointPositionGlobal(joint_parent_parent)
L = B - A
N = B - C
M = C - A
# l = mathlib.length(L);
# n = mathlib.length(N);
# m = mathlib.length(M);
l = mm.length(L)
n = mm.length(N)
m = mm.length(M)
# a = mathlib.ACOS((l*l + n*n - m*m) / (2*l*n))
# b = mathlib.ACOS((l*l + m*m - n*n) / (2*l*m))
a = mm.ACOS((l * l + n * n - m * m) / (2 * l * n))
b = mm.ACOS((l * l + m * m - n * n) / (2 * l * m))
B_new = new_position
L_new = B_new - A
# l_ = mathlib.length(L_new)
l_ = mm.length(L_new)
# a_ = mathlib.ACOS((l_*l_ + n*n - m*m) / (2*l_*n))
# b_ = mathlib.ACOS((l_*l_ + m*m - n*n) / (2*l_*m))
a_ = mm.ACOS((l_ * l_ + n * n - m * m) / (2 * l_ * n))
b_ = mm.ACOS((l_ * l_ + m * m - n * n) / (2 * l_ * m))
# rotate joint in plane
# rotV = mathlib.normalize(numpy.cross(M, L))
rotV = mm.normalize2(np.cross(M, L))
rotb = b - b_
rota = a_ - a - rotb
# posture.rotate_global_orientation(joint_parent_parent, mathlib.exp(rotV, rotb))
# posture.rotate_global_orientation(joint_parent, mathlib.exp(rotV * rota))
posture.mulJointOrientationGlobal(joint_parent_parent, mm.exp(rotV, rotb))
posture.mulJointOrientationGlobal(joint_parent, mm.exp(rotV * rota))
# rotate plane
# rotV2 = mathlib.normalize(numpy.cross(L, L_new))
# l_new = mathlib.length(L_new)
# l_diff = mathlib.length(L_new - L)
# rot2 = mathlib.ACOS((l_new * l_new + l * l - l_diff * l_diff) / (2 * l_new * l))
# posture.rotate_global_orientation(joint_parent_parent, mathlib.exp(rotV2, rot2))
rotV2 = mm.normalize2(np.cross(L, L_new))
l_new = mm.length(L_new)
l_diff = mm.length(L_new - L)
rot2 = mm.ACOS((l_new * l_new + l * l - l_diff * l_diff) / (2 * l_new * l))
posture.mulJointOrientationGlobal(joint_parent_parent, mm.exp(rotV2, rot2))
return posture
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
#motionName = 'wd2_jump_ori.bvh'
if 1:
#motionName = 'wd2_stand.bvh'
motionName = 'woddy2_jump0.bvh'
motion = yf.readBvhFile(motionName, .01)
yme.removeJoint(motion, 'HEad', False)
yme.removeJoint(motion, 'RightShoulder', False)
yme.removeJoint(motion, 'LeftShoulder1', False)
yme.removeJoint(motion, 'RightToes_Effector', False)
yme.removeJoint(motion, 'LeftToes_Effector', False)
yme.removeJoint(motion, 'RightHand_Effector', False)
yme.removeJoint(motion, 'LeftHand_Effector', False)
yme.offsetJointLocal(motion, 'RightArm', (.03, -.05, 0), False)
yme.offsetJointLocal(motion, 'LeftArm', (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01),
False)
yme.rotateJointLocal(motion, 'LeftFoot',
mm.exp(mm.v3(2.5, -0.0, .3), -.5), False)
yme.rotateJointLocal(motion, 'RightFoot',
mm.exp(mm.v3(2.5, 0.0, -.3), -.5), False)
#yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.2), -.5), False)
#yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.2), -.5), False)
yme.rotateJointLocal(motion, 'LeftUpLeg',
mm.exp(mm.v3(0.0, .0, 1.), .08), False)
yme.rotateJointLocal(motion, 'LeftLeg', mm.exp(mm.v3(0.0, 1.0, 0.),
-.2), False)
#yme.rotateJointLocal(motion, 'RightLeg', mm.exp(mm.v3(1.0,0.0,0.), -.1), False)
yme.updateGlobalT(motion)
else:
motionName = 'ww13_41.bvh'
motion = yf.readBvhFile(motionName, 0.056444)
yme.removeJoint(motion, 'LHipJoint', False)
yme.removeJoint(motion, 'RHipJoint', False)
yme.removeJoint(motion, 'Neck', False)
yme.removeJoint(motion, 'Neck1', False)
yme.removeJoint(motion, 'Head', False)
yme.removeJoint(motion, 'RightShoulder', False)
yme.removeJoint(motion, 'LeftShoulder', False)
yme.removeJoint(motion, 'RightToeBase_Effector', False)
yme.removeJoint(motion, 'LeftToeBase_Effector', False)
yme.removeJoint(motion, 'LeftHand', False)
yme.removeJoint(motion, 'LeftFingerBase', False)
yme.removeJoint(motion, 'LeftHandIndex1_Effector', False)
yme.removeJoint(motion, 'LThumb', False)
yme.removeJoint(motion, 'RightHand', False)
yme.removeJoint(motion, 'RightFingerBase', False)
yme.removeJoint(motion, 'RightHandIndex1_Effector', False)
yme.removeJoint(motion, 'RThumb', False)
yme.removeJoint(motion, 'LowerBack', False)
yme.offsetJointLocal(motion, 'RightArm', (-.03, -.05, 0), False)
yme.offsetJointLocal(motion, 'LeftArm', (.03, -.05, 0), False)
#yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1,0,0), .01), False)
yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(-1.5, 0, 1), .4),
False)
yme.rotateJointLocal(motion, 'RightFoot',
mm.exp(mm.v3(1.5, 0, 1), -.4), False)
yme.updateGlobalT(motion)
#motion = motion[50:-1]
motion = motion[240:-1]
#motion = motion[40:-58]
#motion = motion[56:-248]
#motion = motion[-249:-248]
#motion = motion[62:65]
#motion = motion[216:217]
#motion = motion[515:555]
################
motion = motion[515:555]
#motion = motion[96:97]
motion[0:0] = [motion[0]] * 100
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode('Hips')
node.length = .2
node.width = .25
node = mcfg.getNode('Spine1')
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode('Spine')
node.width = .22
#node.length = .2 ####
node = mcfg.getNode('RightFoot')
node.length = .25
node.width = .2
node.mass = 4.
node = mcfg.getNode('LeftFoot')
node.length = .25
node.width = .2
node.mass = 4.
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
'''
config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain
config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain
config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain
config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 2.5
config['Bh'] = 1.
'''
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.3, 'Spine1':.3, 'RightFoot':.3, 'LeftFoot':.3, 'Hips':.5,\
'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3}
config['IKweightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.3, 'Spine1':.3, 'RightFoot':.3, 'LeftFoot':.3, 'Hips':.5,\
'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3}
'''
config['IKweightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':0.5, 'Spine1':0.5, 'RightFoot':1.2, 'LeftFoot':1.2, 'Hips':1.2,\
'RightUpLeg':.9, 'RightLeg':.9, 'LeftUpLeg':.9, 'LeftLeg':.9}
'''
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\
'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5}
config['supLink'] = 'LeftFoot'
config['supLink2'] = 'RightFoot'
#config['end'] = 'Hips'
config['end'] = 'Spine1'
return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
motionName = 'wd2_stand.bvh'
elif MOTION == STAND2:
motionName = 'ww13_41_V001.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == TAEKWONDO2:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == KICK:
motionName = 'wd2_n_kick.bvh'
elif MOTION == WALK:
motionName = 'wd2_WalkForwardNormal00.bvh'
elif MOTION == TIPTOE:
motionName = './MotionFile/cmu/15_07_15_07.bvh'
#motionName = 'ww13_41_V001.bvh'
scale = 0.01
if MOTION == WALK:
scale = 1.0
elif MOTION == TIPTOE:
scale = 0.01
motion = yf.readBvhFile(motionName, scale)
if MOTION != WALK:
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
if FOOT_PART_NUM == 1 and MOTION != WALK:
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
elif (FOOT_PART_NUM == 5 or FOOT_PART_NUM == 6) and MOTION != WALK:
yme.removeJoint(motion, RIGHT_TOES, False)
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
if MOTION != WALK:
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(0, 0, 1), -.01), False)
if FOOT_PART_NUM == 1:
yme.rotateJointLocal(motion, LEFT_FOOT,
mm.exp(mm.v3(2.5, -0.0, .3), -.5), False)
yme.rotateJointLocal(motion, RIGHT_FOOT,
mm.exp(mm.v3(2.5, 0.0, -.3), -.5), False)
if MOTION == WALK:
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1., -0.0, .0),
.4), False)
yme.rotateJointLocal(motion, RIGHT_FOOT,
mm.exp(mm.v3(1., 0.0, -.0), .4), False)
#yme.rotateJointLocal(motion, SPINE1, mm.exp(mm.v3(1.,0.0,0.0), -.8), False)
if MOTION == FORWARD_JUMP:
yme.rotateJointLocal(motion, LEFT_UP_LEG,
mm.exp(mm.v3(0.0, .0, 1.), .08), False)
yme.rotateJointLocal(motion, LEFT_LEG, mm.exp(mm.v3(0.0, 1.0, 0.),
-.2), False)
if FOOT_PART_NUM == 6:
yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_1,
(-0.045, -0.06, 0.03))
yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_3,
(0.045, -0.06, 0.03)) #-0.0037
yme.addJoint(motion, LEFT_METATARSAL_1, LEFT_PHALANGE_1,
(0.0, 0.0, 0.0715))
yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3,
(0.0, 0.0, 0.0715))
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_PHALANGE_Dummy1',
(0.0, 0.0, 0.1))
yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_PHALANGE_Dummy2',
(0.0, 0.0, 0.1))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (0.0, -0.06, -0.02))
yme.addJoint(motion, LEFT_CALCANEUS_1, 'LEFT_CALCANEUS_Dummy1',
(0.0, 0.0, -0.1))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_1,
(0.045, -0.06, 0.03))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_3,
(-0.045, -0.06 - 0.0062, 0.03 + 0.0035))
yme.addJoint(motion, RIGHT_METATARSAL_1, RIGHT_PHALANGE_1,
(0.0, 0.0, 0.0715))
yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3,
(0.0, 0.0, 0.0715))
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_PHALANGE_Dummy1',
(0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_PHALANGE_Dummy2',
(0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1,
(0.0, -0.06, -0.02))
yme.addJoint(motion, RIGHT_CALCANEUS_1, 'RIGHT_CALCANEUS_Dummy1',
(0.0, 0.0, -0.1))
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(.0, 0.0, 1.0), 3.14), False)
'''
yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), 3.14*0.3), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), 3.14*0.3), False)
yme.rotateJointLocal(motion, RIGHT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -3.14*0.3), False)
yme.rotateJointLocal(motion, RIGHT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0, 0.0), -3.14*0.3), False)
yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -3.14*0.3), False)
yme.rotateJointLocal(motion, LEFT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0, 0.0), -3.14*0.3), False)
'''
yme.rotateJointLocal(motion, LEFT_FOOT,
mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2012), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2172), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_3,
mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2172), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2172), False)
yme.rotateJointLocal(motion, RIGHT_FOOT,
mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2283), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2171), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3,
mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2171), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2171), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_1,
mm.exp(mm.v3(0.0, 0.0, 1.0), 0.0747), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3,
mm.exp(mm.v3(0.0, 0.0, 1.0), 0.0747), False)
'''
yme.rotateJointLocal(motion, RIGHT_METATARSAL_1, mm.exp(mm.v3(1.0,0.0,0.0), -0.1), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(1.0,0.0,0.0), -0.1), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_1, mm.exp(mm.v3(0.0,0.0,1.0), 0.06), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(0.0,0.0,1.0), 0.06), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), -0.05), False)
'''
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(0.0,0.0,1.0), 0.0656), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), 0.0871), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(0.0,0.0,1.0), 0.0087), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), 0.0933), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(0.0,0.0,1.0), 0.0746), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), -6.3773e-03), False)
#yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), math.pi*0.25), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), math.pi*0.25), False)
#yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False)
#yme.rotateJointLocal(motion, RIGHT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False)
#yme.rotateJointLocal(motion, LEFT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False)
#yme.rotateJointLocal(motion, RIGHT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False)
#yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), -math.pi*0.16), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), -math.pi*0.16), False)
#yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.16), False)
#yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.16), False)
#yme.rotateJointLocal(motion, LEFT_METATARSAL_3, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.5), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
elif MOTION == TAEKWONDO2:
## Taekwondo base-step
#motion = motion[0:31+40]
## Taekwondo turning-kick
motion = motion[108:-1]
#motion = motion[108:109]
elif MOTION == KICK:
#motion = motion[141:-1]
#motion = motion[100:-1]
#motion = motion[58:-1]
motion = motion[82:-1]
#motion = motion[0:-1]
elif MOTION == STAND2:
motion = motion[1:-1]
elif MOTION == TIPTOE:
#motion = motion[183:440]
#motion = motion[350:410]
motion = motion[350:550]
motion[0:0] = [motion[0]] * 40
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
#node.length = .2 ####
if FOOT_PART_NUM == 1:
length1 = .35
width1 = .2
mass1 = 4.3
elif FOOT_PART_NUM == 6:
mass0 = .4
width0 = 0.028
length0 = 0.1
#Metatarsal1
length1 = .15
width1 = width0 * 3
mass1 = mass0 * 1.8
#Metatarsal3
length3 = .11
width3 = width0 * 3
mass3 = mass0 * 1.86
#Calcaneus1
length4 = .08
width4 = 0.15 * 1
mass4 = mass0 * 1.2 * 2.
#Phalange1
length5 = .08
width5 = width1
mass5 = mass0 * 1.355
#Phalange3
length7 = length5
width7 = width5
mass7 = mass5
#Talus
length8 = .13
width8 = width0 * 3
mass8 = mass0 * 2.
if FOOT_PART_NUM == 6:
node = mcfg.getNode(RIGHT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(RIGHT_METATARSAL_1)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_METATARSAL_3)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.length = length5
node.width = width5
node.mass = mass5
node.offset = (0.0, 0.0, 0.03)
node = mcfg.getNode(RIGHT_PHALANGE_3)
node.length = length7
node.width = width7
node.mass = mass7
node.offset = (0.0, 0.0, 0.01)
node = mcfg.getNode(RIGHT_CALCANEUS_1)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(LEFT_METATARSAL_1)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_METATARSAL_3)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_PHALANGE_1)
node.length = length5
node.width = width5
node.mass = mass5
node.offset = (0.0, 0.0, 0.03)
node = mcfg.getNode(LEFT_PHALANGE_3)
node.length = length7
node.width = width7
node.mass = mass7
node.offset = (0.0, 0.0, 0.01)
node = mcfg.getNode(LEFT_CALCANEUS_1)
node.length = length4
node.width = width4
node.mass = mass4
if FOOT_PART_NUM < 5:
node = mcfg.getNode(RIGHT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
if FOOT_PART_NUM == 1:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 1.,
LEFT_FOOT: 1.,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.
}
elif FOOT_PART_NUM == 6:
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .5,
SPINE1: .3,
RIGHT_FOOT: .7,
LEFT_FOOT: .7,
HIP: .5,
RIGHT_UP_LEG: .7,
RIGHT_LEG: .7,
LEFT_UP_LEG: .7,
LEFT_LEG: .7,
LEFT_METATARSAL_1: .7,
RIGHT_METATARSAL_1: .7,
LEFT_METATARSAL_3: .7,
RIGHT_METATARSAL_3: .7,
RIGHT_CALCANEUS_1: .7,
LEFT_CALCANEUS_1: .7,
LEFT_PHALANGE_1: .4,
LEFT_PHALANGE_3: .4,
RIGHT_PHALANGE_1: .4,
RIGHT_PHALANGE_3: .4
}
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .2,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .3,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .2,
LEFT_UP_LEG: .1,
LEFT_LEG: .2,
LEFT_METATARSAL_1: .1,
RIGHT_METATARSAL_1: .1,
LEFT_METATARSAL_3: .1,
RIGHT_METATARSAL_3: .1,
RIGHT_CALCANEUS_1: .2,
LEFT_CALCANEUS_1: .2,
LEFT_PHALANGE_1: .1,
LEFT_PHALANGE_3: .1,
RIGHT_PHALANGE_1: .1,
RIGHT_PHALANGE_3: .1
}
'''
config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5,
RIGHT_UP_LEG:.1, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3,
LEFT_METATARSAL_3:.2, RIGHT_METATARSAL_3:.2, RIGHT_CALCANEUS:1.2, LEFT_CALCANEUS:1.2,
LEFT_PHALANGE_1:.2, LEFT_PHALANGE_3:.1, RIGHT_PHALANGE_1:.2, RIGHT_PHALANGE_3:.2}
config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.8, SPINE1:.3, RIGHT_FOOT:1., LEFT_FOOT:1., HIP:1.,
RIGHT_UP_LEG:1., RIGHT_LEG:1., LEFT_UP_LEG:1., LEFT_LEG:1.,
LEFT_METATARSAL_3:1., RIGHT_METATARSAL_3:1., RIGHT_CALCANEUS:.3, LEFT_CALCANEUS:.3,
LEFT_PHALANGE_1:.3, LEFT_PHALANGE_3:.3, RIGHT_PHALANGE_1:.3, RIGHT_PHALANGE_3:.3}
'''
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
#config['end'] = HIP
config['end'] = SPINE1
config['const'] = HIP
config['root'] = HIP
config['FootPartNum'] = FOOT_PART_NUM
if FOOT_PART_NUM == 6:
config['FootLPart'] = [
LEFT_FOOT, LEFT_METATARSAL_1, LEFT_METATARSAL_3, LEFT_PHALANGE_1,
LEFT_PHALANGE_3, LEFT_CALCANEUS_1
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_METATARSAL_1, RIGHT_METATARSAL_3,
RIGHT_PHALANGE_1, RIGHT_PHALANGE_3, RIGHT_CALCANEUS_1
]
else:
config['FootLPart'] = [
LEFT_FOOT, LEFT_TOES, LEFT_TARSUS, LEFT_TOES_SIDE_R,
LEFT_FOOT_SIDE_L, LEFT_FOOT_SIDE_R
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_TOES_SIDE_R,
RIGHT_FOOT_SIDE_L, RIGHT_FOOT_SIDE_R
]
return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped():
# motion
# motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
# motionName = 'wd2_stand.bvh'
motionName = 'wd2_stand2.bvh'
# motionName = 'woddy2_jump0_short.bvh'
elif MOTION == STAND2:
motionName = 'ww13_41_V001.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == TAEKWONDO2:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == KICK:
motionName = 'wd2_n_kick.bvh'
elif MOTION == WALK:
motionName = 'wd2_WalkForwardNormal00.bvh'
elif MOTION == TIPTOE:
motionName = './MotionFile/cmu/15_07_15_07.bvh'
# motionName = 'ww13_41_V001.bvh'
scale = 0.01
if MOTION == WALK:
scale = 1.0
elif MOTION == TIPTOE:
scale = 0.01
motion = yf.readBvhFile(motionName, scale)
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
yme.removeJoint(motion, RIGHT_TOES, False)
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0.), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0.), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1., 0., 0.), .01), False)
# yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(0,0,1), -.01), False)
# addFootSegment
# Talus
length1 = .15
width1 = .03
mass1 = .4
# Phalange
length3 = length1 * 0.75
width3 = width1
mass3 = 0.4
length3_2 = length1 * 0.6
width3_2 = width1
mass3_2 = 0.4
#Calcaneus
length4 = 0.05
width4 = width1
mass4 = 0.4
width1 = 0.03
yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_1, (-0.025, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_2, (0.025, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_1, (0.025, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_2, (-0.025, -0.06, -0.05))
yme.addJoint(motion, LEFT_TALUS_1, LEFT_PHALANGE_1,
(0.0, 0.0, length1 - width1 - width3))
yme.addJoint(motion, LEFT_TALUS_2, LEFT_PHALANGE_2,
(0.0, 0.0, length1 - width1 - width3_2))
yme.addJoint(motion, RIGHT_TALUS_1, RIGHT_PHALANGE_1,
(0.0, 0.0, length1 - width1 - width3))
yme.addJoint(motion, RIGHT_TALUS_2, RIGHT_PHALANGE_2,
(0.0, 0.0, length1 - width1 - width3_2))
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_PHALANGE_Effector1',
(0.0, 0.0, length3 - width3))
yme.addJoint(motion, LEFT_PHALANGE_2, 'LEFT_PHALANGE_Effector2',
(0.0, 0.0, length3_2 - width3_2))
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_PHALANGE_Effector1',
(0.0, 0.0, length3 - width3))
yme.addJoint(motion, RIGHT_PHALANGE_2, 'RIGHT_PHALANGE_Effector2',
(0.0, 0.0, length3_2 - width3_2))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (-0.025, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_2, (0.025, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1, (0.025, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_2, (-0.025, -0.06, -0.05))
yme.addJoint(motion, LEFT_CALCANEUS_1, 'LEFT_CALCANEUS_Effector1',
(0.0, 0.0, -length4))
yme.addJoint(motion, LEFT_CALCANEUS_2, 'LEFT_CALCANEUS_Effector2',
(0.0, 0.0, -length4))
yme.addJoint(motion, RIGHT_CALCANEUS_1, 'RIGHT_CALCANEUS_Effector1',
(0.0, 0.0, -length4))
yme.addJoint(motion, RIGHT_CALCANEUS_2, 'RIGHT_CALCANEUS_Effector2',
(0.0, 0.0, -length4))
yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5),
False)
yme.rotateJointLocal(motion, RIGHT_TALUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, RIGHT_TALUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
#yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(0., 1., 0.), 1.57), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5),
False)
yme.rotateJointLocal(motion, LEFT_TALUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, LEFT_TALUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_2,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_2,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
#yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(0.0,-1.0,0.0), 1.57), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
elif MOTION == TAEKWONDO2:
## Taekwondo base-step
#motion = motion[0:31+40]
## Taekwondo turning-kick
motion = motion[108:-1]
#motion = motion[108:109]
elif MOTION == KICK:
#motion = motion[141:-1]
#motion = motion[100:-1]
#motion = motion[58:-1]
motion = motion[82:-1]
#motion = motion[0:-1]
elif MOTION == STAND2:
motion = motion[1:-1]
elif MOTION == TIPTOE:
#motion = motion[183:440]
#motion = motion[350:410]
motion = motion[350:550]
motion[0:0] = [motion[0]] * 40
motion.extend([motion[-1]] * 2000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = 1.
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
# node.length = .2 ####
node = mcfg.getNode('RightFoot')
node.length = .1
node.width = .1
node = mcfg.getNode('LeftFoot')
node.length = .1
node.width = .1
#Talus
#length1 = .12
#width1 = 0.03
#mass1 = 0.4
#Phalange
#length3 = length1
#width3 = width1
#mass3 = 0.4
#length3_2 = length1*0.8
#width3_2 = width1
#mass3_2 = 0.4
#Calcaneus1
#length4 = 0.1
#width4 = width1
#mass4 = 0.4
#Foot
length8 = .1
width8 = 0.028 * 3
mass8 = .4 * 1.5
node = mcfg.getNode(RIGHT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(RIGHT_TALUS_1)
node.length = length1 - width1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_TALUS_2)
node.length = length1 - width1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_PHALANGE_2)
node.length = length3_2
node.width = width3_2
node.mass = mass3_2
node = mcfg.getNode(RIGHT_CALCANEUS_1)
node.length = length4 - width4
node.width = width4
node.mass = mass4
node = mcfg.getNode(RIGHT_CALCANEUS_2)
node.length = length4 - width4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(LEFT_TALUS_1)
node.length = length1 - width1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_TALUS_2)
node.length = length1 - width1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_PHALANGE_1)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_PHALANGE_2)
node.length = length3_2
node.width = width3_2
node.mass = mass3_2
node = mcfg.getNode(LEFT_CALCANEUS_1)
node.length = length4 # - width4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_CALCANEUS_2)
node.length = length4 # - width4
node.width = width4
node.mass = mass4
#node.offset = (0.0, -0.025, 0.0)
node = mcfg.getNode('LeftFoot')
node = mcfg.getNode(RIGHT_TALUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_TALUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_TALUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_TALUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_METATARSAL_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_METATARSAL_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_METATARSAL_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_METATARSAL_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.geom = 'MyFoot4'
node = mcfg.getNode(RIGHT_PHALANGE_2)
node.geom = 'MyFoot4'
node = mcfg.getNode(LEFT_PHALANGE_1)
node.geom = 'MyFoot4'
node = mcfg.getNode(LEFT_PHALANGE_2)
node.geom = 'MyFoot4'
node = mcfg.getNode(RIGHT_CALCANEUS_1)
node.geom = 'MyFoot4'
node = mcfg.getNode(RIGHT_CALCANEUS_2)
node.geom = 'MyFoot4'
node = mcfg.getNode(LEFT_CALCANEUS_1)
node.geom = 'MyFoot4'
node = mcfg.getNode(LEFT_CALCANEUS_2)
node.geom = 'MyFoot4'
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 120
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .2,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .3,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .2,
LEFT_UP_LEG: .1,
LEFT_LEG: .2,
LEFT_TALUS_1: .1,
RIGHT_TALUS_1: .1,
LEFT_TALUS_2: .1,
RIGHT_TALUS_2: .1,
RIGHT_CALCANEUS_1: .2,
LEFT_CALCANEUS_1: .2,
RIGHT_CALCANEUS_2: .2,
LEFT_CALCANEUS_2: .2,
LEFT_PHALANGE_1: .1,
LEFT_PHALANGE_2: .1,
RIGHT_PHALANGE_1: .1,
RIGHT_PHALANGE_2: .1
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .5,
SPINE1: .3,
RIGHT_FOOT: .7,
LEFT_FOOT: .7,
HIP: .5,
RIGHT_UP_LEG: .7,
RIGHT_LEG: .7,
LEFT_UP_LEG: .7,
LEFT_LEG: .7,
LEFT_TALUS_1: .7,
RIGHT_TALUS_1: .7,
LEFT_TALUS_2: .7,
RIGHT_TALUS_2: .7,
RIGHT_CALCANEUS_1: .7,
LEFT_CALCANEUS_1: .7,
RIGHT_CALCANEUS_2: .7,
LEFT_CALCANEUS_2: .7,
LEFT_PHALANGE_1: .4,
LEFT_PHALANGE_2: .4,
RIGHT_PHALANGE_1: .4,
RIGHT_PHALANGE_2: .4
}
'''
(1, 'RightUpLeg')
(2, 'RightLeg')
(3, 'RightFoot')
(4, 'Spine')
(5, 'Spine1')
(6, 'LeftArm')
(7, 'LeftForeArm')
(8, 'RightArm')
(9, 'RightForeArm')
(10, 'LeftUpLeg')
(11, 'LeftLeg')
(12, 'LeftFoot')
(13, 'LeftTalus_1')
(14, 'LeftTalus_2')
(15, 'RightTalus_1')
(16, 'RightTalus_2')
(17, 'LeftPhalange_1')
(18, 'LeftPhalange_2')
(19, 'RightPhalange_1')
(20, 'RightPhalange_2')
(21, 'LeftCalcaneus_1')
(22, 'LeftCalcaneus_2')
(23, 'RightCalcaneus_1')
(24, 'RightCalcaneus_2')
'''
config['trackWMap'] = {
10, 5, .1, 20, 10, 5, 2, 10, 5, .1, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01,
0.01, 0.01, 0.01, 0.01, 0.10, 0.01
}
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
# config['end'] = HIP
config['end'] = SPINE1
config['trunk'] = SPINE
config['const'] = HIP
config['root'] = HIP
config['FootPartNum'] = FOOT_PART_NUM
config['FootLPart'] = [
LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_CALCANEUS_2, LEFT_TALUS_1,
LEFT_TALUS_2, LEFT_PHALANGE_1, LEFT_PHALANGE_2
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_CALCANEUS_2, RIGHT_TALUS_1,
RIGHT_TALUS_2, RIGHT_PHALANGE_1, RIGHT_PHALANGE_2
]
return motion, mcfg, wcfg, stepsPerFrame, config
