def convert_target_ik2fk(self, ik_bone: Bone):
motion = self.options.motion
model = self.options.model
bone_name = ik_bone.name
logger.info("-- 腕ＩＫ変換準備:開始【%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("-- 腕ＩＫ変換準備:終了【%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("☆腕ＩＫ変換成功(%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("☆腕ＩＫ変換ちょっと失敗採用(%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("★腕ＩＫ変換ちょっと失敗不採用(%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("★腕ＩＫ変換失敗(%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("【腕ＩＫ変換 - {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
