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