def calc_rep_global_poses(self, fno: int, data_bone_name_list: list): rep_links = [] for (data_set_idx, bone_name) in data_bone_name_list: # 処理対象カメラオプション camera_option = self.camera_options[data_set_idx] # 処理対象のボーンを計算するためのリンク rep_link = camera_option.rep_links[bone_name] if (data_set_idx, rep_link) not in rep_links: # まだ保持されていないリンクなら保持 rep_links.append((data_set_idx, rep_link)) rep_global_poses = {} for (data_set_idx, rep_link) in rep_links: # 処理対象データセット data_set = self.options.data_set_list[data_set_idx] # 処理対象カメラオプション camera_option = self.camera_options[data_set_idx] # 先モデルのそれぞれのグローバル位置 rep_global_3ds = MServiceUtils.calc_global_pos( data_set.rep_model, rep_link, data_set.motion, fno) for bone_name, rep_vec in rep_global_3ds.items(): if (data_set_idx, bone_name) in data_bone_name_list: # 処理対象ボーンである場合、データを保持 rep_global_poses[(data_set_idx, bone_name)] = rep_vec.data() return rep_global_poses
def calc_org_project_square_poses(self, fno: int, cf: VmdCameraFrame, start_idx: int, end_idx: int, all_org_global_poses: dict, all_org_project_square_poses: dict): for data_set_idx, camera_option in self.camera_options.items(): for link_idx, org_link in enumerate( camera_option.org_links[start_idx:end_idx]): if len(org_link.all().keys()) == 0: # 処理対象がなければスルー continue # 処理対象データセット data_set = self.options.data_set_list[data_set_idx] # 元モデルのそれぞれのグローバル位置 org_global_3ds = MServiceUtils.calc_global_pos( data_set.camera_org_model, org_link, data_set.org_motion, fno) for bone_name, org_vec in org_global_3ds.items(): if bone_name in camera_option.org_link_target.keys(): # 処理対象ボーンである場合、データを保持 all_org_global_poses[(data_set_idx, bone_name)] = org_vec.data() all_org_project_square_poses[( data_set_idx, bone_name)] = self.calc_project_square_pos( cf, org_vec).data() [ logger.test("f: %s, k: %s, v: %s, s: %s", fno, k, v, sv) for (k, v), (sk, sv) in zip(all_org_global_poses.items(), all_org_project_square_poses.items()) ]
def calc_local_axis(model, bone_name): # 定義されていないのも含め全ボーンリンクを取得する links = model.create_link_2_top_one(bone_name, is_defined=False) # 初期スタンスのボーングローバル位置と行列を取得 global_3ds_dic, total_mats = MServiceUtils.calc_global_pos( model, links, VmdMotion(), 0, return_matrix=True, is_local_x=True) # target_mat = MMatrix4x4() # target_mat.setToIdentity() # # 処理対象行列 # for n, (lname, mat) in enumerate(total_mats.items()): # target_link = links.get(lname) # if n == 0: # # 最初は行列そのもの # target_mat = mat.copy() # else: # # 2番目以降は行列をかける # target_mat *= mat.copy() # if n > 0: # # ボーン自身にローカル軸が設定されているか # local_x_matrix = MMatrix4x4() # local_x_matrix.setToIdentity() # local_axis_qq = MQuaternion() # if target_link.local_x_vector == MVector3D(): # # ローカル軸が設定されていない場合、計算 # # 自身から親を引いた軸の向き # local_axis = target_link.position - links.get(lname, offset=-1).position # local_axis_qq = MQuaternion.fromDirection(local_axis.normalized(), MVector3D(0, 0, 1)) # else: # # ローカル軸が設定されている場合、その値を採用 # local_axis_qq = MQuaternion.fromDirection(target_link.local_x_vector.normalized(), MVector3D(0, 0, 1)) # local_x_matrix.rotate(local_axis_qq) # target_mat *= local_x_matrix # ワールド座標系から注目ノードの局所座標系への変換 inv_coord = total_mats[bone_name].inverted() # 自身のボーンからの向き先を取得 axis = model.get_local_x_axis(bone_name) # 最終的な対象ボーンのローカル軸の向き local_axis = (inv_coord * axis).normalized() return local_axis
def convert_target_ik2fk(self, ik_bone: Bone): motion = self.options.motion model = self.options.model bone_name = ik_bone.name logger.info("-- 腕IK変換準備:開始【%s】", bone_name) # モデルのIKボーンのリンク target_links = model.create_link_2_top_one(bone_name, is_defined=False) # モデルの人差し指先ボーンのリンク finger_bone_name = "{0}人指先実体".format(bone_name[0]) finger2_bone_name = "{0}小指先実体".format(bone_name[0]) wrist_bone_name = "{0}手首".format(bone_name[0]) finger_links = model.create_link_2_top_one(finger_bone_name, is_defined=False) finger2_links = model.create_link_2_top_one(finger2_bone_name, is_defined=False) # モデルのIKリンク if not (ik_bone.ik.target_index in model.bone_indexes and model.bone_indexes[ik_bone.ik.target_index] in model.bones): raise SizingException("{0} のTargetが有効なINDEXではありません。PMXの構造を確認してください。".format(bone_name)) # IKエフェクタ effector_bone_name = model.bone_indexes[ik_bone.ik.target_index] effector_links = model.create_link_2_top_one(effector_bone_name, is_defined=False) ik_links = BoneLinks() # 末端にエフェクタ effector_bone = model.bones[effector_bone_name].copy() effector_bone.degree_limit = math.degrees(ik_bone.ik.limit_radian) ik_links.append(effector_bone) for ik_link in ik_bone.ik.link: # IKリンクを末端から順に追加 if not (ik_link.bone_index in model.bone_indexes and model.bone_indexes[ik_link.bone_index] in model.bones): raise SizingException("{0} のLinkに無効なINDEXが含まれています。PMXの構造を確認してください。".format(bone_name)) link_bone = model.bones[model.bone_indexes[ik_link.bone_index]].copy() if link_bone.fixed_axis != MVector3D(): # 捩り系は無視 continue # 単位角 link_bone.degree_limit = math.degrees(ik_bone.ik.limit_radian) # # 角度制限 # if ik_link.limit_angle == 1: # link_bone.limit_min = ik_link.limit_min # link_bone.limit_max = ik_link.limit_max ik_links.append(link_bone) # 回転移管先ボーン transferee_bone = self.get_transferee_bone(ik_bone, effector_bone) # 移管先ボーンのローカル軸 transferee_local_x_axis = model.get_local_x_axis(transferee_bone.name) # 差異の大きい箇所にFKキーフレ追加 fnos = motion.get_differ_fnos(0, [bone_name], limit_degrees=20, limit_length=0.5) prev_sep_fno = 0 for fno in fnos: for link_name in list(ik_links.all().keys())[1:]: bf = motion.calc_bf(link_name, fno) motion.regist_bf(bf, link_name, fno) if fno // 500 > prev_sep_fno and fnos[-1] > 0: logger.count(f"【キーフレ追加 - {bone_name}】", fno, fnos) prev_sep_fno = fno // 500 logger.info("-- 腕IK変換準備:終了【%s】", bone_name) org_motion = motion.copy() # 元モーションを保持したら、IKキーフレ削除 del motion.bones[bone_name] for fno in fnos: # グローバル位置計算(元モーションの位置) target_ik_global_3ds = MServiceUtils.calc_global_pos(model, target_links, org_motion, fno) finger_global_3ds = MServiceUtils.calc_global_pos(model, finger_links, org_motion, fno) finger2_global_3ds = MServiceUtils.calc_global_pos(model, finger2_links, org_motion, fno) target_effector_pos = target_ik_global_3ds[bone_name] prev_diff = MVector3D() org_bfs = {} for link_name in list(ik_links.all().keys())[1:]: # 元モーションの角度で保持 bf = org_motion.calc_bf(link_name, fno).copy() org_bfs[link_name] = bf # 今のモーションも前のキーフレをクリアして再セット motion.regist_bf(bf, link_name, fno) # IK計算実行 for ik_cnt in range(50): MServiceUtils.calc_IK(model, effector_links, motion, fno, target_effector_pos, ik_links, max_count=1) # どちらにせよ一旦bf確定 for link_name in list(ik_links.all().keys())[1:]: ik_bf = motion.calc_bf(link_name, fno) motion.regist_bf(ik_bf, link_name, fno) # 現在のエフェクタ位置 now_global_3ds = MServiceUtils.calc_global_pos(model, effector_links, motion, fno) now_effector_pos = now_global_3ds[effector_bone_name] # 現在のエフェクタ位置との差分(エフェクタ位置が指定されている場合のみ) diff_pos = MVector3D() if target_effector_pos == MVector3D() else target_effector_pos - now_effector_pos if prev_diff == MVector3D() or (prev_diff != MVector3D() and diff_pos.length() < prev_diff.length()): if diff_pos.length() < 0.1: logger.debug("☆腕IK変換成功(%s): f: %s(%s), 指定 [%s], 現在[%s], 差異[%s(%s)]", ik_cnt, fno, bone_name, \ target_effector_pos.to_log(), now_effector_pos.to_log(), diff_pos.to_log(), diff_pos.length()) # org_bfを保持し直し for link_name in list(ik_links.all().keys())[1:]: bf = motion.calc_bf(link_name, fno).copy() org_bfs[link_name] = bf logger.test("org_bf保持: %s [%s]", link_name, bf.rotation.toEulerAngles().to_log()) # そのまま終了 break elif prev_diff == MVector3D() or diff_pos.length() < prev_diff.length(): logger.debug("☆腕IK変換ちょっと失敗採用(%s): f: %s(%s), 指定 [%s], 現在[%s], 差異[%s(%s)]", ik_cnt, fno, bone_name, \ target_effector_pos.to_log(), now_effector_pos.to_log(), diff_pos.to_log(), diff_pos.length()) # org_bfを保持し直し for link_name in list(ik_links.all().keys())[1:]: bf = motion.calc_bf(link_name, fno).copy() org_bfs[link_name] = bf logger.test("org_bf保持: %s [%s]", link_name, bf.rotation.toEulerAngles().to_log()) # 前回とまったく同じ場合か、充分に近い場合、IK的に動きがないので終了 if prev_diff == diff_pos or np.count_nonzero(np.where(np.abs(diff_pos.data()) > 0.05, 1, 0)) == 0: logger.debug("動きがないので終了") break # 前回差異を保持 prev_diff = diff_pos else: logger.debug("★腕IK変換ちょっと失敗不採用(%s): f: %s(%s), 指定 [%s], 現在[%s], 差異[%s(%s)]", ik_cnt, fno, bone_name, \ target_effector_pos.to_log(), now_effector_pos.to_log(), diff_pos.to_log(), diff_pos.length()) # 前回とまったく同じ場合か、充分に近い場合、IK的に動きがないので終了 if prev_diff == diff_pos or np.count_nonzero(np.where(np.abs(diff_pos.data()) > 0.05, 1, 0)) == 0: break else: logger.debug("★腕IK変換失敗(%s): f: %s(%s), 指定 [%s], 現在[%s], 差異[%s(%s)]", ik_cnt, fno, bone_name, \ target_effector_pos.to_log(), now_effector_pos.to_log(), diff_pos.to_log(), diff_pos.length()) # 前回とまったく同じ場合か、充分に近い場合、IK的に動きがないので終了 if prev_diff == diff_pos or np.count_nonzero(np.where(np.abs(diff_pos.data()) > 0.05, 1, 0)) == 0: logger.debug("動きがないので終了") break # 最後に成功したところに戻す for link_name in list(ik_links.all().keys())[1:]: bf = org_bfs[link_name].copy() logger.debug("確定bf: %s [%s]", link_name, bf.rotation.toEulerAngles().to_log()) motion.regist_bf(bf, link_name, fno) # 指先の現在の位置を再計算 if finger_links and finger_links.get(finger_bone_name) and finger_links.get(wrist_bone_name) and \ finger2_links and finger2_links.get(finger2_bone_name) and finger2_links.get(wrist_bone_name) and transferee_bone.name == wrist_bone_name: # 手首がある場合のみ、再計算 now_finger_global_3ds, now_finger_matrixs = MServiceUtils.calc_global_pos(model, finger_links, motion, fno, return_matrix=True) # 人指先のローカル位置 finger_initial_local_pos = now_finger_matrixs[wrist_bone_name].inverted() * now_finger_global_3ds[finger_bone_name] finger_local_pos = now_finger_matrixs[wrist_bone_name].inverted() * finger_global_3ds[finger_bone_name] finger_rotation = MQuaternion.rotationTo(finger_initial_local_pos, finger_local_pos) logger.debug("finger_rotation: %s [%s]", finger_bone_name, finger_rotation.toEulerAngles().to_log()) finger_x_qq, finger_y_qq, finger_z_qq, _ = MServiceUtils.separate_local_qq(fno, bone_name, finger_rotation, transferee_local_x_axis) logger.debug("finger_x_qq: %s [%s]", finger_bone_name, finger_x_qq.toEulerAngles().to_log()) logger.debug("finger_y_qq: %s [%s]", finger_bone_name, finger_y_qq.toEulerAngles().to_log()) logger.debug("finger_z_qq: %s [%s]", finger_bone_name, finger_z_qq.toEulerAngles().to_log()) # 手首の回転は、手首から見た指先の方向 transferee_bf = motion.calc_bf(transferee_bone.name, fno) transferee_bf.rotation = finger_rotation # 捩りなしの状態で一旦登録する motion.regist_bf(transferee_bf, transferee_bone.name, fno) now_finger2_global_3ds, now_finger2_matrixs = MServiceUtils.calc_global_pos(model, finger2_links, motion, fno, return_matrix=True) # 小指先のローカル位置 finger2_initial_local_pos = now_finger2_matrixs[wrist_bone_name].inverted() * now_finger2_global_3ds[finger2_bone_name] finger2_local_pos = now_finger2_matrixs[wrist_bone_name].inverted() * finger2_global_3ds[finger2_bone_name] finger2_rotation = MQuaternion.rotationTo(finger2_initial_local_pos, finger2_local_pos) logger.debug("finger2_rotation: %s [%s]", finger2_bone_name, finger2_rotation.toEulerAngles().to_log()) finger2_x_qq = MQuaternion.fromAxisAndAngle(transferee_local_x_axis, finger_x_qq.toDegree() + finger2_rotation.toDegree()) transferee_bf.rotation = finger_y_qq * finger2_x_qq * finger_z_qq motion.regist_bf(transferee_bf, transferee_bone.name, fno) logger.debug("transferee_qq: %s [%s], x [%s]", transferee_bone.name, transferee_bf.rotation.toEulerAngles().to_log(), finger2_x_qq.toEulerAngles().to_log()) logger.count("【腕IK変換 - {0}】".format(bone_name), fno, fnos)
def convert_parent(self): motion = self.options.motion model = self.options.model root_bone_name = "全ての親" center_bone_name = "センター" center_parent_bone_name = "センター親" waist_bone_name = "腰" upper_bone_name = "上半身" lower_bone_name = "下半身" left_leg_ik_bone_name = "左足IK" right_leg_ik_bone_name = "右足IK" left_leg_ik_parent_bone_name = "左足IK親" right_leg_ik_parent_bone_name = "右足IK親" # まずキー登録 # for bone_name in [root_bone_name]: # if bone_name in model.bones: # prev_sep_fno = 0 # fnos = motion.get_bone_fnos(root_bone_name, center_bone_name, waist_bone_name, upper_bone_name, lower_bone_name, \ # left_leg_ik_bone_name, right_leg_ik_bone_name, center_parent_bone_name, \ # left_leg_ik_parent_bone_name, right_leg_ik_parent_bone_name) # for fno in fnos: # bf = motion.calc_bf(bone_name, fno) # motion.regist_bf(bf, bone_name, fno) # if fno // 2000 > prev_sep_fno and fnos[-1] > 0: # logger.info("-- %sフレーム目:終了(%s%)【準備 - %s】", fno, round((fno / fnos[-1]) * 100, 3), bone_name) # prev_sep_fno = fno // 2000 for bone_name in [center_bone_name]: if bone_name in model.bones: prev_sep_fno = 0 fno = 0 fnos = motion.get_bone_fnos(bone_name, root_bone_name) for fno in fnos: bf = motion.calc_bf(bone_name, fno) motion.regist_bf(bf, bone_name, fno) if fno // 2000 > prev_sep_fno and fnos[-1] > 0: logger.count(f"【準備 - {bone_name}】", fno, fnos) prev_sep_fno = fno // 2000 logger.count(f"【準備 - {bone_name}】", fno, fnos) if self.options.center_rotatation_flg: for bone_name in [ center_bone_name, upper_bone_name, lower_bone_name ]: if bone_name in model.bones: prev_sep_fno = 0 fno = 0 fnos = motion.get_bone_fnos(bone_name, center_bone_name, root_bone_name) for fno in fnos: bf = motion.calc_bf(bone_name, fno) motion.regist_bf(bf, bone_name, fno) if fno // 2000 > prev_sep_fno and fnos[-1] > 0: logger.count(f"【準備 - {bone_name}】", fno, fnos) prev_sep_fno = fno // 2000 logger.count(f"【準備 - {bone_name}】", fno, fnos) for bone_name in [right_leg_ik_bone_name, left_leg_ik_bone_name]: if bone_name in model.bones: prev_sep_fno = 0 fno = 0 fnos = motion.get_bone_fnos(bone_name, root_bone_name, left_leg_ik_parent_bone_name, right_leg_ik_parent_bone_name) for fno in fnos: bf = motion.calc_bf(bone_name, fno) motion.regist_bf(bf, bone_name, fno) if fno // 2000 > prev_sep_fno and fnos[-1] > 0: logger.count(f"【準備 - {bone_name}】", fno, fnos) prev_sep_fno = fno // 2000 logger.count(f"【準備 - {bone_name}】", fno, fnos) logger.info("移植開始", decoration=MLogger.DECORATION_LINE) # センターの移植 for bone_name in [center_bone_name]: if bone_name in model.bones: prev_sep_fno = 0 fno = 0 links = model.create_link_2_top_one(bone_name, is_defined=False) fnos = motion.get_bone_fnos(bone_name, root_bone_name) # 移植 for fno in fnos: root_bf = motion.calc_bf(root_bone_name, fno) center_parent_bf = motion.calc_bf(center_parent_bone_name, fno) bf = motion.calc_bf(bone_name, fno) # logger.info("f: %s, prev bf.position: %s", fno, bf.position) # relative_3ds_dic = MServiceUtils.calc_relative_position(model, links, motion, fno) # [logger.info("R %s", v.to_log()) for v in relative_3ds_dic] global_3ds_dic = MServiceUtils.calc_global_pos( model, links, motion, fno) bone_global_pos = global_3ds_dic[bone_name] # [logger.info("G %s: %s", k, v.to_log()) for k, v in global_3ds_dic.items()] # logger.info("f: %s, bone_global_pos: %s", fno, bone_global_pos) # グローバル位置からの差(センター親があった場合、それも加味して入れてしまう) bf.position = bone_global_pos - model.bones[ bone_name].position # logger.info("f: %s, bf.position: %s", fno, bf.position) # 回転の吸収 bf.rotation = root_bf.rotation * center_parent_bf.rotation * bf.rotation motion.regist_bf(bf, bone_name, fno) if fno // 2000 > prev_sep_fno and fnos[-1] > 0: logger.count(f"【移植 - {bone_name}】", fno, fnos) prev_sep_fno = fno // 2000 logger.count(f"【移植 - {bone_name}】", fno, fnos) # 足IKの移植 for bone_name, parent_bone_name in [ (right_leg_ik_bone_name, right_leg_ik_parent_bone_name), (left_leg_ik_bone_name, left_leg_ik_parent_bone_name) ]: if bone_name in model.bones: prev_sep_fno = 0 fno = 0 links = model.create_link_2_top_one(bone_name, is_defined=False) fnos = motion.get_bone_fnos(bone_name, root_bone_name) # 移植 for fno in fnos: root_bf = motion.calc_bf(root_bone_name, fno) leg_ik_parent_bf = motion.calc_bf(parent_bone_name, fno) bf = motion.calc_bf(bone_name, fno) global_3ds_dic = MServiceUtils.calc_global_pos( model, links, motion, fno) bone_global_pos = global_3ds_dic[bone_name] # グローバル位置からの差(足IK親があった場合、それも加味して入れてしまう) bf.position = bone_global_pos - model.bones[ bone_name].position # 回転 bf.rotation = root_bf.rotation * leg_ik_parent_bf.rotation * bf.rotation motion.regist_bf(bf, bone_name, fno) if fno // 2000 > prev_sep_fno and fnos[-1] > 0: logger.count(f"【移植 - {bone_name}】", fno, fnos) prev_sep_fno = fno // 2000 logger.count(f"【移植 - {bone_name}】", fno, fnos) # 全ての親削除 if root_bone_name in motion.bones: del motion.bones[root_bone_name] # センター親削除 if center_parent_bone_name in motion.bones: del motion.bones[center_parent_bone_name] # 足IK親削除 if left_leg_ik_parent_bone_name in motion.bones: del motion.bones[left_leg_ik_parent_bone_name] if right_leg_ik_parent_bone_name in motion.bones: del motion.bones[right_leg_ik_parent_bone_name] # logger.info("center_rotatation_flg: %s", self.options.center_rotatation_flg) if self.options.center_rotatation_flg: # センター→上半身・下半身の移植 prev_sep_fno = 0 fno = 0 fnos = motion.get_bone_fnos(upper_bone_name, lower_bone_name, center_bone_name) for fno in fnos: center_bf = motion.calc_bf(center_bone_name, fno) waist_bf = motion.calc_bf(waist_bone_name, fno) upper_bf = motion.calc_bf(upper_bone_name, fno) lower_bf = motion.calc_bf(lower_bone_name, fno) center_links = model.create_link_2_top_one(center_bone_name, is_defined=False) lower_links = model.create_link_2_top_one(lower_bone_name, is_defined=False) # 一旦移動量を保持 center_global_3ds_dic = MServiceUtils.calc_global_pos( model, lower_links, motion, fno, limit_links=center_links) # 回転移植 upper_bf.rotation = center_bf.rotation * waist_bf.rotation * upper_bf.rotation motion.regist_bf(upper_bf, upper_bone_name, fno) lower_bf.rotation = center_bf.rotation * waist_bf.rotation * lower_bf.rotation motion.regist_bf(lower_bf, lower_bone_name, fno) # 腰クリア if waist_bone_name in model.bones: waist_bf.rotation = MQuaternion() motion.regist_bf(waist_bf, waist_bone_name, fno) # センター回転クリア center_bf.rotation = MQuaternion() # 移動を下半身ベースで再計算 center_bf.position = center_global_3ds_dic[ lower_bone_name] - model.bones[lower_bone_name].position motion.regist_bf(center_bf, center_bone_name, fno) if fno // 1000 > prev_sep_fno and fnos[-1] > 0: logger.count("【移植 - 上半身・下半身】", fno, fnos) prev_sep_fno = fno // 1000 logger.count("【移植 - 上半身・下半身】", fno, fnos) logger.info("移植完了", decoration=MLogger.DECORATION_LINE) if self.options.remove_unnecessary_flg: futures = [] with ThreadPoolExecutor( thread_name_prefix="remove", max_workers=self.options.max_workers) as executor: for bone_name in [ center_bone_name, upper_bone_name, lower_bone_name, right_leg_ik_bone_name, left_leg_ik_bone_name ]: futures.append( executor.submit(self.remove_unnecessary_bf, bone_name)) concurrent.futures.wait( futures, timeout=None, return_when=concurrent.futures.FIRST_EXCEPTION) for f in futures: if not f.result(): return False return True
def test_calc_global_pos01(self): motion = VmdReader( "D:/MMD/MikuMikuDance_v926x64/UserFile/Motion/ダンス_1人/ドラマツルギー motion 配布用 moka/ドラマツルギー_0-500.vmd" ).read_data() model = PmxReader( "D:/MMD/MikuMikuDance_v926x64/UserFile/Model/VOCALOID/初音ミク/Tda式デフォ服ミク_ver1.1 金子卵黄/Tda式初音ミク_デフォ服ver.pmx" ).read_data() # -------------- links = model.create_link_2_top_one("グルーブ") # --------- pos_dic = MServiceUtils.calc_global_pos(model, links, motion, 420) self.assertEqual(4, len(pos_dic.keys())) # SIZING_ROOT_BONE print(pos_dic["SIZING_ROOT_BONE"]) self.assertAlmostEqual(pos_dic["SIZING_ROOT_BONE"].x(), 0, delta=0.1) self.assertAlmostEqual(pos_dic["SIZING_ROOT_BONE"].y(), 0, delta=0.1) self.assertAlmostEqual(pos_dic["SIZING_ROOT_BONE"].z(), 0, delta=0.1) # 全ての親 print(pos_dic["全ての親"]) self.assertAlmostEqual(pos_dic["全ての親"].x(), 0, delta=0.1) self.assertAlmostEqual(pos_dic["全ての親"].y(), 0, delta=0.1) self.assertAlmostEqual(pos_dic["全ての親"].z(), 0, delta=0.1) # センター print(pos_dic["センター"]) self.assertAlmostEqual(pos_dic["センター"].x(), 3.2, delta=0.1) self.assertAlmostEqual(pos_dic["センター"].y(), 8.4, delta=0.1) self.assertAlmostEqual(pos_dic["センター"].z(), 2.7, delta=0.1) # グルーブ print(pos_dic["グルーブ"]) self.assertAlmostEqual(pos_dic["グルーブ"].x(), 3.2, delta=0.1) self.assertAlmostEqual(pos_dic["グルーブ"].y(), 8.4, delta=0.1) self.assertAlmostEqual(pos_dic["グルーブ"].z(), 2.7, delta=0.1) # -------------- links = model.create_link_2_top_one("左足IK") # --------- pos_dic = MServiceUtils.calc_global_pos(model, links, motion, 420) self.assertEqual(4, len(pos_dic.keys())) # SIZING_ROOT_BONE print(pos_dic["SIZING_ROOT_BONE"]) self.assertAlmostEqual(pos_dic["SIZING_ROOT_BONE"].x(), 0, delta=0.1) self.assertAlmostEqual(pos_dic["SIZING_ROOT_BONE"].y(), 0, delta=0.1) self.assertAlmostEqual(pos_dic["SIZING_ROOT_BONE"].z(), 0, delta=0.1) # 全ての親 print(pos_dic["全ての親"]) self.assertAlmostEqual(pos_dic["全ての親"].x(), 0, delta=0.1) self.assertAlmostEqual(pos_dic["全ての親"].y(), 0, delta=0.1) self.assertAlmostEqual(pos_dic["全ての親"].z(), 0, delta=0.1) # 左足IK親 print(pos_dic["左足IK親"]) self.assertAlmostEqual(pos_dic["左足IK親"].x(), 1.0, delta=0.1) self.assertAlmostEqual(pos_dic["左足IK親"].y(), 0, delta=0.1) self.assertAlmostEqual(pos_dic["左足IK親"].z(), 0.7, delta=0.1) # 右足IK親 print(pos_dic["左足IK"]) self.assertAlmostEqual(pos_dic["左足IK"].x(), 3.5, delta=0.1) self.assertAlmostEqual(pos_dic["左足IK"].y(), 3.9, delta=0.1) self.assertAlmostEqual(pos_dic["左足IK"].z(), 2.1, delta=0.1) # -------------- links = model.create_link_2_top_one("右手首") # --------- pos_dic = MServiceUtils.calc_global_pos(model, links, motion, 420) # self.assertEqual(17, len(pos_dic.keys())) # SIZING_ROOT_BONE print(pos_dic["SIZING_ROOT_BONE"]) self.assertAlmostEqual(pos_dic["SIZING_ROOT_BONE"].x(), 0, delta=0.1) self.assertAlmostEqual(pos_dic["SIZING_ROOT_BONE"].y(), 0, delta=0.1) self.assertAlmostEqual(pos_dic["SIZING_ROOT_BONE"].z(), 0, delta=0.1) # 全ての親 print(pos_dic["全ての親"]) self.assertAlmostEqual(pos_dic["全ての親"].x(), 0, delta=0.1) self.assertAlmostEqual(pos_dic["全ての親"].y(), 0, delta=0.1) self.assertAlmostEqual(pos_dic["全ての親"].z(), 0, delta=0.1) # センター print(pos_dic["センター"]) self.assertAlmostEqual(pos_dic["センター"].x(), 3.2, delta=0.1) self.assertAlmostEqual(pos_dic["センター"].y(), 8.4, delta=0.1) self.assertAlmostEqual(pos_dic["センター"].z(), 2.7, delta=0.1) # グルーブ print(pos_dic["グルーブ"]) self.assertAlmostEqual(pos_dic["グルーブ"].x(), 3.2, delta=0.1) self.assertAlmostEqual(pos_dic["グルーブ"].y(), 8.4, delta=0.1) self.assertAlmostEqual(pos_dic["グルーブ"].z(), 2.7, delta=0.1) # 腰 self.assertAlmostEqual(pos_dic["腰"].x(), 3.2, delta=0.1) self.assertAlmostEqual(pos_dic["腰"].y(), 12.1, delta=0.1) self.assertAlmostEqual(pos_dic["腰"].z(), 3.0, delta=0.1) # 上半身 self.assertAlmostEqual(pos_dic["上半身"].x(), 3.2, delta=0.1) self.assertAlmostEqual(pos_dic["上半身"].y(), 13.0, delta=0.1) self.assertAlmostEqual(pos_dic["上半身"].z(), 2.2, delta=0.1) # 上半身2 self.assertAlmostEqual(pos_dic["上半身2"].x(), 3.25, delta=0.1) self.assertAlmostEqual(pos_dic["上半身2"].y(), 14.0, delta=0.1) self.assertAlmostEqual(pos_dic["上半身2"].z(), 2.25, delta=0.1) # 右肩P self.assertAlmostEqual(pos_dic["右肩P"].x(), 3.03, delta=0.1) self.assertAlmostEqual(pos_dic["右肩P"].y(), 16.26, delta=0.1) self.assertAlmostEqual(pos_dic["右肩P"].z(), 2.28, delta=0.1) # 右肩 self.assertAlmostEqual(pos_dic["右肩"].x(), 3.03, delta=0.1) self.assertAlmostEqual(pos_dic["右肩"].y(), 16.26, delta=0.1) self.assertAlmostEqual(pos_dic["右肩"].z(), 2.28, delta=0.1) # # 右肩C # self.assertAlmostEqual(pos_dic["右肩C"].x(), 3.03, delta=0.1) # self.assertAlmostEqual(pos_dic["右肩C"].y(), 16.26, delta=0.1) # self.assertAlmostEqual(pos_dic["右肩C"].z(), 2.28, delta=0.1) # 右腕 self.assertAlmostEqual(pos_dic["右腕"].x(), 2.15, delta=0.1) self.assertAlmostEqual(pos_dic["右腕"].y(), 16.32, delta=0.1) self.assertAlmostEqual(pos_dic["右腕"].z(), 2.39, delta=0.1) # 右腕捩 self.assertAlmostEqual(pos_dic["右腕捩"].x(), 0.66, delta=0.1) self.assertAlmostEqual(pos_dic["右腕捩"].y(), 15.64, delta=0.1) self.assertAlmostEqual(pos_dic["右腕捩"].z(), 2.19, delta=0.1) # 右ひじ self.assertAlmostEqual(pos_dic["右ひじ"].x(), -0.31, delta=0.1) self.assertAlmostEqual(pos_dic["右ひじ"].y(), 15.17, delta=0.1) self.assertAlmostEqual(pos_dic["右ひじ"].z(), 2.03, delta=0.1) # 右手捩 self.assertAlmostEqual(pos_dic["右手捩"].x(), 0.21, delta=0.1) self.assertAlmostEqual(pos_dic["右手捩"].y(), 14.36, delta=0.1) self.assertAlmostEqual(pos_dic["右手捩"].z(), 0.95, delta=0.1) # 右手首 self.assertAlmostEqual(pos_dic["右手首"].x(), 0.56, delta=0.1) self.assertAlmostEqual(pos_dic["右手首"].y(), 13.83, delta=0.1) self.assertAlmostEqual(pos_dic["右手首"].z(), 0.23, delta=0.1)
def convert_leg_fk2ik(self, direction: str): logger.info("足IK変換 【%s足IK】", direction, decoration=MLogger.DECORATION_LINE) motion = self.options.motion model = self.options.model leg_ik_bone_name = "{0}足IK".format(direction) toe_ik_bone_name = "{0}つま先IK".format(direction) leg_bone_name = "{0}足".format(direction) knee_bone_name = "{0}ひざ".format(direction) ankle_bone_name = "{0}足首".format(direction) # 足FK末端までのリンク fk_links = model.create_link_2_top_one(ankle_bone_name, is_defined=False) # 足IK末端までのリンク ik_links = model.create_link_2_top_one(leg_ik_bone_name, is_defined=False) # つま先IK末端までのリンク toe_ik_links = model.create_link_2_top_one(toe_ik_bone_name, is_defined=False) # つま先(足首の子ボーン)の名前 ankle_child_bone_name = model.bone_indexes[ model.bones[toe_ik_bone_name].ik.target_index] # つま先末端までのリンク toe_fk_links = model.create_link_2_top_one(ankle_child_bone_name, is_defined=False) fnos = motion.get_bone_fnos(leg_bone_name, knee_bone_name, ankle_bone_name) # まずキー登録 prev_sep_fno = 0 fno = 0 for fno in fnos: bf = motion.calc_bf(leg_ik_bone_name, fno) motion.regist_bf(bf, leg_ik_bone_name, fno) if fno // 1000 > prev_sep_fno and fnos[-1] > 0: logger.count(f"【準備 - {leg_ik_bone_name}】", fno, fnos) prev_sep_fno = fno // 1000 logger.info("準備完了 【%s足IK】", direction, decoration=MLogger.DECORATION_LINE) ik_parent_name = ik_links.get(leg_ik_bone_name, offset=-1).name # 足IKの移植 prev_sep_fno = 0 # 移植 fno = 0 for fno in fnos: leg_fk_3ds_dic = MServiceUtils.calc_global_pos( model, fk_links, motion, fno) _, leg_ik_matrixs = MServiceUtils.calc_global_pos( model, ik_links, motion, fno, return_matrix=True) # 足首の角度がある状態での、つま先までのグローバル位置 leg_toe_fk_3ds_dic = MServiceUtils.calc_global_pos( model, toe_fk_links, motion, fno) # IKの親から見た相対位置 leg_ik_parent_matrix = leg_ik_matrixs[ik_parent_name] bf = motion.calc_bf(leg_ik_bone_name, fno) # 足IKの位置は、足IKの親から見た足首のローカル位置(足首位置マイナス) bf.position = leg_ik_parent_matrix.inverted() * ( leg_fk_3ds_dic[ankle_bone_name] - (model.bones[ankle_bone_name].position - model.bones[ik_parent_name].position)) bf.rotation = MQuaternion() # 一旦足IKの位置が決まった時点で登録 motion.regist_bf(bf, leg_ik_bone_name, fno) # 足IK回転なし状態でのつま先までのグローバル位置 leg_ik_3ds_dic, leg_ik_matrisxs = MServiceUtils.calc_global_pos( model, toe_ik_links, motion, fno, return_matrix=True) [ logger.debug("f: %s, leg_ik_3ds_dic[%s]: %s", fno, k, v.to_log()) for k, v in leg_ik_3ds_dic.items() ] # つま先のローカル位置 ankle_child_initial_local_pos = leg_ik_matrisxs[ leg_ik_bone_name].inverted() * leg_ik_3ds_dic[toe_ik_bone_name] ankle_child_local_pos = leg_ik_matrisxs[leg_ik_bone_name].inverted( ) * leg_toe_fk_3ds_dic[ankle_child_bone_name] logger.debug("f: %s, ankle_child_initial_local_pos: %s", fno, ankle_child_initial_local_pos.to_log()) logger.debug("f: %s, ankle_child_local_pos: %s", fno, ankle_child_local_pos.to_log()) # 足IKの回転は、足首から見たつま先の方向 bf.rotation = MQuaternion.rotationTo(ankle_child_initial_local_pos, ankle_child_local_pos) logger.debug("f: %s, ik_rotation: %s", fno, bf.rotation.toEulerAngles4MMD().to_log()) motion.regist_bf(bf, leg_ik_bone_name, fno) if fno // 500 > prev_sep_fno and fnos[-1] > 0: logger.count(f"【足IK変換 - {leg_ik_bone_name}】", fno, fnos) prev_sep_fno = fno // 500 logger.info("変換完了 【%s足IK】", direction, decoration=MLogger.DECORATION_LINE) # IKon for showik in self.options.motion.showiks: for ikf in showik.ik: if ikf.name == leg_ik_bone_name or ikf.name == toe_ik_bone_name: ikf.onoff = 1 # 不要キー削除処理 if self.options.remove_unnecessary_flg: self.options.motion.remove_unnecessary_bf(0, leg_ik_bone_name, self.options.model.bones[leg_ik_bone_name].getRotatable(), \ self.options.model.bones[leg_ik_bone_name].getTranslatable()) return True
def a_test_split_bf_by_fno02(self): original_motion = VmdReader(u"test/data/補間曲線テスト01.vmd").read_data() model = PmxReader( "D:/MMD/MikuMikuDance_v926x64/UserFile/Model/ダミーボーン頂点追加2.pmx" ).read_data() target_bone_name = "ボーン01" links = BoneLinks() links.append(model.bones["SIZING_ROOT_BONE"]) links.append(model.bones["ボーン01"]) base_params = [0, 16, 32, 127] # https://qiita.com/wakame1367/items/0744268e928a28810c20 for xparams, yparams, zparams, rparams in zip(np.array(np.meshgrid(base_params, base_params, base_params, base_params)).T.reshape(-1, 4), \ np.array(np.meshgrid(base_params, base_params, base_params, base_params)).T.reshape(-1, 4), \ np.array(np.meshgrid(base_params, base_params, base_params, base_params)).T.reshape(-1, 4), \ np.array(np.meshgrid(base_params, base_params, base_params, base_params)).T.reshape(-1, 4)): try: for fill_fno in range( original_motion.get_bone_fnos(target_bone_name)[0] + 1, original_motion.get_bone_fnos(target_bone_name)[-1]): motion = cPickle.loads(cPickle.dumps(original_motion, -1)) # bfの補間曲線を再設定する next_bf = motion.bones[target_bone_name][ motion.get_bone_fnos(target_bone_name)[-1]] motion.reset_interpolation_parts(target_bone_name, next_bf, [None, MVector2D(xparams[0], xparams[1]), MVector2D(xparams[2], xparams[3]), None], \ MBezierUtils.MX_x1_idxs, MBezierUtils.MX_y1_idxs, MBezierUtils.MX_x2_idxs, MBezierUtils.MX_y2_idxs) motion.reset_interpolation_parts(target_bone_name, next_bf, [None, MVector2D(yparams[0], yparams[1]), MVector2D(yparams[2], yparams[3]), None], \ MBezierUtils.MY_x1_idxs, MBezierUtils.MY_y1_idxs, MBezierUtils.MY_x2_idxs, MBezierUtils.MY_y2_idxs) motion.reset_interpolation_parts(target_bone_name, next_bf, [None, MVector2D(zparams[0], zparams[1]), MVector2D(zparams[2], zparams[3]), None], \ MBezierUtils.MZ_x1_idxs, MBezierUtils.MZ_y1_idxs, MBezierUtils.MZ_x2_idxs, MBezierUtils.MZ_y2_idxs) motion.reset_interpolation_parts(target_bone_name, next_bf, [None, MVector2D(rparams[0], rparams[1]), MVector2D(rparams[2], rparams[3]), None], \ MBezierUtils.R_x1_idxs, MBezierUtils.R_y1_idxs, MBezierUtils.R_x2_idxs, MBezierUtils.R_y2_idxs) # 補間曲線を再設定したモーションを再保持 org_motion = cPickle.loads(cPickle.dumps(motion, -1)) # 間のキーフレをテスト prev_bf = motion.bones[target_bone_name][ motion.get_bone_fnos(target_bone_name)[0]] next_bf = motion.bones[target_bone_name][ motion.get_bone_fnos(target_bone_name)[-1]] result = motion.split_bf_by_fno(target_bone_name, prev_bf, next_bf, fill_fno) # 分割に成功した場合、誤差小。失敗してる場合は誤差大 delta = 0.3 if result else 1 # print("-----------------------------") # for now_fno in motion.get_bone_fnos(target_bone_name): # # 有効なキーフレをテスト # now_bf = motion.calc_bf(target_bone_name, now_fno) # org_pos_dic = MServiceUtils.calc_global_pos(model, links, org_motion, now_fno) # now_pos_dic = MServiceUtils.calc_global_pos(model, links, motion, now_fno) # print("fill_fno: %s, now_fno: %s key: %s ------------" % (fill_fno, now_bf.fno, now_bf.key)) # print("xparams: %s" % xparams) # print("yparams: %s" % yparams) # print("zparams: %s" % zparams) # print("rparams: %s" % rparams) # print("position: %s" % now_bf.position) # print("rotation: %s" % now_bf.rotation.toEulerAngles4MMD()) # print("int move x: %s, %s, %s, %s" % (now_bf.interpolation[MBezierUtils.MX_x1_idxs[3]], now_bf.interpolation[MBezierUtils.MX_y1_idxs[3]], \ # now_bf.interpolation[MBezierUtils.MX_x2_idxs[3]], now_bf.interpolation[MBezierUtils.MX_y2_idxs[3]])) # print("int move y: %s, %s, %s, %s" % (now_bf.interpolation[MBezierUtils.MY_x1_idxs[3]], now_bf.interpolation[MBezierUtils.MY_y1_idxs[3]], \ # now_bf.interpolation[MBezierUtils.MY_x2_idxs[3]], now_bf.interpolation[MBezierUtils.MY_y2_idxs[3]])) # print("int move z: %s, %s, %s, %s" % (now_bf.interpolation[MBezierUtils.MZ_x1_idxs[3]], now_bf.interpolation[MBezierUtils.MZ_y1_idxs[3]], \ # now_bf.interpolation[MBezierUtils.MZ_x2_idxs[3]], now_bf.interpolation[MBezierUtils.MZ_y2_idxs[3]])) # print("int rot: %s, %s, %s, %s" % (now_bf.interpolation[MBezierUtils.R_x1_idxs[3]], now_bf.interpolation[MBezierUtils.R_y1_idxs[3]], \ # now_bf.interpolation[MBezierUtils.R_x2_idxs[3]], now_bf.interpolation[MBezierUtils.R_y2_idxs[3]])) # self.assertAlmostEqual(org_pos_dic[target_bone_name].x(), now_pos_dic[target_bone_name].x(), delta=(delta * 2)) # self.assertAlmostEqual(org_pos_dic[target_bone_name].y(), now_pos_dic[target_bone_name].y(), delta=(delta * 3)) # self.assertAlmostEqual(org_pos_dic[target_bone_name].z(), now_pos_dic[target_bone_name].z(), delta=(delta * 4)) print("-----------------------------") for fno in range( motion.get_bone_fnos(target_bone_name)[-1]): # org_bf = org_motion.calc_bf(target_bone_name, fno) now_bf = motion.calc_bf(target_bone_name, fno) org_pos_dic = MServiceUtils.calc_global_pos( model, links, org_motion, fno) now_pos_dic = MServiceUtils.calc_global_pos( model, links, motion, fno) print("** fill_fno: %s, fno: %s key: %s ------------" % (fill_fno, now_bf.fno, now_bf.key)) print("xparams: %s" % xparams) print("yparams: %s" % yparams) print("zparams: %s" % zparams) print("rparams: %s" % rparams) print("** position: %s" % now_bf.position) print("** rotation: %s" % now_bf.rotation.toEulerAngles4MMD()) print("** int move x: %s, %s, %s, %s" % (now_bf.interpolation[MBezierUtils.MX_x1_idxs[3]], now_bf.interpolation[MBezierUtils.MX_y1_idxs[3]], \ now_bf.interpolation[MBezierUtils.MX_x2_idxs[3]], now_bf.interpolation[MBezierUtils.MX_y2_idxs[3]])) print("** int move y: %s, %s, %s, %s" % (now_bf.interpolation[MBezierUtils.MY_x1_idxs[3]], now_bf.interpolation[MBezierUtils.MY_y1_idxs[3]], \ now_bf.interpolation[MBezierUtils.MY_x2_idxs[3]], now_bf.interpolation[MBezierUtils.MY_y2_idxs[3]])) print("** int move z: %s, %s, %s, %s" % (now_bf.interpolation[MBezierUtils.MZ_x1_idxs[3]], now_bf.interpolation[MBezierUtils.MZ_y1_idxs[3]], \ now_bf.interpolation[MBezierUtils.MZ_x2_idxs[3]], now_bf.interpolation[MBezierUtils.MZ_y2_idxs[3]])) print("** int rot: %s, %s, %s, %s" % (now_bf.interpolation[MBezierUtils.R_x1_idxs[3]], now_bf.interpolation[MBezierUtils.R_y1_idxs[3]], \ now_bf.interpolation[MBezierUtils.R_x2_idxs[3]], now_bf.interpolation[MBezierUtils.R_y2_idxs[3]])) self.assertAlmostEqual( org_pos_dic[target_bone_name].x(), now_pos_dic[target_bone_name].x(), delta=(delta * 2)) self.assertAlmostEqual( org_pos_dic[target_bone_name].y(), now_pos_dic[target_bone_name].y(), delta=(delta * 3)) self.assertAlmostEqual( org_pos_dic[target_bone_name].z(), now_pos_dic[target_bone_name].z(), delta=(delta * 4)) # now = datetime.now() # data_set = MOptionsDataSet(motion, model, model, "E:/WebDownload/test_split_bf_by_fno01_{0:%Y%m%d_%H%M%S%f}.vmd".format(now), False, False) # VmdWriter(data_set).write() # print(data_set.output_vmd_path) # data_set = MOptionsDataSet(org_motion, model, model, "E:/WebDownload/test_split_bf_by_fno01_{0:%Y%m%d_%H%M%S%f}_orignal.vmd".format(now), False, False) # VmdWriter(data_set).write() # print(data_set.output_vmd_path) except Exception as e: # エラーになったらデータを出力する now = datetime.now() data_set = MOptionsDataSet( motion, model, model, "E:/WebDownload/test_split_bf_by_fno01_{0:%Y%m%d_%H%M%S%f}.vmd" .format(now), False, False) VmdWriter(data_set).write() print(data_set.output_vmd_path) data_set = MOptionsDataSet( org_motion, model, model, "E:/WebDownload/test_split_bf_by_fno01_{0:%Y%m%d_%H%M%S%f}_orignal.vmd" .format(now), False, False) VmdWriter(data_set).write() print(data_set.output_vmd_path) raise e
def test_split_bf_by_fno01(self): original_motion = VmdReader(u"test/data/補間曲線テスト01.vmd").read_data() model = PmxReader( "D:/MMD/MikuMikuDance_v926x64/UserFile/Model/ダミーボーン頂点追加2.pmx" ).read_data() target_bone_name = "ボーン01" links = BoneLinks() links.append(model.bones["SIZING_ROOT_BONE"]) links.append(model.bones["ボーン01"]) for pidx in range(10): try: params = np.random.randint(0, 127, (1, 4)) # params = [[116, 24, 22, 82]] for fill_fno in range( original_motion.get_bone_fnos(target_bone_name)[0] + 1, original_motion.get_bone_fnos(target_bone_name)[-1]): motion = original_motion.copy() # bfの補間曲線を再設定する next_bf = motion.bones[target_bone_name][ motion.get_bone_fnos(target_bone_name)[-1]] motion.reset_interpolation_parts(target_bone_name, next_bf, [None, MVector2D(20, 20), MVector2D(107, 107), None], \ MBezierUtils.R_x1_idxs, MBezierUtils.R_y1_idxs, MBezierUtils.R_x2_idxs, MBezierUtils.R_y2_idxs) motion.reset_interpolation_parts(target_bone_name, next_bf, [None, MVector2D(params[0][0], params[0][1]), MVector2D(params[0][2], params[0][3]), None], \ MBezierUtils.MX_x1_idxs, MBezierUtils.MX_y1_idxs, MBezierUtils.MX_x2_idxs, MBezierUtils.MX_y2_idxs) motion.reset_interpolation_parts(target_bone_name, next_bf, [None, MVector2D(20, 20), MVector2D(107, 107), None], \ MBezierUtils.MY_x1_idxs, MBezierUtils.MY_y1_idxs, MBezierUtils.MY_x2_idxs, MBezierUtils.MY_y2_idxs) motion.reset_interpolation_parts(target_bone_name, next_bf, [None, MVector2D(20, 20), MVector2D(107, 107), None], \ MBezierUtils.MZ_x1_idxs, MBezierUtils.MZ_y1_idxs, MBezierUtils.MZ_x2_idxs, MBezierUtils.MZ_y2_idxs) # 補間曲線を再設定したモーションを再保持 org_motion = motion.copy() # 間のキーフレをテスト prev_bf = motion.bones[target_bone_name][ motion.get_bone_fnos(target_bone_name)[0]] next_bf = motion.bones[target_bone_name][ motion.get_bone_fnos(target_bone_name)[-1]] result = motion.split_bf_by_fno(target_bone_name, prev_bf, next_bf, fill_fno) # 分割に成功した場合、誤差小。失敗してる場合は誤差大 delta = 0.3 if result else 1 print("-----------------------------") for now_fno in motion.get_bone_fnos(target_bone_name): # 有効なキーフレをテスト now_bf = motion.calc_bf(target_bone_name, now_fno) org_pos_dic = MServiceUtils.calc_global_pos( model, links, org_motion, now_fno) now_pos_dic = MServiceUtils.calc_global_pos( model, links, motion, now_fno) print( "fill_fno: %s, now_fno: %s key: %s (%s) ------------" % (fill_fno, now_bf.fno, now_bf.key, pidx)) print("params: %s" % params) print("position: %s" % now_bf.position) print("rotation: %s" % now_bf.rotation.toEulerAngles4MMD()) print("int move x: %s, %s, %s, %s" % (now_bf.interpolation[MBezierUtils.MX_x1_idxs[3]], now_bf.interpolation[MBezierUtils.MX_y1_idxs[3]], \ now_bf.interpolation[MBezierUtils.MX_x2_idxs[3]], now_bf.interpolation[MBezierUtils.MX_y2_idxs[3]])) print("int move y: %s, %s, %s, %s" % (now_bf.interpolation[MBezierUtils.MY_x1_idxs[3]], now_bf.interpolation[MBezierUtils.MY_y1_idxs[3]], \ now_bf.interpolation[MBezierUtils.MY_x2_idxs[3]], now_bf.interpolation[MBezierUtils.MY_y2_idxs[3]])) print("int move z: %s, %s, %s, %s" % (now_bf.interpolation[MBezierUtils.MZ_x1_idxs[3]], now_bf.interpolation[MBezierUtils.MZ_y1_idxs[3]], \ now_bf.interpolation[MBezierUtils.MZ_x2_idxs[3]], now_bf.interpolation[MBezierUtils.MZ_y2_idxs[3]])) print("int rot: %s, %s, %s, %s" % (now_bf.interpolation[MBezierUtils.R_x1_idxs[3]], now_bf.interpolation[MBezierUtils.R_y1_idxs[3]], \ now_bf.interpolation[MBezierUtils.R_x2_idxs[3]], now_bf.interpolation[MBezierUtils.R_y2_idxs[3]])) self.assertAlmostEqual( org_pos_dic[target_bone_name].x(), now_pos_dic[target_bone_name].x(), delta=0.2) self.assertAlmostEqual( org_pos_dic[target_bone_name].y(), now_pos_dic[target_bone_name].y(), delta=0.2) self.assertAlmostEqual( org_pos_dic[target_bone_name].z(), now_pos_dic[target_bone_name].z(), delta=0.2) print("-----------------------------") for fno in range( motion.get_bone_fnos(target_bone_name)[-1]): # org_bf = org_motion.calc_bf(target_bone_name, fno) now_bf = motion.calc_bf(target_bone_name, fno) org_pos_dic = MServiceUtils.calc_global_pos( model, links, org_motion, fno) now_pos_dic = MServiceUtils.calc_global_pos( model, links, motion, fno) print( "** fill_fno: %s, fno: %s key: %s (%s) ------------" % (fill_fno, now_bf.fno, now_bf.key, pidx)) print("** params: %s" % params) print("** position: %s" % now_bf.position) print("** rotation: %s" % now_bf.rotation.toEulerAngles4MMD()) print("** int move x: %s, %s, %s, %s" % (now_bf.interpolation[MBezierUtils.MX_x1_idxs[3]], now_bf.interpolation[MBezierUtils.MX_y1_idxs[3]], \ now_bf.interpolation[MBezierUtils.MX_x2_idxs[3]], now_bf.interpolation[MBezierUtils.MX_y2_idxs[3]])) print("** int move y: %s, %s, %s, %s" % (now_bf.interpolation[MBezierUtils.MY_x1_idxs[3]], now_bf.interpolation[MBezierUtils.MY_y1_idxs[3]], \ now_bf.interpolation[MBezierUtils.MY_x2_idxs[3]], now_bf.interpolation[MBezierUtils.MY_y2_idxs[3]])) print("** int move z: %s, %s, %s, %s" % (now_bf.interpolation[MBezierUtils.MZ_x1_idxs[3]], now_bf.interpolation[MBezierUtils.MZ_y1_idxs[3]], \ now_bf.interpolation[MBezierUtils.MZ_x2_idxs[3]], now_bf.interpolation[MBezierUtils.MZ_y2_idxs[3]])) print("** int rot: %s, %s, %s, %s" % (now_bf.interpolation[MBezierUtils.R_x1_idxs[3]], now_bf.interpolation[MBezierUtils.R_y1_idxs[3]], \ now_bf.interpolation[MBezierUtils.R_x2_idxs[3]], now_bf.interpolation[MBezierUtils.R_y2_idxs[3]])) self.assertAlmostEqual( org_pos_dic[target_bone_name].x(), now_pos_dic[target_bone_name].x(), delta=(delta * 2)) self.assertAlmostEqual( org_pos_dic[target_bone_name].y(), now_pos_dic[target_bone_name].y(), delta=(delta * 3)) self.assertAlmostEqual( org_pos_dic[target_bone_name].z(), now_pos_dic[target_bone_name].z(), delta=(delta * 4)) now = datetime.now() data_set = MOptionsDataSet( motion, model, model, "E:/WebDownload/test_split_bf_by_fno01_{0:%Y%m%d_%H%M%S%f}.vmd" .format(now), False, False) VmdWriter(data_set).write() print(data_set.output_vmd_path) data_set = MOptionsDataSet( org_motion, model, model, "E:/WebDownload/test_split_bf_by_fno01_{0:%Y%m%d_%H%M%S%f}_orignal.vmd" .format(now), False, False) VmdWriter(data_set).write() print(data_set.output_vmd_path) except Exception as e: # エラーになったらデータを出力する now = datetime.now() data_set = MOptionsDataSet( motion, model, model, "E:/WebDownload/test_split_bf_by_fno01_{0:%Y%m%d_%H%M%S%f}.vmd" .format(now), False, False) VmdWriter(data_set).write() print(data_set.output_vmd_path) data_set = MOptionsDataSet( org_motion, model, model, "E:/WebDownload/test_split_bf_by_fno01_{0:%Y%m%d_%H%M%S%f}_orignal.vmd" .format(now), False, False) VmdWriter(data_set).write() print(data_set.output_vmd_path) raise e
def prepare_avoidance_dataset(self, data_set_idx: int, direction: str): logger.info("接触回避準備【No.%s - %s】", (data_set_idx + 1), direction) logger.copy(self.options) # 処理対象データセット data_set = self.options.data_set_list[data_set_idx] # 回避用オプション avoidance_options = self.avoidance_options[(data_set_idx, direction)] all_avoidance_list = [{}] prev_collisions = [] prev_block_fno = 0 fno = 0 fnos = data_set.motion.get_bone_fnos("{0}腕".format(direction), "{0}腕捩".format(direction), "{0}ひじ".format(direction), "{0}手捩".format(direction), "{0}手首".format(direction)) for fno in fnos: # 衝突しておらず、かつ前回の衝突情報がある場合、追加 if prev_collisions.count(True) == 0 and len( all_avoidance_list[-1].keys()) > 0: # 前回衝突なしで今回衝突していたら、リスト追加 all_avoidance_list.append({}) prev_collisions = [] for ((avoidance_name, avodance_link), avoidance) in zip(avoidance_options.avoidance_links.items(), avoidance_options.avoidances.values()): # 剛体の現在位置をチェック rep_avbone_global_3ds, rep_avbone_global_mats = \ MServiceUtils.calc_global_pos(data_set.rep_model, avodance_link, data_set.motion, fno, return_matrix=True) obb = avoidance.get_obb( fno, avodance_link.get(avodance_link.last_name()).position, rep_avbone_global_mats, self.options.arm_options.alignment, direction == "左") for arm_link in avoidance_options.arm_links: # 先モデルのそれぞれのグローバル位置 rep_global_3ds, rep_matrixs = MServiceUtils.calc_global_pos( data_set.rep_model, arm_link, data_set.motion, fno, return_matrix=True) # [logger.test("f: %s, k: %s, v: %s", fno, k, v) for k, v in rep_global_3ds.items()] # 衝突情報を取る collision, near_collision, x_distance, z_distance, rep_x_collision_vec, rep_z_collision_vec \ = obb.get_collistion(rep_global_3ds[arm_link.last_name()], rep_global_3ds["{0}腕".format(direction)], \ data_set.rep_model.bones["{0}腕".format(direction)].position.distanceToPoint(data_set.rep_model.bones[arm_link.last_name()].position)) if collision or near_collision: logger.debug("f: %s(%s-%s:%s), c[%s], nc[%s], xd[%s], zd[%s], xv[%s], zv[%s]", \ fno, (data_set_idx + 1), arm_link.last_name(), avoidance_name, collision, near_collision, \ x_distance, z_distance, rep_x_collision_vec.to_log(), rep_z_collision_vec.to_log()) # 衝突していたら、その中にキーフレ/ボーン名で登録 all_avoidance_list[-1][(fno, avoidance_name, arm_link.last_name())] = \ {"fno": fno, "avoidance_name": avoidance_name, "bone_name": arm_link.last_name(), "collision": collision, "near_collision": near_collision, \ "x_distance": x_distance, "z_distance": z_distance, "rep_x_collision_vec": rep_x_collision_vec, "rep_z_collision_vec": rep_z_collision_vec} prev_collisions.append(collision) prev_collisions.append(near_collision) if fno // 500 > prev_block_fno and fnos[-1] > 0: logger.info("-- %sフレーム目:終了(%s%)【No.%s - 接触回避準備① - %s】", fno, round((fno / fnos[-1]) * 100, 3), data_set_idx + 1, direction) prev_block_fno = fno // 500 prev_block_fno = 0 all_avoidance_axis = {} for aidx, avoidance_list in enumerate(all_avoidance_list): # 衝突ブロック単位で判定 block_x_distance = 0 block_z_distance = 0 for (fno, avoidance_name, bone_name), avf in avoidance_list.items(): collision = avf["collision"] near_collision = avf["near_collision"] x_distance = avf["x_distance"] z_distance = avf["z_distance"] rep_x_collision_vec = avf["rep_x_collision_vec"] rep_z_collision_vec = avf["rep_z_collision_vec"] # 距離を加算 block_x_distance += x_distance block_z_distance += z_distance if len(avoidance_list.keys()) > 0: # 範囲 from_fno = min([ fno for (fno, avoidance_name, bone_name) in avoidance_list.keys() ]) to_fno = max([ fno for (fno, avoidance_name, bone_name) in avoidance_list.keys() ]) # トータルで近い方の軸に移動させる all_avoidance_axis[from_fno] = { "from_fno": from_fno, "to_fno": to_fno, "axis": ("x" if block_x_distance * 1.5 < block_z_distance else "z") } logger.debug( "aidx: %s, d: %s, from: %s, to: %s, axis: %s, xd: %s, zd: %s", aidx, direction, from_fno, to_fno, all_avoidance_axis[from_fno], block_x_distance, block_z_distance) if fno // 1000 > prev_block_fno and fnos[-1] > 0: logger.info("-- %sフレーム目:終了(%s%)【No.%s - 接触回避準備② - %s】", fno, round((fno / fnos[-1]) * 100, 3), data_set_idx + 1, direction) prev_block_fno = fno // 1000 return all_avoidance_axis
def execute_avoidance(self, data_set_idx: int, direction: str, all_avoidance_axis: dict): logger.info("接触回避処理【No.%s - %s】", (data_set_idx + 1), direction) logger.copy(self.options) # 処理対象データセット data_set = self.options.data_set_list[data_set_idx] # 回避用オプション avoidance_options = self.avoidance_options[(data_set_idx, direction)] arm_bone_name = "{0}腕".format(direction) elbow_bone_name = "{0}ひじ".format(direction) wrist_bone_name = "{0}手首".format(direction) target_bone_names = [ arm_bone_name, "{0}腕捩".format(direction), elbow_bone_name, "{0}手捩".format(direction), wrist_bone_name ] avoidance_axis = {} prev_block_fno = 0 fnos = data_set.motion.get_bone_fnos(*target_bone_names) # 一度全部キーを追加する(キー自体は無効化のまま) for fno in fnos: for bone_name in target_bone_names: if bone_name not in data_set.motion.bones: data_set.motion.bones[bone_name] = {} data_set.motion.bones[bone_name][ fno] = data_set.motion.calc_bf(bone_name, fno) while len(fnos) > 0: fno = fnos[0] if fno in all_avoidance_axis: # 回避ブロックが始まったら、保持 avoidance_axis = all_avoidance_axis[fno] for ((avoidance_name, avodance_link), avoidance) in zip(avoidance_options.avoidance_links.items(), avoidance_options.avoidances.values()): # 剛体の現在位置をチェック rep_avbone_global_3ds, rep_avbone_global_mats = \ MServiceUtils.calc_global_pos(data_set.rep_model, avodance_link, data_set.motion, fno, return_matrix=True) obb = avoidance.get_obb( fno, avodance_link.get(avodance_link.last_name()).position, rep_avbone_global_mats, self.options.arm_options.alignment, direction == "左") # # 剛体の原点 --------------- # debug_bone_name = "原点" # debug_bf = VmdBoneFrame(fno) # debug_bf.key = True # debug_bf.set_name(debug_bone_name) # debug_bf.position = obb.origin # if debug_bone_name not in data_set.motion.bones: # data_set.motion.bones[debug_bone_name] = {} # data_set.motion.bones[debug_bone_name][fno] = debug_bf # # -------------- for arm_link in avoidance_options.arm_links: # 先モデルのそれぞれのグローバル位置 rep_global_3ds = MServiceUtils.calc_global_pos( data_set.rep_model, arm_link, data_set.motion, fno) # [logger.test("f: %s, k: %s, v: %s", fno, k, v) for k, v in rep_global_3ds.items()] # 衝突情報を取る collision, near_collision, x_distance, z_distance, rep_x_collision_vec, rep_z_collision_vec \ = obb.get_collistion(rep_global_3ds[arm_link.last_name()], rep_global_3ds[arm_bone_name], \ data_set.rep_model.bones[arm_bone_name].position.distanceToPoint(data_set.rep_model.bones[arm_link.last_name()].position)) if collision or near_collision: logger.debug("f: %s(%s-%s:%s), c[%s], nc[%s], xd[%s], zd[%s], xv[%s], zv[%s]", \ fno, (data_set_idx + 1), arm_link.last_name(), avoidance_name, collision, near_collision, \ x_distance, z_distance, rep_x_collision_vec.to_log(), rep_z_collision_vec.to_log()) is_success = [] failured_last_names = [] # 衝突単位のbf情報 org_bfs = {} for ik_links_list in avoidance_options.ik_links_list.values( ): for ik_links in ik_links_list: for link_name in ik_links.all().keys(): if link_name not in org_bfs: bf = data_set.motion.calc_bf( link_name, fno) bf.org_rotation = bf.rotation.copy() data_set.motion.regist_bf( bf, link_name, fno) org_bfs[link_name] = bf.copy() # 既定の回避方向(なければとりあえずZ) axis = "z" if "axis" not in avoidance_axis else avoidance_axis[ "axis"] # 回避方向 rep_collision_vec = rep_x_collision_vec if axis == "x" else rep_z_collision_vec if collision: logger.info("○接触あり: f: %s(%s-%s:%s:%s), 元: %s, 回避: %s", fno, \ (data_set_idx + 1), arm_link.last_display_name(), avoidance_name, axis, rep_global_3ds[arm_link.last_name()].to_log(), rep_collision_vec.to_log()) else: logger.info("-近接あり: f: %s(%s-%s:%s:%s), 元: %s, 回避: %s", fno, \ (data_set_idx + 1), arm_link.last_display_name(), avoidance_name, axis, rep_global_3ds[arm_link.last_name()].to_log(), rep_collision_vec.to_log()) # # 回避後の先端ボーン位置 ------------- # debug_bone_name = "{0}A".format(arm_link.last_name()) # debug_bf = VmdBoneFrame(fno) # debug_bf.key = True # debug_bf.set_name(debug_bone_name) # debug_bf.position = rep_collision_vec # if debug_bone_name not in data_set.motion.bones: # data_set.motion.bones[debug_bone_name] = {} # data_set.motion.bones[debug_bone_name][fno] = debug_bf # # ---------- # IK処理実行 for ik_cnt, (ik_links, ik_max_count) in enumerate(zip(avoidance_options.ik_links_list[arm_link.last_name()], \ avoidance_options.ik_count_list[arm_link.last_name()])): prev_rep_diff = MVector3D() # ひじを含んでいるか is_in_elbow = elbow_bone_name in list( ik_links.all().keys()) for now_ik_max_count in range(1, ik_max_count + 1): logger.debug("IK計算開始(%s): f: %s(%s:%s:%s), axis: %s, now[%s], new[%s]", now_ik_max_count, fno, (data_set_idx + 1), \ list(ik_links.all().keys()), avoidance_name, axis, rep_global_3ds[arm_link.last_name()].to_log(), rep_collision_vec.to_log()) # 修正角度がない場合、IK計算実行 MServiceUtils.calc_IK(data_set.rep_model, arm_link, data_set.motion, fno, rep_collision_vec, ik_links, max_count=1) # 現在のエフェクタ位置 now_rep_global_3ds = MServiceUtils.calc_global_pos( data_set.rep_model, arm_link, data_set.motion, fno) now_rep_effector_pos = now_rep_global_3ds[ arm_link.last_name()] # 現在のエフェクタ位置との差分(エフェクタ位置が指定されている場合のみ) rep_diff = MVector3D( ) if rep_collision_vec == MVector3D( ) else rep_collision_vec - now_rep_effector_pos # IKの関連ボーンの内積チェック dot_dict = {} dot_limit_dict = {} for link_name, link_bone in ik_links.all( ).items(): dot_dict[ link_name] = MQuaternion.dotProduct( org_bfs[link_name].rotation, data_set.motion.calc_bf( link_name, fno).rotation) dot_limit_dict[ link_name] = link_bone.dot_limit # まずは一旦確定 for link_name in list( ik_links.all().keys())[1:]: now_bf = data_set.motion.calc_bf( link_name, fno) data_set.motion.regist_bf( now_bf, link_name, fno) if np.count_nonzero( np.where( np.array(list(dot_dict.values())) < np.array( list(dot_limit_dict.values())), 1, 0)) == 0: if (prev_rep_diff == MVector3D() or np.sum(np.abs(rep_diff.data())) < np.sum(np.abs(prev_rep_diff.data()))) and \ np.count_nonzero(np.where(np.abs(rep_diff.data()) > (0.2 if data_set.original_xz_ratio > 0.5 else 0.1), 1, 0)) == 0: logger.debug("☆接触回避実行成功(%s): f: %s(%s:%s:%s), axis: %s, 指定 [%s], 現在[%s], 差異[%s], dot[%s]", now_ik_max_count, fno, (data_set_idx + 1), \ list(ik_links.all().keys()), avoidance_name, axis, rep_collision_vec.to_log(), \ now_rep_effector_pos.to_log(), rep_diff.to_log(), list(dot_dict.values())) # # 回避後の先端ボーン位置 ------------- # debug_bone_name = "{0}B".format(arm_link.last_name()) # debug_bf = VmdBoneFrame(fno) # debug_bf.key = True # debug_bf.set_name(debug_bone_name) # debug_bf.position = now_rep_effector_pos # if debug_bone_name not in data_set.motion.bones: # data_set.motion.bones[debug_bone_name] = {} # data_set.motion.bones[debug_bone_name][fno] = debug_bf # # ---------- # org_bfを保持し直し for link_name in ik_links.all().keys(): org_bfs[ link_name] = data_set.motion.calc_bf( link_name, fno).copy() # 回避方向保持 for link_name in list( ik_links.all().keys())[1:]: data_set.motion.bones[link_name][ fno].avoidance = axis # 大体同じ位置にあって、角度もそう大きくズレてない場合、OK(全部上書き) is_success = [True] # 衝突を計り直す collision, near_collision, x_distance, z_distance, rep_x_collision_vec, rep_z_collision_vec \ = obb.get_collistion(now_rep_global_3ds[arm_link.last_name()], now_rep_global_3ds[arm_link.first_name()], \ data_set.rep_model.bones[arm_bone_name].position.distanceToPoint(data_set.rep_model.bones[arm_link.last_name()].position)) if ( not collision and not near_collision ) or prev_rep_diff == rep_diff or np.count_nonzero( np.where( np.abs(rep_diff.data()) > 0.05, 1, 0)) == 0: # 衝突していなければこのIKターンは終了 # 前回とまったく同じ場合か、充分に近い場合、IK的に動きがないので終了 break prev_rep_diff = rep_diff elif (prev_rep_diff == MVector3D() or (prev_rep_diff != MVector3D() and np.sum(np.abs(rep_diff.data())) < np.sum(np.abs(prev_rep_diff.data())))) and \ (np.count_nonzero(np.where(np.abs(rep_diff.data()) > (1 if data_set.original_xz_ratio > 0.5 else 0.5), 1, 0)) == 0): logger.debug("☆接触回避実行ちょっと失敗採用(%s): f: %s(%s:%s:%s), axis: %s, 指定 [%s], 現在[%s], 差異[%s], dot[%s]", now_ik_max_count, fno, (data_set_idx + 1), \ list(ik_links.all().keys()), avoidance_name, axis, rep_collision_vec.to_log(), \ now_rep_effector_pos.to_log(), rep_diff.to_log(), list(dot_dict.values())) # # 回避後の先端ボーン位置 ------------- # debug_bone_name = "{0}B".format(arm_link.last_name()) # debug_bf = VmdBoneFrame(fno) # debug_bf.key = True # debug_bf.set_name(debug_bone_name) # debug_bf.position = now_rep_effector_pos # if debug_bone_name not in data_set.motion.bones: # data_set.motion.bones[debug_bone_name] = {} # data_set.motion.bones[debug_bone_name][fno] = debug_bf # # ---------- # org_bfを保持し直し for link_name in ik_links.all().keys(): org_bfs[ link_name] = data_set.motion.calc_bf( link_name, fno).copy() # 回避方向保持 for link_name in list( ik_links.all().keys())[1:]: data_set.motion.bones[link_name][ fno].avoidance = axis # 採用されたらOK is_success.append(True) # 衝突を計り直す collision, near_collision, x_distance, z_distance, rep_x_collision_vec, rep_z_collision_vec \ = obb.get_collistion(now_rep_global_3ds[arm_link.last_name()], now_rep_global_3ds[arm_link.first_name()], \ data_set.rep_model.bones[arm_bone_name].position.distanceToPoint(data_set.rep_model.bones[arm_link.last_name()].position)) if ( not collision and not near_collision ) or prev_rep_diff == rep_diff or np.count_nonzero( np.where( np.abs(rep_diff.data()) > 0.05, 1, 0)) == 0: # 衝突していなければこのIKターンは終了 # 前回とまったく同じ場合か、充分に近い場合、IK的に動きがないので終了 break # 再チェックしてまだ接触してて、かつ最後の場合は失敗とする if now_ik_max_count == len( avoidance_options. ik_links_list[ arm_link.last_name()]) - 1: # 最後が失敗していたら失敗 is_success.append(False) failured_last_names.append( arm_link.last_name()) prev_rep_diff = rep_diff else: logger.debug("★接触回避実行ちょっと失敗不採用(%s): f: %s(%s:%s:%s), axis: %s, 指定 [%s], 現在[%s], 差異[%s], dot[%s]", now_ik_max_count, fno, (data_set_idx + 1), \ list(ik_links.all().keys()), avoidance_name, axis, rep_collision_vec.to_log(), \ now_rep_effector_pos.to_log(), rep_diff.to_log(), list(dot_dict.values())) is_success.append(False) # 再チェックしてまだ接触してて、かつ最後の場合は失敗とする if now_ik_max_count == len( avoidance_options. ik_links_list[ arm_link.last_name()]) - 1: # 最後が失敗していたら失敗 failured_last_names.append( arm_link.last_name()) # 前回とまったく同じ場合か、充分に近い場合、IK的に動きがないので終了 if prev_rep_diff == rep_diff or np.count_nonzero( np.where( np.abs(rep_diff.data()) > 0.05, 1, 0)) == 0: break if prev_rep_diff == MVector3D(): # 初回失敗の場合、とりあえず設定 prev_rep_diff = rep_diff else: logger.debug("★接触回避実行失敗(%s): f: %s(%s:%s:%s), axis: %s, 指定 [%s], 現在[%s], 差異[%s], dot[%s]", now_ik_max_count, fno, (data_set_idx + 1), \ list(ik_links.all().keys()), avoidance_name, axis, rep_collision_vec.to_log(), \ now_rep_effector_pos.to_log(), rep_diff.to_log(), list(dot_dict.values())) is_success.append(False) # 再チェックしてまだ接触してて、かつ最後の場合は失敗とする if now_ik_max_count == len( avoidance_options.ik_links_list[ arm_link.last_name()]) - 1: # 最後が失敗していたら失敗 failured_last_names.append( arm_link.last_name()) # 前回とまったく同じ場合か、充分に近い場合、IK的に動きがないので終了 if prev_rep_diff == rep_diff or np.count_nonzero( np.where( np.abs(rep_diff.data()) > 0.05, 1, 0)) == 0: break if is_success == [True]: # 成功していたらそのまま終了 break if len(is_success) > 0: if is_success.count(True) == 0: # 全てのパターンで失敗してる場合、失敗ログ logger.info("×接触回避失敗: f: %s(%s-%s)", fno, (data_set_idx + 1), arm_link.last_display_name()) # 元々の値に戻す for ik_links in avoidance_options.ik_links_list[ arm_link.last_name()]: for link_name in list( ik_links.all().keys())[1:]: data_set.motion.regist_bf( org_bfs[link_name].copy(), link_name, fno) else: # どっか成功していたら、最後に成功したトコまで戻す is_in_elbow = False for ik_links in avoidance_options.ik_links_list[ arm_link.last_name()]: is_in_elbow = is_in_elbow or elbow_bone_name in list( ik_links.all().keys()) for link_name in list( ik_links.all().keys())[1:]: data_set.motion.regist_bf( org_bfs[link_name].copy(), link_name, fno) if not is_in_elbow: # IKリストの中にひじが含まれていない場合、キャンセル arm_bf = data_set.motion.calc_bf( arm_bone_name, fno) elbow_bf = data_set.motion.calc_bf( elbow_bone_name, fno) # 腕の変化量YZのみをひじに加算 elbow_adjust_qq = arm_bf.rotation.inverted( ) * arm_bf.org_rotation * elbow_bf.rotation logger.debug("◆手首調整: f: %s(%s:%s), arm_bf.org_rotation [%s], arm_bf.rotation[%s], elbow_bf.rotation[%s], adjust_qq[%s]", \ fno, (data_set_idx + 1), avoidance_name, arm_bf.org_rotation.toEulerAngles().to_log(), \ arm_bf.rotation.toEulerAngles().to_log(), elbow_bf.rotation.toEulerAngles().to_log(), elbow_adjust_qq.toEulerAngles().to_log()) # 再設定 elbow_bf.rotation = elbow_adjust_qq data_set.motion.regist_bf( elbow_bf, elbow_bone_name, fno) if len(is_success) > 1 and is_success.count( False) > 0: # どこかのパターンで失敗している場合、一部成功ログ logger.info("△接触回避一部成功: f: %s(%s-%s)", fno, (data_set_idx + 1), arm_link.last_display_name()) else: # 全部成功している場合、成功ログ logger.info("○接触回避成功: f: %s(%s-%s)", fno, (data_set_idx + 1), arm_link.last_display_name()) if fno // 500 > prev_block_fno and fnos[-1] > 0: logger.info("-- %sフレーム目:終了(%s%)【No.%s - 接触回避 - %s】", fno, round((fno / fnos[-1]) * 100, 3), data_set_idx + 1, direction) prev_block_fno = fno // 500 # キーの登録が増えているかもなので、ここで取り直す fnos = data_set.motion.get_bone_fnos(*target_bone_names, start_fno=(fno + 1)) for bone_name in target_bone_names: # 非活性キー削除 data_set.motion.remove_unkey_bf(data_set_idx + 1, bone_name) return True