def _rec_make_skeleton_joint(skeleton_: motion.Skeleton,
htr_joint: Htr.Joint,
parent=None):
skeleton_joint = motion.JointNode(htr_joint.name)
# (base_translation) (base_rotation) (local_translation) = (transformation)
# translation is translation part of (transformation)
if parent is None:
# root node
skeleton_joint.set_translation(mm.o_vec3())
skeleton_.set_root(skeleton_joint)
else:
if self.property_dict["EulerRotationOrder"] != "XYZ":
raise ValueError("undefined euler rotation order")
# XYZ order
# base_local_r : se3 = (base_rotation)
base_local_r = mm.so3_to_se3(
np.dot(
np.dot(
mm.rot_x(self.root.base_euler_rotation[0] *
mm.RAD),
mm.rot_y(self.root.base_euler_rotation[1] *
mm.RAD)),
mm.rot_z(self.root.base_euler_rotation[2] * mm.RAD)))
base_local_p = htr_joint.base_translation
# choose average of local_translation as fixed local translation of skeleton
local_p = np.average(htr_joint.translations, 0)
fixed_translation = base_local_p + mm.se3_to_vec3(
np.dot(base_local_r, mm.vec3_to_se3(local_p)))
skeleton_joint.set_translation(fixed_translation * scale)
skeleton_.add_node(skeleton_joint, parent)
for child in self.find_children(htr_joint):
_rec_make_skeleton_joint(skeleton_, child, skeleton_joint)
def blend(self, posture, t):
blended_posture = JointPosture(self._skeleton)
blended_posture.set_root_position(
mm.linearInterpol(self._root_position, posture.get_root_position(),
t))
blended_local_rs = list()
for i in range(len(self._local_rs)):
blended_local_rs.append(
mm.so3_to_se3(
mm.slerp(mm.se3_to_so3(self._local_rs[i]),
mm.se3_to_so3(posture.get_local_r(i)), t)))
blended_posture.set_local_rs(blended_local_rs)
return blended_posture
def _make_base_transformations(self, skeleton: motion.Skeleton):
# base_transformations are used for calculating local_rs from htr
# base_transformations are same order as motion.Skeleton.get_nodes()
base_ts = list()
for node in skeleton.get_nodes():
htr_joint = self.joint_dict[node.label]
if self.property_dict["EulerRotationOrder"] != "XYZ":
raise ValueError("undefined euler rotation order")
# XYZ order
# base_local_r : se3 = (base_rotation)
base_local_r = mm.so3_to_se3(
np.dot(
np.dot(mm.rot_x(self.root.base_euler_rotation[0] * mm.RAD),
mm.rot_y(self.root.base_euler_rotation[1] *
mm.RAD)),
mm.rot_z(self.root.base_euler_rotation[2] * mm.RAD)))
base_local_p = htr_joint.base_translation
base_ts.append(np.dot(mm.vec3_to_se3(base_local_p), base_local_r))
return base_ts
def _set_local_rs_from_htr(self, frame: int,
joint_posture: motion.JointPosture,
base_ts: list):
skeleton = joint_posture.get_skeleton()
for i in range(len(skeleton.get_nodes())):
skeleton_joint = skeleton.get_node_at(i)
htr_joint = self.joint_dict[skeleton_joint.label]
if htr_joint is self.root:
print("root!!!")
root_position = mm.se3_to_vec3(
np.dot(base_ts[i],
mm.vec3_to_se3(htr_joint.translations[frame])))
joint_posture.set_root_position(root_position)
# (base_transformation) (local_transformation) = (transformation)
# XYZ order
local_r_so3 = np.dot(
np.dot(mm.rot_x(htr_joint.euler_rotations[frame][0] * mm.RAD),
mm.rot_y(htr_joint.euler_rotations[frame][1] * mm.RAD)),
mm.rot_z(htr_joint.euler_rotations[frame][2] * mm.RAD))
base_local_r_so3 = mm.se3_to_so3(base_ts[i])
r_so3 = np.dot(base_local_r_so3, local_r_so3)
joint_posture.set_local_r_without_update(
i, mm.so3_to_se3(r_so3, mm.o_vec3()))
def ik_analytic(posture: motion.JointPosture,
joint_name_or_index,
new_position,
parent_joint_axis=None):
skeleton = posture.get_skeleton()
if isinstance(joint_name_or_index, int):
joint_index = joint_name_or_index
else:
joint_index = skeleton.get_index_by_label(joint_name_or_index)
joint_parent_index = skeleton.get_parent_index_at(joint_index)
joint_grand_parent_index = skeleton.get_parent_index_at(joint_parent_index)
B = posture.get_global_p(joint_index)
C = posture.get_global_p(joint_parent_index)
A = posture.get_global_p(joint_grand_parent_index)
L = B - A
N = B - C
M = C - A
l = mm.length(L)
n = mm.length(N)
m = mm.length(M)
a = mm.acos((l * l + n * n - m * m) / (2 * l * n))
b = mm.acos((l * l + m * m - n * n) / (2 * l * m))
B_new = new_position
L_new = B_new - A
l_ = mm.length(L_new)
a_ = mm.acos((l_ * l_ + n * n - m * m) / (2 * l_ * n))
b_ = mm.acos((l_ * l_ + m * m - n * n) / (2 * l_ * m))
# rotate joint in plane
if parent_joint_axis is not None:
rotV = np.dot(posture.get_global_r(joint_parent_index),
mm.normalize(parent_joint_axis))
else:
rotV = mm.normalize(np.cross(M, L))
if mm.length(rotV) <= 1e-9:
z_axis = np.array([0, 0, 1], float)
rotV = np.dot(posture.get_global_r(joint_parent_index), z_axis)
print(
"mm_analytic_ik.py: ik_analytic: length of rotV is 0. check the orientations of results of ik"
)
rotb = b - b_
rota = a_ - a - rotb
# posture.mulJointOrientationGlobal(joint_parent_parent, mm.exp(rotV, rotb))
# posture.mulJointOrientationGlobal(joint_parent, mm.exp(rotV * rota))
posture.set_local_r(
joint_grand_parent_index,
np.dot(posture.get_local_r(joint_grand_parent_index),
mm.so3_to_se3(mm.exp(rotV, rotb))))
posture.set_local_r(
joint_parent_index,
np.dot(posture.get_local_r(joint_parent_index),
mm.so3_to_se3(mm.exp(rotV * rota))))
# rotate plane
rotV2 = mm.normalize(np.cross(L, L_new))
l_new = mm.length(L_new)
l_diff = mm.length(L_new - L)
rot2 = mm.acos((l_new * l_new + l * l - l_diff * l_diff) / (2 * l_new * l))
# posture.mulJointOrientationGlobal(joint_parent_parent, mm.exp(rotV2, rot2))
posture.set_local_r(
joint_grand_parent_index,
np.dot(posture.get_local_r(joint_grand_parent_index),
mm.so3_to_se3(mm.exp(rotV2, rot2))))
return posture