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 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