def __init__(self): glutInit() self.center = numpy.array([0., 0.5, 0.]) self.rotateY = mmMath.deg2Rad(0.) self.rotateX = mmMath.deg2Rad(-15.0) # self.distance = 10. self.distance = 3.1
def preFrameCallback_Always(frame): if frame < 120 + start_frame: viewer.doc.setRendererVisible('controlModel', True) viewer.doc.setRendererVisible('skeleton', False) viewer.motionViewWnd.glWindow.camera.rotateX = math.pi / 180. * -25. if frame > start_frame: viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad( (frame - start_frame) * 3 + 45.) else: viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad(45.) viewer.motionViewWnd.glWindow.camera.distance = .4 viewer.motionViewWnd.glWindow.camera.center = \ .5*(controlModel.getBodyPositionGlobal(idDic['RightFoot']) + controlModel.getBodyPositionGlobal(idDic['LeftFoot'])) + np.array([0., -0.05, 0.]) elif frame > 340: viewer.doc.setRendererVisible('controlModel', False) viewer.doc.setRendererVisible('skeleton', True) else: if 0 <= frame % 50 < 22: viewer.doc.setRendererVisible('controlModel', True) viewer.doc.setRendererVisible('skeleton', False) elif 22 <= frame % 50 < 25: viewer.doc.setRendererVisible('controlModel', False) viewer.doc.setRendererVisible('skeleton', False) elif 25 <= frame % 50 < 47: viewer.doc.setRendererVisible('controlModel', False) viewer.doc.setRendererVisible('skeleton', True) elif 47 <= frame % 50 < 50: viewer.doc.setRendererVisible('controlModel', False) viewer.doc.setRendererVisible('skeleton', False)
def preFrameCallback_Always(frame): if False: viewer.motionViewWnd.glWindow.camera.rotateX = math.pi / 180. * -25. if frame > start_frame: viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad( (frame - start_frame) * 3 + 45.) else: viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad(45.) viewer.motionViewWnd.glWindow.camera.distance = .4 viewer.motionViewWnd.glWindow.camera.center = \ .5*(controlModel.getBodyPositionGlobal(idDic['RightFoot']) + controlModel.getBodyPositionGlobal(idDic['LeftFoot'])) + np.array([0., -0.05, 0.])
def viewFromTop(self): self.viewMode = VIEW_TOP if self.projectionOrtho == False: self.prevRotX = self.camera.rotateX self.prevRotY = self.camera.rotateY self.projectionOrtho = True self.projectionChanged = True self.camera.rotateX = mmMath.deg2Rad(-90.0) self.camera.rotateY = 0 self.camera.rotateZ = 0
def viewFromRight(self): self.viewMode = VIEW_RIGHT if self.projectionOrtho == False: self.prevRotX = self.camera.rotateX self.prevRotY = self.camera.rotateY self.projectionOrtho = True self.projectionChanged = True self.camera.rotateX = 0 self.camera.rotateY = mmMath.deg2Rad(90.0) self.camera.rotateZ = 0
def addJointSO3FromBvhJoint(self, jointPosture, bvhJoint, channelValues, scale=1.0): localR = mm.I_SO3() local_t = mm.O_Vec3() for channel in bvhJoint.channels: if channel.channelType == 'XPOSITION': # jointPosture.rootPos[0] = channelValues[channel.channelIndex]*scale local_t[0] = channelValues[channel.channelIndex] * scale elif channel.channelType == 'YPOSITION': # jointPosture.rootPos[1] = channelValues[channel.channelIndex]*scale local_t[1] = channelValues[channel.channelIndex] * scale elif channel.channelType == 'ZPOSITION': # jointPosture.rootPos[2] = channelValues[channel.channelIndex]*scale local_t[2] = channelValues[channel.channelIndex] * scale elif channel.channelType == 'XROTATION': localR = numpy.dot( localR, mm.exp(mm.s2v((1, 0, 0)), mm.deg2Rad(channelValues[channel.channelIndex]))) elif channel.channelType == 'YROTATION': localR = numpy.dot( localR, mm.exp(mm.s2v((0, 1, 0)), mm.deg2Rad(channelValues[channel.channelIndex]))) elif channel.channelType == 'ZROTATION': localR = numpy.dot( localR, mm.exp(mm.s2v((0, 0, 1)), mm.deg2Rad(channelValues[channel.channelIndex]))) # jointPosture.setLocalR(bvhJoint.name, localR) jointPosture.setLocalR( jointPosture.skeleton.getElementIndex(bvhJoint.name), localR) jointPosture.setLocal_t( jointPosture.skeleton.getElementIndex(bvhJoint.name), local_t) for i in range(len(bvhJoint.children)): self.addJointSO3FromBvhJoint(jointPosture, bvhJoint.children[i], channelValues, scale)
def preFrameCallback_Always(frame): # print(mm.rad2Deg(math.pi/6.*math.sin((frame-30)*math.pi/180.))) if frame <= start_frame: vpWorld.set_plane(0, mm.unitY(), np.zeros(3)) if frame > start_frame: if math.sin((frame - start_frame) / 360. * math.pi) > 0.: if frame < start_frame + 50: setParamVal( 'com Z offset', 0.02 * math.sin(2. * (frame - start_frame) / 360. * math.pi)) else: setParamVal('com Z offset', 0.0) if math.sin((frame - start_frame) / 360. * math.pi) > 0.: foot_viewer.check_not_all_seg() foot_viewer.check_tiptoe_all() setParamVal( 'tiptoe angle', mm.deg2Rad(10.) * math.sin( (frame - start_frame) / 360. * math.pi)) # foot_viewer.check_h_l.value(False) # foot_viewer.check_h_r.value(False) else: foot_viewer.check_all_seg() # foot_viewer.check_tiptoe_all() # foot_viewer.check_h_l.value(True) # foot_viewer.check_h_r.value(True) vpWorld.set_plane( 0, np.dot( mm.exp( -mm.unitX(), mm.deg2Rad(10.) * math.sin( (frame - start_frame) / 360. * math.pi)), mm.unitY()), np.zeros(3)) plane_list = vpWorld.get_plane_list() plane_normal = plane_list[0][0] plane_origin = plane_list[0][1] viewer.motionViewWnd.glWindow.pOnPlaneshadow = plane_origin + plane_normal * 0.001 viewer.motionViewWnd.glWindow.normalshadow = plane_normal
def preFrameCallback_Always(frame): global COLOR_ON viewer.motionViewWnd.glWindow.camera.rotateX = math.pi / 180. * -25. viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad(45.) viewer.motionViewWnd.glWindow.camera.distance = .5 viewer.motionViewWnd.glWindow.camera.center = np.array( [0.04347, -0.01005, -0.31936]) if frame < start_frame: setParamVal('com Y offset', -0.11) viewer.doc.setRendererVisible('motionModel', False) viewer.doc.setRendererVisible('skeleton', True) elif frame < start_frame + 20: viewer.doc.setRendererVisible('motionModel', True) elif frame < start_frame + 50: setParamVal('com X offset', (frame - start_frame - 20) * 0.003) elif frame < start_frame + 70: pass elif frame < start_frame + 90: COLOR_ON = True elif frame < start_frame + 105: setParamVal('com Y offset', -0.11 + (frame - start_frame - 90) * 0.0032) setParamVal('tiptoe angle', (frame - start_frame - 90) * 0.012) elif frame < start_frame + 120: pass elif frame < start_frame + 135: setParamVal('com Y offset', -0.065 - (frame - start_frame - 120) * 0.0032) setParamVal('tiptoe angle', 0.168 - (frame - start_frame - 120) * 0.012) elif frame < start_frame + 150: pass elif frame < start_frame + 165: setParamVal('com Y offset', -0.11 + (frame - start_frame - 150) * 0.0004) setParamVal('left tilt angle', -(frame - start_frame - 150) * 0.012) elif frame < start_frame + 180: pass elif frame < start_frame + 195: setParamVal('com Y offset', -0.104 - (frame - start_frame - 180) * 0.0004) setParamVal('left tilt angle', -0.168 + (frame - start_frame - 180) * 0.012)
def handle(self, e): returnVal = 0 if e == FL_RELEASE: self.mouseX = Fl.event_x() self.mouseY = Fl.event_y() if self.mouse_pick_on: self.checkMousePick() returnVal = 1 elif e == FL_PUSH: self.mouseX = Fl.event_x() self.mouseY = Fl.event_y() returnVal = 1 elif e == FL_DRAG: self.mouseX = Fl.event_x() self.mouseY = Fl.event_y() mouseDeltaX = self.mouseX - self.mousePrevX mouseDeltaY = self.mouseY - self.mousePrevY button = Fl.event_button() if button == 1: self.camera.rotateY -= mmMath.deg2Rad(mouseDeltaX) if self.viewMode == VIEW_PERSPECTIVE: self.camera.rotateX -= mmMath.deg2Rad(mouseDeltaY) elif button == 2: if self.viewMode != VIEW_TOP: self.camera.center[ 1] += self.camera.distance * .05 * mouseDeltaY / 12.0 elif button == 3: # right self.camera.center[0] -= self.camera.distance * .05 * math.cos( self.camera.rotateY) * mouseDeltaX / 12.0 self.camera.center[ 2] -= self.camera.distance * .05 * -math.sin( self.camera.rotateY) * mouseDeltaX / 12.0 if self.viewMode == VIEW_PERSPECTIVE or self.viewMode == VIEW_TOP: # look self.camera.center[ 0] -= self.camera.distance * .05 * math.sin( self.camera.rotateY) * mouseDeltaY / 12.0 self.camera.center[ 2] -= self.camera.distance * .05 * math.cos( self.camera.rotateY) * mouseDeltaY / 12.0 returnVal = 1 elif e == FL_MOUSEWHEEL: self.camera.distance -= self.camera.distance * .2 * Fl.event_dy( ) / 3.0 # self.camera.distance -= Fl.event_dy() / 3.0 if self.camera.distance < 0.0001: self.camera.distance = 0.0001 self.projectionChanged = True returnVal = 1 elif e == FL_KEYUP: if Fl.event_key() == ord('!'): self.viewFromFront() elif Fl.event_key() == ord('@'): self.viewFromRight() elif Fl.event_key() == ord('#'): self.viewFromTop() elif Fl.event_key() == ord('$'): self.viewPerspective() # elif Fl.event_key()==ord('a'): # self.cameraMode ^= True # elif Fl.event_key()==ord('q'): # self.viewFromFront() # elif Fl.event_key()==ord('w'): # self.viewFromRight() # elif Fl.event_key()==ord('e'): # self.viewFromTop() # elif Fl.event_key()==ord('r'): # self.viewPerspective() # elif Fl.event_key()==ord('v'): # self.force = [0, -self.forceMag/4., self.forceMag] # #self.force = [self.forceMag, -self.forceMag/2, self.forceMag] # self.forceDir = 1 # elif Fl.event_key()==ord('f'): # self.force = [0, 0, 0] # self.forceDir = 0 ''' elif Fl.event_key()==ord('p'): self.forceMag += 1.0 if self.forceDir == 1: self.force[2] = self.forceMag elif self.forceDir == 2: self.force[2] = -self.forceMag elif self.forceDir == 3: self.force[0] = -self.forceMag elif self.forceDir == 4: self.force[0] = self.forceMag print("forceMag", self.forceMag) elif Fl.event_key()==ord('o'): self.forceMag -= 1.0 if self.forceDir == 1: self.force[2] = self.forceMag elif self.forceDir == 2: self.force[2] = -self.forceMag elif self.forceDir == 3: self.force[0] = -self.forceMag elif self.forceDir == 4: self.force[0] = self.forceMag print("forceMag", self.forceMag) ''' returnVal = 1 self.mousePrevX = self.mouseX self.mousePrevY = self.mouseY if returnVal == 1: self.redraw() return returnVal
def create_biped(motionName='wd2_n_kick.bvh', SEGMENT_FOOT=True, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=None): """ :param motionName: motion file name :param SEGMENT_FOOT: whether segment foot is :param SEGMENT_FOOT_MAG: :return: """ # :rtype: ym.JointMotion, ypc.ModelConfig, ypc.WorldConfig, int, dict[str, float|dict[str, float]], float if SEGMENT_FOOT_RAD is None: SEGMENT_FOOT_RAD = SEGMENT_FOOT_MAG * .5 SEGMENT_FOOT_SEPARATE = False SEGMENT_FOOT_OUTSIDE_JOINT_FIRST = True SEGMENT_FOOT_ARC = True SEGMENT_BETWEEN_SPACE = 1.2 SEGMENT_METATARSAL_LEN = 2.5 SEGMENT_THIRD_PHA_LEN = 1.8 SEGMENT_FOURTH_PHA_RATIO = 5. / 6. SEGMENT_HEEL_LEN = 1.2 # motion # motionName = 'wd2_n_kick.bvh' # motionName = 'wd2_tiptoe.bvh' # motionName = 'wd2_n_kick_zygote.bvh' # motionName = 'wd2_jump.bvh' # motionName = 'wd2_stand.bvh' bvh = yf.readBvhFileAsBvh(motionName) bvh.set_scale(.01) if SEGMENT_FOOT: # partBvhFilePath = '../PyCommon/modules/samples/simpleJump_long_test2.bvh' current_path = os.path.dirname(os.path.abspath(__file__)) partBvhFilePath = current_path + '/../../PyCommon/modules/samples/' if SEGMENT_FOOT_SEPARATE: partBvhFilePath = partBvhFilePath + 'simpleJump_long_test5.bvh' elif SEGMENT_FOOT_OUTSIDE_JOINT_FIRST: # partBvhFilePath = partBvhFilePath + 'simpleJump_long_test3.bvh' # partBvhFilePath = partBvhFilePath + 'foot_model_01.bvh' partBvhFilePath = partBvhFilePath + 'foot_model_01.bvh' else: partBvhFilePath = partBvhFilePath + 'simpleJump_long_test4.bvh' partBvh = yf.readBvhFileAsBvh(partBvhFilePath) partSkeleton = partBvh.toJointSkeleton(1., False) SEGMENT_BETWEEN_SPACE = partSkeleton.getOffset( partSkeleton.getElementIndex('foot_0_1'))[0] SEGMENT_METATARSAL_LEN = partSkeleton.getOffset( partSkeleton.getElementIndex('foot_0_1_0'))[2] SEGMENT_THIRD_PHA_LEN = partSkeleton.getOffset( partSkeleton.getElementIndex('foot_0_0_0_Effector'))[2] SEGMENT_HEEL_LEN = abs( partSkeleton.getOffset( partSkeleton.getElementIndex('foot_1_0_Effector'))[2]) bvh.replaceJointFromBvh('RightFoot', partBvh, SEGMENT_FOOT_MAG) partBvh = yf.readBvhFileAsBvh(partBvhFilePath) partBvh.mirror('YZ') bvh.replaceJointFromBvh('LeftFoot', partBvh, SEGMENT_FOOT_MAG) motion = bvh.toJointMotion(1., False) # type: ym.JointMotion # motion.translateByOffset((0., 0.15, 0.)) # motion.translateByOffset((0., -0.12, 0.)) # motion.rotateByOffset(mm.rotZ(math.pi*1./18.)) # motion = yf.readBvhFile(motionName, .01) # 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,-0.0,.3), -.5), False) # yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False) # yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.5,0), -.6), False) # yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.5,0), -.6), False) # yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.1), False) # yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.1), False) # yme.removeJoint(motion, 'RightFoot_foot_1_1') # yme.removeJoint(motion, 'RightFoot_foot_1_2') # yme.removeJoint(motion, 'LeftFoot_foot_1_1') # yme.removeJoint(motion, 'LeftFoot_foot_1_2') if motionName == 'wd2_n_kick.bvh' or motionName == 'wd2_n_kick_zygote.bvh': yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False) yme.updateGlobalT(motion) motion.translateByOffset((0, 0.04, 0)) for i in range(2000): motion.data.insert(0, copy.deepcopy(motion[0])) motion.extend([motion[-1]] * 300) elif motionName == 'wd2_tiptoe.bvh' or motionName == 'wd2_tiptoe_zygote.bvh': yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False) yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1., 0., 0.), -.1), False) yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1., 0., 0.), -.1), False) yme.updateGlobalT(motion) motion.translateByOffset((0, 0.06, 0)) # if motionName == 'wd2_tiptoe.bvh': # motion.translateByOffset((0, 0.06, 0)) # else: # motion.translateByOffset((0, -0.03, 0)) del motion[:270] for i in range(2000): motion.data.insert(0, copy.deepcopy(motion[0])) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 60 # stepsPerFrame = 30 frame_rate = 30 wcfg.timeStep = 1. / (frame_rate * stepsPerFrame) # wcfg.timeStep = (1/30.)/(stepsPerFrame) # wcfg.timeStep = (1/1000.) # width : x axis on body frame # height: y axis on body frame # length: z axis on body frame node = mcfg.getNode('Hips') node.length = 4. / 27. node.width = .25 # node.height = .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 = mcfg.getNode('RightFoot') node.length = .25 #node.length = .2 #node.width = .15 node.width = .2 node.mass = 2. node = mcfg.getNode('LeftFoot') node.length = .25 #node.length = .2 #node.width = .15 node.width = .2 node.mass = 2. def capsulize(node_name): node_capsule = mcfg.getNode(node_name) node_capsule.geom = 'MyFoot4' node_capsule.width = 0.01 node_capsule.density = 200. # node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., math.pi/4., 0.])], ypc.CapsuleMaterial(1000., .02, .2)) # node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., math.pi/4., 0.])], ypc.CapsuleMaterial(1000., .02, .1)) # node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., 0., 0.])], ypc.CapsuleMaterial(1000., .01, -1)) # node.addGeom('MyFoot4', None, ypc.CapsuleMaterial(1000., .02, .1)) # capsulize('RightFoot') # capsulize('LeftFoot') if SEGMENT_FOOT: node = mcfg.getNode('RightFoot') node.density = 200. node.geom = 'MyFoot5' node.width = 0.01 node.jointType = 'B' node = mcfg.getNode('LeftFoot') node.density = 200. node.geom = 'MyFoot5' node.width = 0.01 node.jointType = 'B' # bird foot # capsulize('RightFoot_foot_0_0') # capsulize('RightFoot_foot_0_1') # capsulize('RightFoot_foot_1_0') # capsulize('RightFoot_foot_1_1') # capsulize('RightFoot_foot_2_0') # capsulize('RightFoot_foot_2_1') # capsulize('LeftFoot_foot_0_0') # capsulize('LeftFoot_foot_0_1') # capsulize('LeftFoot_foot_1_0') # capsulize('LeftFoot_foot_1_1') # capsulize('LeftFoot_foot_2_0') # capsulize('LeftFoot_foot_2_1') # human foot if SEGMENT_FOOT: footJointType = 'B' capsulDensity = 400. if SEGMENT_FOOT_SEPARATE: # RightFoot_foot_0_0 : outside metatarsals capsulize('RightFoot_foot_0_0') node = mcfg.getNode('RightFoot_foot_0_0') # node.addGeom('MyFoot3', [SEGMENT_FOOT_MAG*np.array([0., 0., 2.5*0.25]), mm.exp([0., 0., 0.])], node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0., 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0. - 1.2, 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # RightFoot_foot_0_0_0 : outside phalanges capsulize('RightFoot_foot_0_0_0') node = mcfg.getNode('RightFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_0_1 : inside metatarsals capsulize('RightFoot_foot_0_1') node = mcfg.getNode('RightFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_0_1_0 : inside phalanges capsulize('RightFoot_foot_0_1_0') node = mcfg.getNode('RightFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_1_0 : center heel capsulize('RightFoot_foot_1_0') node = mcfg.getNode('RightFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType capsulize('LeftFoot_foot_0_0') node = mcfg.getNode('LeftFoot_foot_0_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0., 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0. + 1.2, 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType capsulize('LeftFoot_foot_0_0_0') node = mcfg.getNode('LeftFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_0_1') node = mcfg.getNode('LeftFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_0_1_0') node = mcfg.getNode('LeftFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_1_0') node = mcfg.getNode('LeftFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType elif SEGMENT_FOOT_OUTSIDE_JOINT_FIRST and not SEGMENT_FOOT_ARC: # RightFoot_foot_0_0 : outside metatarsals capsulize('RightFoot_foot_0_0') node = mcfg.getNode('RightFoot_foot_0_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-0.3, 0., 2.5 * 0.25]), mm.exp([0., -math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-0.3 - 1.2, 0., 2.5 * 0.25]), mm.exp([0., -math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' # RightFoot_foot_0_0_0 : outside phalanges capsulize('RightFoot_foot_0_0_0') node = mcfg.getNode('RightFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] # RightFoot_foot_0_1 : inside metatarsals capsulize('RightFoot_foot_0_1') node = mcfg.getNode('RightFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['Z'] # RightFoot_foot_0_1_0 : inside phalanges capsulize('RightFoot_foot_0_1_0') node = mcfg.getNode('RightFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] # RightFoot_foot_1_0 : center heel capsulize('RightFoot_foot_1_0') node = mcfg.getNode('RightFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' # left foot # outside metatarsals capsulize('LeftFoot_foot_0_0') node = mcfg.getNode('LeftFoot_foot_0_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0.3, 0., 2.5 * 0.25]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0.3 + 1.2, 0., 2.5 * 0.25]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' capsulize('LeftFoot_foot_0_0_0') node = mcfg.getNode('LeftFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] capsulize('LeftFoot_foot_0_1') node = mcfg.getNode('LeftFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['Z'] capsulize('LeftFoot_foot_0_1_0') node = mcfg.getNode('LeftFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] capsulize('LeftFoot_foot_1_0') node = mcfg.getNode('LeftFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' elif SEGMENT_FOOT_OUTSIDE_JOINT_FIRST and SEGMENT_FOOT_ARC: FIRST_METATARSAL_ANGLE = mm.deg2Rad(30.) SECOND_METATARSAL_ANGLE = mm.deg2Rad(20.) THIRD_METATARSAL_ANGLE = mm.deg2Rad(15.) # RightFoot_foot_0_0 : outside metatarsals capsulize('RightFoot_foot_0_0') node = mcfg.getNode('RightFoot_foot_0_0') node.bone_dir_child = 'RightFoot_foot_0_0_0' # third node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ 0., .5 * SEGMENT_METATARSAL_LEN * math.tan(THIRD_METATARSAL_ANGLE), 0. ]), mm.exp(THIRD_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(THIRD_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) # fourth node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-SEGMENT_BETWEEN_SPACE, 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) # node.jointType = footJointType node.jointType = 'B' capsulize('LeftFoot_foot_0_0') node = mcfg.getNode('LeftFoot_foot_0_0') node.bone_dir_child = 'LeftFoot_foot_0_0_0' node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ 0., .5 * SEGMENT_METATARSAL_LEN * math.tan(THIRD_METATARSAL_ANGLE), 0. ]), mm.exp(THIRD_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(THIRD_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([SEGMENT_BETWEEN_SPACE, 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) # node.jointType = footJointType node.jointType = 'B' # RightFoot_foot_0_0_0 : outside phalanges SEGMENT_FOURTH_PHA_OFFSET = .5 * SEGMENT_THIRD_PHA_LEN * ( 1. - SEGMENT_FOURTH_PHA_RATIO) capsulize('RightFoot_foot_0_0_0') node = mcfg.getNode('RightFoot_foot_0_0_0') # third node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([0., 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) # fourth node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([ -SEGMENT_BETWEEN_SPACE, 0., -SEGMENT_FOURTH_PHA_OFFSET ]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_THIRD_PHA_LEN * SEGMENT_FOURTH_PHA_RATIO + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] capsulize('LeftFoot_foot_0_0_0') node = mcfg.getNode('LeftFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([0., 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([ SEGMENT_BETWEEN_SPACE, 0., -SEGMENT_FOURTH_PHA_OFFSET ]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_THIRD_PHA_LEN * SEGMENT_FOURTH_PHA_RATIO + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] # RightFoot_foot_0_1 : inside metatarsals capsulize('RightFoot_foot_0_1') node = mcfg.getNode('RightFoot_foot_0_1') # second node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ 0., .5 * SEGMENT_METATARSAL_LEN * math.tan(SECOND_METATARSAL_ANGLE), 0. ]), mm.exp(SECOND_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(SECOND_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) # first node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ SEGMENT_BETWEEN_SPACE, .5 * SEGMENT_METATARSAL_LEN * math.tan(FIRST_METATARSAL_ANGLE), 0. ]), mm.exp(FIRST_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(FIRST_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['Z'] capsulize('LeftFoot_foot_0_1') node = mcfg.getNode('LeftFoot_foot_0_1') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ 0., .5 * SEGMENT_METATARSAL_LEN * math.tan(SECOND_METATARSAL_ANGLE), 0. ]), mm.exp(SECOND_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(SECOND_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ -SEGMENT_BETWEEN_SPACE, .5 * SEGMENT_METATARSAL_LEN * math.tan(FIRST_METATARSAL_ANGLE), 0. ]), mm.exp(FIRST_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(FIRST_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['Z'] # RightFoot_foot_0_1_0 : inside phalanges capsulize('RightFoot_foot_0_1_0') node = mcfg.getNode('RightFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([SEGMENT_BETWEEN_SPACE, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] capsulize('LeftFoot_foot_0_1_0') node = mcfg.getNode('LeftFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-SEGMENT_BETWEEN_SPACE, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] # RightFoot_foot_1_0 : center heel capsulize('RightFoot_foot_1_0') node = mcfg.getNode('RightFoot_foot_1_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-SEGMENT_BETWEEN_SPACE / 2., 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([+SEGMENT_BETWEEN_SPACE / 2., 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' capsulize('LeftFoot_foot_1_0') node = mcfg.getNode('LeftFoot_foot_1_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-SEGMENT_BETWEEN_SPACE / 2., 0., .0]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([+SEGMENT_BETWEEN_SPACE / 2., 0., .0]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' else: # SEGMENT_FOOT_INSIDE_FIRST # TODO: # adjust transformation of geometries # RightFoot_foot_0_1 : inside metatarsals capsulize('RightFoot_foot_0_1') node = mcfg.getNode('RightFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0., math.atan2(1.2, 2.5), 0.])], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_0_1_0 : inside phalanges capsulize('RightFoot_foot_0_1_0') node = mcfg.getNode('RightFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_0_0 : outside metatarsals capsulize('RightFoot_foot_0_0') node = mcfg.getNode('RightFoot_foot_0_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-0.3, 0., 2.5 * 0.25]), mm.exp([0., -math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-0.3 - 1.2, 0., 2.5 * 0.25]), mm.exp([0., -math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # RightFoot_foot_0_0_0 : outside phalanges capsulize('RightFoot_foot_0_0_0') node = mcfg.getNode('RightFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_1_0 : center heel capsulize('RightFoot_foot_1_0') node = mcfg.getNode('RightFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType capsulize('LeftFoot_foot_0_1') node = mcfg.getNode('LeftFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_0_1_0') node = mcfg.getNode('LeftFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_0_0') node = mcfg.getNode('LeftFoot_foot_0_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0.3, 0., 2.5 * 0.25]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0.3 + 1.2, 0., 2.5 * 0.25]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType capsulize('LeftFoot_foot_0_0_0') node = mcfg.getNode('LeftFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_1_0') node = mcfg.getNode('LeftFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # 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'] = 15000 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':1., 'Spine1':1., 'RightFoot':.5, 'LeftFoot':.5, 'Hips':1.5,\ #'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.} #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1., 'Spine1':1., 'RightFoot':1.0, 'LeftFoot':1.0, 'Hips':1.5,\ #'RightUpLeg':2., 'RightLeg':2., 'LeftUpLeg':2., 'LeftLeg':2.} config['weightMap'] = { 'RightArm': .2, 'RightForeArm': .2, 'LeftArm': .2, 'LeftForeArm': .2, 'Spine': .6, 'Spine1': .6, 'RightFoot': .2, 'LeftFoot': .2, 'Hips': 0.5, 'RightUpLeg': .1, 'RightLeg': .3, 'LeftUpLeg': .1, 'LeftLeg': .3 } if SEGMENT_FOOT: segfoot_weight = 10. # segfoot_weight = .1 config['weightMap'] = { 'RightArm': .2, 'RightForeArm': .2, 'LeftArm': .2, 'LeftForeArm': .2, 'Spine': .6, 'Spine1': .6, 'RightFoot': .2, 'LeftFoot': .2, 'Hips': 0.5, 'RightUpLeg': .1, 'RightLeg': .3, 'LeftUpLeg': .1, 'LeftLeg': .3, 'RightFoot_foot_0_0': segfoot_weight, 'RightFoot_foot_0_1': segfoot_weight, 'RightFoot_foot_1_0': segfoot_weight, 'RightFoot_foot_1_1': segfoot_weight, 'RightFoot_foot_1_2': segfoot_weight, 'RightFoot_foot_0_0_0': segfoot_weight, 'RightFoot_foot_0_1_0': segfoot_weight, 'LeftFoot_foot_0_0': segfoot_weight, 'LeftFoot_foot_0_1': segfoot_weight, 'LeftFoot_foot_1_0': segfoot_weight, 'LeftFoot_foot_1_1': segfoot_weight, 'LeftFoot_foot_1_2': segfoot_weight, 'LeftFoot_foot_0_0_0': segfoot_weight, 'LeftFoot_foot_0_1_0': segfoot_weight } #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':1., 'Hips':0.5,\ #'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.5, 'LeftLeg':1.5} #success!! ''' config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':.5, 'Spine1':.5, 'RightFoot':1., 'LeftFoot':1., 'Hips':0.5,\ 'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.} ''' #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['supLink1'] = 'LeftFoot' config['supLink2'] = 'RightFoot' #config['end'] = 'Hips' config['end'] = 'Spine1' return motion, mcfg, wcfg, stepsPerFrame, config, frame_rate
def handle(self, e): # print yse.Fl_Event.text[e], id(self) # print id(Fl.focus()) returnVal = 0 if e == FL_RELEASE: self.mouseX = Fl.event_x() self.mouseY = Fl.event_y() returnVal = 1 elif e == FL_PUSH: self.mouseX = Fl.event_x() self.mouseY = Fl.event_y() returnVal = 1 elif e == FL_DRAG: self.mouseX = Fl.event_x() self.mouseY = Fl.event_y() mouseDeltaX = self.mouseX - self.mousePrevX mouseDeltaY = self.mouseY - self.mousePrevY button = Fl.event_button() if button == 1: self.camera.rotateY -= mmMath.deg2Rad(mouseDeltaX) if self.viewMode == VIEW_PERSPECTIVE: self.camera.rotateX -= mmMath.deg2Rad(mouseDeltaY) elif button == 2: if self.viewMode != VIEW_TOP: self.camera.center[ 1] += self.camera.distance * .05 * mouseDeltaY / 12.0 elif button == 3: # right self.camera.center[0] -= self.camera.distance * .05 * math.cos( self.camera.rotateY) * mouseDeltaX / 12.0 self.camera.center[ 2] -= self.camera.distance * .05 * -math.sin( self.camera.rotateY) * mouseDeltaX / 12.0 if self.viewMode == VIEW_PERSPECTIVE or self.viewMode == VIEW_TOP: # look self.camera.center[ 0] -= self.camera.distance * .05 * math.sin( self.camera.rotateY) * mouseDeltaY / 12.0 self.camera.center[ 2] -= self.camera.distance * .05 * math.cos( self.camera.rotateY) * mouseDeltaY / 12.0 returnVal = 1 self.redraw() elif e == FL_MOUSEWHEEL: if self.wheelWork: self.camera.distance -= self.camera.distance * .2 * Fl.event_dy( ) / 3.0 # self.camera.distance -= Fl.event_dy() / 3.0 if self.camera.distance < 0.0001: self.camera.distance = 0.0001 self.projectionChanged = True returnVal = 1 self.redraw() # elif e == FL_ENTER: ## Fl.focus(self) # returnVal = 1 # elif e == FL_FOCUS: # returnVal = 1 elif e == FL_KEYUP: if Fl.event_key() == ord('1'): self.viewFromFront() elif Fl.event_key() == ord('2'): self.viewFromRight() elif Fl.event_key() == ord('3'): self.viewFromTop() elif Fl.event_key() == ord('4'): self.viewPerspective() elif Fl.event_key() == ord('6'): self.event_handle(1) elif Fl.event_key() == ord('7'): self.event_handle(2) elif Fl.event_key() == ord('8'): self.event_handle(3) elif Fl.event_key() == ord('9'): self.event_handle(4) elif Fl.event_key() == ord('0'): self.event_handle(5) # elif Fl.event_key()==ord('f'): # self.event_handle(5) # elif Fl.event_key()==ord('g'): # self.event_handle(6) # elif Fl.event_key()==ord('v'): # self.event_handle(7) # elif Fl.event_key()==ord('c'): # self.event_handle(8) # elif Fl.event_key()==ord('d'): # self.event_handle(9) returnVal = 0 self.redraw() self.mousePrevX = self.mouseX self.mousePrevY = self.mouseY # if returnVal == 1: # self.redraw() return returnVal