def editing1(filename, scale=1.): bvh = yf.readBvhFileAsBvh(filename) for joint in bvh.joints: if 'dummy' in joint.name.lower(): bvh.remove_joint_element(joint.name, preseve_child_offset=True) # elif 'toes' in joint.name.lower() and not('effector' in joint.name.lower()): # bvh.remove_joint_element(joint.name) # elif 'hand' in joint.name.lower() and not('effector' in joint.name.lower()): # bvh.remove_joint_element(joint.name) if bvh.joints[1].name == 'RightUpLeg': bvh.swap_joint('RightUpLeg', 'LeftUpLeg') bvh.set_scale(scale) bvh.joints[0].offset = np.zeros(3) bvh.writeBvhFile('./ori_motion/' + os.path.splitext(os.path.basename(filename))[0] + '_edit.bvh')
def build_graph(): # motion_files = ['wd2_jump0.bvh'] # motion_files = ['woody_walk_normal.bvh', 'wd2_jump0.bvh'] motion_files = ['woody_walk_normal.bvh'] n_motions = len(motion_files) motions = [] for i in range(n_motions): # motions.append(PmLinearMotion(human)) # motions[i].openAMC(motion_files[i], 1, 1) motions.append(yf.readBvhFileAsBvh(motion_files[i]).toPmLinearMotion(1., False)) # for i in range(skeleton.getElementNum()): # print(skeleton.getElement(i)) # create FlowGraph fGraph = FlowGraph() fGraph.init(n_motions, motions) fGraph.setLocalCoordinate(True) fGraph.setThreshold(1e-10) fGraph.setVariance(2.) fGraph.setMinJump(10) # fGraph.setMinJump(60) fGraph.setPelvisWeight(0.) # calculate distances between two frames pointSetFile = 'point_set.txt' distFile = 'dist.txt' poseDimension = fGraph.vectorize(pointSetFile) # frame distance fGraph.distance_matrix = AllPointKNearest(fGraph.getSize(), poseDimension, fGraph.getSize()//20, pointSetFile, distFile) # build up Motion-Graph fGraph.build(n_motions, motions) # seed = fGraph.stronglyConnectedComponents() # fGraph.preventDeadLock(seed) return fGraph
def __init__(self, env_name='walk', dart_skel_file='../data/wd2_seg.xml'): self.dart_skel_file = dart_skel_file self.world = pydart.World(1. / 1200., self.dart_skel_file) self.world.control_skel = self.world.skeletons[1] self.world.disable_recording() self.world.collision_auto_update = False self.skel = self.world.skeletons[1] self.Kp, self.Kd = 400., 40. self.env_name = env_name self.ref_motion = None # type: ym.Motion motion_name = None if env_name == 'walk': motion_name = "../data/segfoot_wd2_WalkForwardNormal00.bvh" elif env_name == 'walk_repeated': motion_name = "../data/segfoot_wd2_WalkForwardNormal00_REPEATED.bvh" elif env_name == 'walk_fast': motion_name = "../data/segfoot_wd2_WalkForwardVFast00.bvh" elif env_name == 'walk_sukiko': motion_name = '../data/segfoot_wd2_WalkSukiko00.bvh' elif env_name == 'walk_u_turn': motion_name = '../data/segfoot_wd2_u-turn_edit.bvh' elif env_name == '1foot_contact_run': motion_name = '../data/segfoot_wd2_1foot_contact_run2_edit.bvh' elif env_name == 'round_girl': motion_name = '../data/segfoot_wd2_boxing_round_round_girl1_edit.bvh' elif env_name == 'fast_2foot_hop': motion_name = '../data/segfoot_wd2_fast_2foot_hop_edit.bvh' elif env_name == 'slow_2foot_hop': motion_name = '../data/segfoot_wd2_slow_2foot_hop_edit.bvh' elif env_name == 'long_broad_jump': motion_name = '../data/segfoot_wd2_long_broad_jump_edit.bvh' elif env_name == 'short_broad_jump': motion_name = '../data/segfoot_wd2_short_broad_jump_edit.bvh' elif env_name == 'n_kick': motion_name = '../data/segfoot_wd2_n_kick_edit.bvh' elif env_name == 'jump': motion_name = '../data/wd2_jump0_edited.bvh' SEGMENT_FOOT_MAG = 0.01 SEGMENT_FOOT_RAD = 0.008 bvh = yf.readBvhFileAsBvh(motion_name) current_path = os.path.dirname(os.path.abspath(__file__)) partBvhFilePath = current_path + '/../../PyCommon/modules/samples/' partBvhFilePath = partBvhFilePath + 'foot_model_real_joint_01.bvh' partBvh = yf.readBvhFileAsBvh(partBvhFilePath) partSkeleton = partBvh.toJointSkeleton(1., False) bvh.replaceJointFromBvh('RightFoot', partBvh, SEGMENT_FOOT_MAG) partBvh = yf.readBvhFileAsBvh(partBvhFilePath) partBvh.mirror('YZ') bvh.replaceJointFromBvh('LeftFoot', partBvh, SEGMENT_FOOT_MAG) self.ref_motion = bvh.toJointMotion(1., False) # type: ym.JointMotion if env_name == 'walk': # self.ref_motion = self.ref_motion[20:] self.ref_motion.translateByOffset([0., -0.03, 0.]) if env_name == 'walk_repeated': # self.ref_motion = self.ref_motion[20:] self.ref_motion.translateByOffset([0., -0.03, 0.]) elif env_name == 'jump': self.ref_motion = self.ref_motion[164:280] self.ref_motion.translateByOffset([0., -0.06, 0.]) elif env_name == 'walk_u_turn': self.ref_motion = self.ref_motion[25:214] self.ref_motion.translateByOffset([0., -0.09, 0.]) elif env_name == 'jump_whole': self.ref_motion = self.ref_motion[315:966] elif env_name == 'walk_u_turn_whole': self.ref_motion.translateByOffset([0., 0.03, 0.]) elif env_name == '1foot_contact_run': self.ref_motion.translateByOffset([0., -0.09, 0.]) elif env_name == 'round_girl': self.ref_motion = self.ref_motion[505:658] self.ref_motion.translateByOffset([0., -0.01, 0.]) elif env_name == 'fast_2foot_hop': self.ref_motion.translateByOffset([0., -0.09, 0.]) elif env_name == 'slow_2foot_hop': self.ref_motion.translateByOffset([0., -0.09, 0.]) elif env_name == 'long_broad_jump': self.ref_motion.translateByOffset([0., -0.09, 0.]) elif env_name == 'short_broad_jump': self.ref_motion.translateByOffset([0., -0.09, 0.]) elif env_name == 'n_kick': self.ref_motion.translateByOffset([0., -0.09, 0.]) self.ref_world = pydart.World(1. / 1200., "../data/wd2_seg.xml") self.ref_skel = self.ref_world.skeletons[1] self.step_per_frame = round( (1. / self.world.time_step()) / self.ref_motion.fps) # self.step_per_frame = 40 fix_motion_data_by_foot(self.ref_motion, self.ref_skel, SEGMENT_FOOT_RAD) self.rsi = True self.rsi_num = None # self.w_p = 0.65 self.w_p = 0.50 self.w_v = 0.1 self.w_e = 0.10 self.w_c = 0.1 self.w_t = 0.1 self.w_y_ankle = 0.1 self.exp_p = 2. self.exp_v = 0.1 self.exp_e = 40. self.exp_c = 10. self.exp_t = 20. self.exp_y_ankle = 5. # soohwan style # self.w_p = 0.15 # self.w_v = 0.05 # self.w_c = 0.4 # self.w_c_v = 0.05 # self.w_e = 0.2 # self.w_e_ori = 0.05 # # self.exp_p = 2. # self.exp_v = 20. # self.exp_c = .3 # self.exp_c_v = 2. # self.exp_e = 2. # self.exp_e_ori = 2. self.body_num = self.skel.num_bodynodes() self.idx_e = [ self.skel.bodynode_index('LeftFoot'), self.skel.bodynode_index('RightFoot'), self.skel.bodynode_index('LeftForeArm'), self.skel.bodynode_index('RightForeArm') ] self.ref_body_e = list(map(self.ref_skel.body, self.idx_e)) self.ankle_joint_names = ['j_LeftFoot', 'j_RightFoot'] self.motion_len = len(self.ref_motion) self.motion_time = len(self.ref_motion) / self.ref_motion.fps self.time_offset = 0. self.viewer = None self.phase_frame = 0 self.prev_ref_q = np.zeros(self.ref_skel.num_dofs()) self.prev_ref_dq = np.zeros(self.ref_skel.num_dofs()) self.prev_ref_p_e_hat = np.asarray([ body.world_transform()[:3, 3] for body in self.ref_body_e ]).flatten() self.prev_ref_p_e_ori_hat = [ body.world_transform()[:3, :3] for body in self.ref_body_e ] self.prev_ref_com = self.ref_skel.com() self.prev_ref_com_vel = self.ref_skel.com_velocity() self.prev_ref_com_spatial_vel = self.ref_skel.com_spatial_velocity() self.prev_ref_torso_ori = self.ref_skel.body( 'Spine').world_transform()[:3, :3] self.prev_ref_ankle_joint_y_pos = np.asarray([ joint.get_world_frame_after_transform()[1, 3] for joint in [ self.ref_skel.joint(joint_name) for joint_name in self.ankle_joint_names ] ]).flatten() # setting for reward self.reward_joint = list() self.reward_joint.append('j_Hips') self.reward_joint.append('j_RightUpLeg') self.reward_joint.append('j_RightLeg') self.reward_joint.append('j_LeftUpLeg') self.reward_joint.append('j_LeftLeg') self.reward_joint.append('j_Spine') self.reward_joint.append('j_Spine1') self.reward_joint.append('j_RightArm') self.reward_joint.append('j_RightForeArm') self.reward_joint.append('j_LeftArm') self.reward_joint.append('j_LeftForeArm') # setting for pd gain self.foot_joint = list() self.foot_joint.append('j_RightFoot') self.foot_joint.append('j_RightFoot_foot_0_0') self.foot_joint.append('j_RightFoot_foot_0_0_0') self.foot_joint.append('j_RightFoot_foot_0_1_0') self.foot_joint.append('j_RightFoot_foot_1_0') self.foot_joint.append('j_LeftFoot') self.foot_joint.append('j_LeftFoot_foot_0_0') self.foot_joint.append('j_LeftFoot_foot_0_0_0') self.foot_joint.append('j_LeftFoot_foot_0_1_0') self.foot_joint.append('j_LeftFoot_foot_1_0') state_num = 1 + (3 * 3 + 4) * self.body_num action_num = self.skel.num_dofs() - 6 + len(self.foot_joint) state_high = np.array([np.finfo(np.float32).max] * state_num) action_high = np.array([pi * 10.] * action_num) self.action_space = gym.spaces.Box(-action_high, action_high, dtype=np.float32) self.observation_space = gym.spaces.Box(-state_high, state_high, dtype=np.float32) self.force_done = False self.applied_force = np.zeros(3) self.applied_force_offset = np.zeros(3)
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 attachPartBvh(self, partFilePath, attachPart, scale=1.0, mirror=None): partBvh = yf.readBvhFileAsBvh(partFilePath) if mirror is not None: partBvh.mirror(mirror) self.bvh.replaceJointFromBvh(attachPart, partBvh, scale)
def setMainBvh(self, filename): self.bvh = yf.readBvhFileAsBvh(filename) self.skelname = filename.split("/")[-1].split(".")[0]
def create_biped(SEGMENT_FOOT=True, SEGMENT_FOOT_MAG=.03): SEGMENT_FOOT_RAD = SEGMENT_FOOT_MAG * .5 #motion motionName = 'wd2_n_kick.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/simpleJump_long_test3.bvh' partBvh = yf.readBvhFileAsBvh(partBvhFilePath) 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) # 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.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') yme.updateGlobalT(motion) #motion.translateByOffset((0, -0.07, 0)) #motion = motion[40:-58] #motion[0:0] = [motion[0]]*20 #motion.extend([motion[-1]]*5000) # motion = motion[40:] #motion[0:0] = [motion[0]]*50 #motion.extend([motion[-1]]*5000) #motion = motion[30:151] motion = motion[30:] #motion = motion[30:31] #motion[5:5] = [motion[5]]*30 # motion[0:0] = [motion[0]]*2000 # motion.extend([motion[-1]]*300) # motion = motion[37:] motion[0:0] = [motion[0]] * 2000 motion.extend([motion[-1]] * 300) #motion = motion[40:41] #motion[0:0] = [motion[0]]*5000 #motion = motion[56:-248] #motion = motion[-249:-248] #motion[0:0] = [motion[0]]*10 #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] 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 = .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. # 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.addGeom('MyFoot4', [0.02*np.array([-1.2, 0., 0.]), mm.exp([0., 0., 0.])], ypc.CapsuleMaterial(1000., .01, -1)) 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., .4]), 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., .4]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) # node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(1000., .01, -1)) node.jointType = footJointType # RightFoot_foot_1_1 : inside heel # capsulize('RightFoot_foot_1_1') # node = mcfg.getNode('RightFoot_foot_1_1') # node.addGeom('MyFoot3', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) # node.jointType = footJointType # RightFoot_foot_1_2 : outside heel # capsulize('RightFoot_foot_1_2') # node = mcfg.getNode('RightFoot_foot_1_2') # node.addGeom('MyFoot3', [np.array([0.]*3), 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.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(1000., .01, -1)) 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.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(1000., .01, -1)) node.jointType = footJointType # capsulize('LeftFoot_foot_1_1') # node = mcfg.getNode('LeftFoot_foot_1_1') # node.addGeom('MyFoot3', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) # node.jointType = footJointType # capsulize('LeftFoot_foot_1_2') # node = mcfg.getNode('LeftFoot_foot_1_2') # node.addGeom('MyFoot3', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) # 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'] = 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':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 = .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 __init__(self, env_name='walk'): motionFile = '../data/wd2_tiptoe_zygote.bvh' motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_MAG=0.01, SEGMENT_FOOT_RAD=0.008, motion_scale=.01) self.world = cvw.VpWorld(wcfg) self.world.SetGlobalDamping(0.999) self.skel = cvm.VpControlModel(self.world, motion[0], mcfg) self.world.initialize() self.skel.initializeHybridDynamics() self.Kp, self.Kd = 400., 40. self.Ks, self.Ds = config['Ks'], config['Ds'] self.env_name = env_name self.ref_motion = None # type: ym.Motion motion_name = None if env_name == 'walk': motion_name = "../data/segfoot_wd2_WalkForwardNormal00.bvh" elif env_name == 'walk_fast': motion_name = "../data/segfoot_wd2_WalkForwardVFast00.bvh" elif env_name == 'walk_sukiko': motion_name = '../data/segfoot_wd2_WalkSukiko00.bvh' SEGMENT_FOOT_MAG = 0.01 SEGMENT_FOOT_RAD = 0.008 bvh = yf.readBvhFileAsBvh(motion_name) current_path = os.path.dirname(os.path.abspath(__file__)) partBvhFilePath = current_path + '/../../PyCommon/modules/samples/' partBvhFilePath = partBvhFilePath + 'foot_model_real_joint_01.bvh' partBvh = yf.readBvhFileAsBvh(partBvhFilePath) partSkeleton = partBvh.toJointSkeleton(1., False) bvh.replaceJointFromBvh('RightFoot', partBvh, SEGMENT_FOOT_MAG) partBvh = yf.readBvhFileAsBvh(partBvhFilePath) partBvh.mirror('YZ') bvh.replaceJointFromBvh('LeftFoot', partBvh, SEGMENT_FOOT_MAG) self.ref_motion = bvh.toJointMotion(1., False) # type: ym.JointMotion if env_name == 'walk': # self.ref_motion = self.ref_motion[20:] self.ref_motion.translateByOffset([0., -0.03, 0.]) elif env_name == 'jump': self.ref_motion = self.ref_motion[164:280] elif env_name == 'walk_u_turn': self.ref_motion = self.ref_motion[25:214] self.ref_motion.translateByOffset([0., 0.03, 0.]) elif env_name == 'jump_whole': self.ref_motion = self.ref_motion[315:966] elif env_name == 'walk_u_turn_whole': self.ref_motion.translateByOffset([0., 0.03, 0.]) ref_wcfg = copy.deepcopy(wcfg) self.ref_world = cvw.VpWorld(ref_wcfg) self.ref_skel = cvm.VpControlModel(self.ref_world, motion[0], mcfg) self.ref_world.initialize() self.ref_skel.set_q(np.zeros_like(self.skel.get_q())) self.step_per_frame = stepsPerFrame fix_motion_data_by_foot(self.ref_motion, self.ref_skel, self.ref_world) self.rsi = True self.w_p = 0.65 self.w_v = 0.1 self.w_e = 0.15 self.w_c = 0.1 self.exp_p = 2. self.exp_v = 0.1 self.exp_e = 40. self.exp_c = 10. self.body_num = self.skel.getBodyNum() self.idx_e = [self.skel.name2index('LeftFoot'), self.skel.name2index('RightFoot'), self.skel.name2index('LeftForeArm'), self.skel.name2index('RightForeArm')] # self.body_e = list(map(self.skel.body, self.idx_e)) # self.ref_body_e = list(map(self.ref_skel.body, self.idx_e)) self.motion_len = len(self.ref_motion) self.motion_time = len(self.ref_motion) / self.ref_motion.fps self.time_offset = 0. self.viewer = None self.phase_frame = 0 self.prev_ref_q = self.ref_motion.getDOFPositions(self.phase_frame) self.prev_ref_dq = self.ref_motion.getDOFVelocities(self.phase_frame) self.prev_ref_p_e_hat = np.asarray([self.ref_skel.getBodyPositionGlobal(body_idx) for body_idx in self.idx_e]).flatten() self.prev_ref_com = self.ref_skel.getCOM() # setting for reward self.reward_joint = list() # self.reward_joint.append('Hips') self.reward_joint.append('RightUpLeg') self.reward_joint.append('RightLeg') self.reward_joint.append('LeftUpLeg') self.reward_joint.append('LeftLeg') self.reward_joint.append('Spine') self.reward_joint.append('Spine1') self.reward_joint.append('RightArm') self.reward_joint.append('RightForeArm') self.reward_joint.append('LeftArm') self.reward_joint.append('LeftForeArm') # setting for pd gain self.foot_joint = list() self.foot_joint.append('RightFoot') self.foot_joint.append('RightFoot_foot_0_0') self.foot_joint.append('RightFoot_foot_0_0_0') self.foot_joint.append('RightFoot_foot_0_1_0') self.foot_joint.append('RightFoot_foot_1_0') self.foot_joint.append('LeftFoot') self.foot_joint.append('LeftFoot_foot_0_0') self.foot_joint.append('LeftFoot_foot_0_0_0') self.foot_joint.append('LeftFoot_foot_0_1_0') self.foot_joint.append('LeftFoot_foot_1_0') state_num = 1 + (3*3 + 4) * self.body_num action_num = self.skel.getTotalDOF() - 6 state_high = np.array([np.finfo(np.float32).max] * state_num) action_high = np.array([pi*10.] * action_num) self.action_space = gym.spaces.Box(-action_high, action_high, dtype=np.float32) self.observation_space = gym.spaces.Box(-state_high, state_high, dtype=np.float32)
def __init__(self, env_name='walk'): self.world = pydart.World(1. / 1800., "../data/wd2_without_ground.xml") # self.world.control_skel = self.world.skeletons[1] # self.skel = self.world.skeletons[1] self.world.control_skel = self.world.skeletons[0] self.skel = self.world.skeletons[0] self.Kp, self.Kd = 400., 40. self.env_name = env_name self.ref_motion = None # type: ym.Motion motion_name = None if env_name == 'walk': motion_name = "../data/segfoot_wd2_WalkForwardNormal00.bvh" elif env_name == 'walk_fast': motion_name = "../data/segfoot_wd2_WalkForwardVFast00.bvh" elif env_name == 'walk_sukiko': motion_name = '../data/segfoot_wd2_WalkSukiko00.bvh' SEGMENT_FOOT_MAG = 0.01 SEGMENT_FOOT_RAD = 0.008 bvh = yf.readBvhFileAsBvh(motion_name) current_path = os.path.dirname(os.path.abspath(__file__)) partBvhFilePath = current_path + '/../../PyCommon/modules/samples/' partBvhFilePath = partBvhFilePath + 'foot_model_real_joint_01.bvh' partBvh = yf.readBvhFileAsBvh(partBvhFilePath) partSkeleton = partBvh.toJointSkeleton(1., False) bvh.replaceJointFromBvh('RightFoot', partBvh, SEGMENT_FOOT_MAG) partBvh = yf.readBvhFileAsBvh(partBvhFilePath) partBvh.mirror('YZ') bvh.replaceJointFromBvh('LeftFoot', partBvh, SEGMENT_FOOT_MAG) self.ref_motion = bvh.toJointMotion(1., False) # type: ym.JointMotion if env_name == 'walk': # self.ref_motion = self.ref_motion[20:] self.ref_motion.translateByOffset([0., -0.03, 0.]) elif env_name == 'jump': self.ref_motion = self.ref_motion[164:280] elif env_name == 'walk_u_turn': self.ref_motion = self.ref_motion[25:214] self.ref_motion.translateByOffset([0., 0.03, 0.]) elif env_name == 'jump_whole': self.ref_motion = self.ref_motion[315:966] elif env_name == 'walk_u_turn_whole': self.ref_motion.translateByOffset([0., 0.03, 0.]) self.ref_world = pydart.World(1. / 1200., "../data/wd2_without_ground.xml") # self.ref_skel = self.ref_world.skeletons[1] self.ref_skel = self.ref_world.skeletons[0] # self.step_per_frame = round((1./self.world.time_step()) / self.ref_motion.fps) self.step_per_frame = 40 self.rsi = True self.w_p = 0.65 self.w_v = 0.1 self.w_e = 0.15 self.w_c = 0.1 self.exp_p = 2. self.exp_v = 0.1 self.exp_e = 40. self.exp_c = 10. self.body_num = self.skel.num_bodynodes() self.idx_e = [ self.skel.bodynode_index('LeftFoot'), self.skel.bodynode_index('RightFoot'), self.skel.bodynode_index('LeftForeArm'), self.skel.bodynode_index('RightForeArm') ] self.body_e = list(map(self.skel.body, self.idx_e)) self.ref_body_e = list(map(self.ref_skel.body, self.idx_e)) self.motion_len = len(self.ref_motion) self.motion_time = len(self.ref_motion) / self.ref_motion.fps self.time_offset = 0. self.viewer = None self.phase_frame = 0 self.prev_ref_q = np.zeros(self.ref_skel.num_dofs()) self.prev_ref_dq = np.zeros(self.ref_skel.num_dofs()) self.prev_ref_p_e_hat = np.asarray([ body.world_transform()[:3, 3] for body in self.ref_body_e ]).flatten() self.prev_ref_com = self.ref_skel.com() # setting for reward self.reward_joint = list() self.reward_joint.append('j_Hips') self.reward_joint.append('j_RightUpLeg') self.reward_joint.append('j_RightLeg') self.reward_joint.append('j_LeftUpLeg') self.reward_joint.append('j_LeftLeg') self.reward_joint.append('j_Spine') self.reward_joint.append('j_Spine1') self.reward_joint.append('j_RightArm') self.reward_joint.append('j_RightForeArm') self.reward_joint.append('j_LeftArm') self.reward_joint.append('j_LeftForeArm') # setting for pd gain self.foot_joint = list() self.foot_joint.append('j_RightFoot') self.foot_joint.append('j_RightFoot_foot_0_0') self.foot_joint.append('j_RightFoot_foot_0_0_0') self.foot_joint.append('j_RightFoot_foot_0_1_0') self.foot_joint.append('j_RightFoot_foot_1_0') self.foot_joint.append('j_LeftFoot') self.foot_joint.append('j_LeftFoot_foot_0_0') self.foot_joint.append('j_LeftFoot_foot_0_0_0') self.foot_joint.append('j_LeftFoot_foot_0_1_0') self.foot_joint.append('j_LeftFoot_foot_1_0') state_num = 1 + (3 * 3 + 4) * self.body_num action_num = self.skel.num_dofs() - 6 + len(self.foot_joint) state_high = np.array([np.finfo(np.float32).max] * state_num) action_high = np.array([pi * 10.] * action_num) self.action_space = gym.spaces.Box(-action_high, action_high, dtype=np.float32) self.observation_space = gym.spaces.Box(-state_high, state_high, dtype=np.float32)
PPM = 50. SCREEN_WIDTH = 1280 SCREEN_HEIGHT = 240 TARGET_FPS = 40 TIME_STEP = 1.0 / TARGET_FPS FRAME_PER_TIME_STEP = 25 WORLD_TIME_STEP = 1. / (TARGET_FPS * FRAME_PER_TIME_STEP) pydart.init() current_path = os.path.dirname(os.path.abspath(__file__)) motionDir = current_path + '/../DartWalkingFoot/ppmotion/' filename = 'segfoot_wd2_WalkForwardNormal00.bvh' bvh = yf.readBvhFileAsBvh(motionDir + filename) motion_ori = bvh.toJointMotion(1., False) # motion_ori = yf.readBvhFile(motionDir+filename) frameTime = 1 / motion_ori.fps mcfg = buildMcfg() c_locking_vel = .05 wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False wcfg.lockingVel = c_locking_vel stepsPerFrame = 50 wcfg.timeStep = frameTime / stepsPerFrame
from PyCommon.modules.Resource import ysMotionLoader as yf import PyCommon.modules.Motion.ysSkeletonEdit as yse from PyCommon.modules.Math import mmMath as mm bvh = yf.readBvhFileAsBvh('wd2_jump0.bvh') print(bvh) bvh.swap_joint('RightUpLeg', 'LeftUpLeg') print(bvh) # bvh.writeBvhFile('wd2_jump0_edited.bvh') motion = bvh.toJointMotion(scale=1., applyRootOffset=False) yse.rotateJointLocal(motion, 'LeftFoot', mm.rotX(0.4), False) yse.rotateJointLocal(motion, 'RightFoot', mm.rotX(0.4), False) yf.writeBvhFile('wd2_jump0_edited.bvh', motion)
def main2(): import PyCommon.modules.Motion.ysMotionAnalysis as yma pydart.init() world = [ pydart.World(1. / 1200., "../DartDeep/data/woody_with_ground_v2.xml") for _ in range(2) ] motion_files = ['wd2_jump0.bvh'] motion_files = ['woody_walk_normal.bvh'] motion_files = ['walk_left_90degree.bvh'] motion = yf.readBvhFileAsBvh(motion_files[0]).toPmLinearMotion(1., False) # 70, 112, 156 right heel strike mg = hmg.MotionGraph() mg.add_motion(motion) mg.build() total_num = len(mg.transition_processed) # viewer settings rd_contact_positions = [None] rd_contact_forces = [None] viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False) viewer.doc.addRenderer('motionModel[0]', yr.DartRenderer(world[0], (255, 240, 255))) viewer.doc.addRenderer('motionModel[1]', yr.DartRenderer(world[1])) motion_state = [0] def callback_0(_): motion_state[0] = 0 def callback_1(_): motion_state[0] = 1 def callback_2(_): motion_state[0] = 2 viewer.objectInfoWnd.addBtn('0', callback_0) viewer.objectInfoWnd.addBtn('1', callback_1) viewer.objectInfoWnd.addBtn('2', callback_2) hRef = 0.1 vRef = 0.4 lc = yma.getElementContactStates(motion, 'LeftFoot', hRef, vRef) rc = yma.getElementContactStates(motion, 'RightFoot', hRef, vRef) lindex = motion[0].skeleton.getElementIndex('LeftFoot') rindex = motion[0].skeleton.getElementIndex('RightFoot') def postCallback(frame): transition = mg.transition_processed[frame] world[0].skeletons[1].set_positions( motion.get_q(transition.motion_from_idx)) world[1].skeletons[1].set_positions( motion.get_q(transition.motion_to_idx)) print(frame, transition.motion_from_idx, transition.motion_to_idx, transition.dist) print(motion.getPosition(lindex)) print(lc[transition.motion_from_idx], rc[transition.motion_from_idx]) print(lc[transition.motion_to_idx], rc[transition.motion_to_idx]) viewer.setPreFrameCallback_Always(postCallback) viewer.setMaxFrame(total_num - 1) viewer.startTimer(1. / 30.) viewer.show() all_node_set = set() dot = Digraph() for i in range(len(mg.transition_processed)): all_node_set.add(mg.transition_processed[i].motion_from_idx) all_node_set.add(mg.transition_processed[i].motion_to_idx) all_node_list = list(all_node_set) all_node_list.sort() for i in all_node_list: dot.node(str(i)) all_edge_set = set() for i in range(len(all_node_list) - 1): all_edge_set.add((all_node_list[i], all_node_list[i + 1])) for i in range(len(mg.transition_processed)): all_edge_set.add((mg.transition_processed[i].motion_from_idx, mg.transition_processed[i].motion_to_idx)) for i in list(all_edge_set): dot.edge(str(i[0]), str(i[1])) # print(dot.source) dot.render('test', view=True) dot.pipe() Fl.run()