def __init__(self, name="no name"):
self.name = name
# the base data of the joint
self.base_translation = mm.o_vec3()
self.base_euler_rotation = mm.o_vec3()
self.base_bone_length = None
# the data per frame of the joint
self.translations = []
self.euler_rotations = []
self.sfs = []
# the data name order of the data per frame
self.data_names = []
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 _get_derivative(self, index, frame0, frame1, position_func, sub_func):
if frame0 == frame1 or len(self) == 1:
return mm.o_vec3()
if frame1 is None:
frame0, frame1 = self._get_finite_difference_frames(frame0)
p0 = position_func(index, frame0)
p1 = position_func(index, frame1)
return (self.fps / (frame1 - frame0)) * sub_func(p1, p0)
def _get_derivatives(self, frame0, frame1, positions_func, sub_func):
if frame0 == frame1 or len(self) == 1:
return [mm.o_vec3()] * len(positions_func(frame0))
if frame1 is None:
frame0, frame1 = self._get_finite_difference_frames(frame0)
positions0 = positions_func(frame0)
positions1 = positions_func(frame1)
return list(
map(
lambda p0, p1: (self.fps /
(frame1 - frame0)) * sub_func(p1, p0),
positions0, positions1))
def initializeGL(self):
print("initializeGL")
GL.glClearColor(0., 0., 0., 1.)
# GL.glShadeModel(GL.GL_SMOOTH)
GL.glShadeModel(GL.GL_FLAT)
GL.glEnable(GL.GL_DEPTH_TEST)
GL.glEnable(GL.GL_CULL_FACE)
self.base_plane_renderer = renderer.PlaneRenderer(
mm.seq_to_vec3([1, 0, 0]), mm.seq_to_vec3([0, 1, 0]), 1000, 1000,
mm.o_vec3())
self.base_plane_renderer.color = (0., 0., 1., .7)
self.base_plane_renderer.set_mode(renderer.PlaneRenderer.GRID_MODE)
def add_end_effector(self, joint_name, translation):
joint = self.joint_dict[joint_name]
child_joint = Htr.Joint(joint_name + "_end_effector")
self.joint_dict[joint_name + "_end_effector"] = child_joint
self.parents.append(joint)
self.children.append(child_joint)
child_joint.base_bone_length = 1.0
total_frames = self.get_num_frame()
child_joint.translations = [translation] * total_frames
child_joint.euler_rotations = [mm.o_vec3()] * total_frames
child_joint.sfs = [1.0] * total_frames
child_joint.data_names = joint.data_names.copy()
def __init__(self,
x_axis,
y_axis,
half_size_x,
half_size_y,
center=mm.o_vec3()):
super(PlaneRenderer, self).__init__()
self.x_axis = x_axis
self.y_axis = y_axis
self.half_size_x = half_size_x
self.half_size_y = half_size_y
self.center = center
self.grid_num_x = 10
self.grid_num_y = 10
self.mode = PlaneRenderer.FILL_MODE
self.make_gl_objects()
def draw_plane(x_axis, y_axis, half_size_x, half_size_y, center=mm.o_vec3()):
# ^ y axis
# |
#
# ------y_p----
# | |
# | c x_p ---> x axis
# | |
# -------------
#
# length(x_p - c) : half_size_x, length(y_p - c) : half_size_y
vertices = [*(center - half_size_x * x_axis + half_size_y * y_axis),
*(center - half_size_x * x_axis - half_size_y * y_axis),
*(center + half_size_x * x_axis - half_size_y * y_axis),
*(center + half_size_x * x_axis + half_size_y * y_axis)
]
indices = [0, 1, 2, 3]
GL.glEnableClientState(GL.GL_VERTEX_ARRAY)
GL.glVertexPointer(3, GL.GL_FLOAT, 0, vertices)
GL.glDrawElements(GL.GL_QUADS, len(indices), GL.GL_UNSIGNED_BYTE, indices)
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 mouseReleaseEvent(self, a0: QtGui.QMouseEvent):
self.last_pos = mm.o_vec3()
def parse_htr_file(self, filepath_or_fileobject):
if isinstance(filepath_or_fileobject, str):
file = open(filepath_or_fileobject)
else:
file = filepath_or_fileobject
lines = file.read().split("\n")
lines.reverse()
while len(lines) > 0:
line = lines.pop()
if line == "[Header]":
while len(lines) > 0:
line = lines.pop()
if Htr._is_htr_keyword(line):
lines.append(line)
break
tokens = line.split()
if len(tokens) > 1:
self.property_dict[tokens[0]] = tokens[1]
elif len(tokens) == 1:
self.property_dict[tokens[0]] = None
elif line == "[SegmentNames&Hierarchy]":
lines.pop()
while len(lines) > 0:
line = lines.pop()
if Htr._is_htr_keyword(line):
lines.append(line)
break
tokens = line.split()
if len(tokens) > 1:
if tokens[0][0] == "#":
continue
try:
parent_joint = self.joint_dict[tokens[1]]
except KeyError:
parent_joint = Htr.Joint(
tokens[1]
) if tokens[1].upper() != "GLOBAL" else None
self.joint_dict[tokens[1]] = parent_joint
try:
child_joint = self.joint_dict[tokens[0]]
except KeyError:
child_joint = Htr.Joint(tokens[0])
self.joint_dict[tokens[0]] = child_joint
self.parents.append(parent_joint)
self.children.append(child_joint)
if self.root is None and parent_joint is None:
self.root = child_joint
elif line == "[BasePosition]":
while len(lines) > 0:
line = lines.pop()
if Htr._is_htr_keyword(line):
lines.append(line)
break
tokens = line.split()
if line[0] == "#":
self.base_position_names = line[1:].split()
elif len(tokens) > 0:
if len(self.base_position_names) < 1:
raise ValueError(
"the order of base position is not defined")
joint = self.joint_dict[tokens[
self.base_position_names.index("SegmentName")]]
for i in range(len(self.base_position_names)):
if self.base_position_names[i] == "Tx":
joint.base_translation[0] = tokens[i]
elif self.base_position_names[i] == "Ty":
joint.base_translation[1] = tokens[i]
elif self.base_position_names[i] == "Tz":
joint.base_translation[2] = tokens[i]
elif self.base_position_names[i] == "Rx":
joint.base_euler_rotation[0] = tokens[i]
elif self.base_position_names[i] == "Ry":
joint.base_euler_rotation[1] = tokens[i]
elif self.base_position_names[i] == "Rz":
joint.base_euler_rotation[2] = tokens[i]
elif self.base_position_names[i] == "BoneLength":
joint.base_bone_length = tokens[i]
elif line == "#Beginning of Data." or line == "#Beginning of Data":
joint = None
while len(lines) > 0:
line = lines.pop()
if Htr._is_htr_keyword(line):
lines.append(line)
break
tokens = line.split()
if len(tokens) < 1:
continue
elif tokens[0][0] == "[" and tokens[0][-1] == "]":
try:
joint = self.joint_dict[tokens[0][1:-1]]
except KeyError:
raise KeyError("undefined joint data")
elif joint is not None:
if line[0] == "#":
joint.data_names = line[1:].split()
else:
translation = mm.o_vec3()
euler_rotation = mm.o_vec3()
sf = 1.0
for i in range(len(joint.data_names)):
if joint.data_names[i] == "Tx":
translation[0] = tokens[i]
elif joint.data_names[i] == "Ty":
translation[1] = tokens[i]
elif joint.data_names[i] == "Tz":
translation[2] = tokens[i]
elif joint.data_names[i] == "Rx":
euler_rotation[0] = tokens[i]
elif joint.data_names[i] == "Ry":
euler_rotation[1] = tokens[i]
elif joint.data_names[i] == "Rz":
euler_rotation[2] = tokens[i]
elif joint.data_names[i] == "SF":
sf = float(tokens[i])
joint.translations.append(translation)
joint.euler_rotations.append(euler_rotation)
joint.sfs.append(sf)
if isinstance(filepath_or_fileobject, str):
file.close()
def draw_grid_plane(x_axis, y_axis, half_size_x, half_size_y, grid_num_x, grid_num_y, center=mm.o_vec3()):
corner = [(center - half_size_x * x_axis + half_size_y * y_axis),
(center - half_size_x * x_axis - half_size_y * y_axis),
(center + half_size_x * x_axis - half_size_y * y_axis),
(center + half_size_x * x_axis + half_size_y * y_axis)]
step_x = (corner[3] - corner[0]) / (grid_num_x + 1)
step_y = (corner[1] - corner[0]) / (grid_num_y + 1)
vertices = [*corner[0], *corner[1], *corner[2], *corner[3]]
indices = [0, 1, 3, 2, 0, 3, 1, 2]
vertex_i = 3
for i in range(grid_num_x):
vertices.extend([*(corner[0] + step_x * (i+1))])
vertices.extend([*(corner[1] + step_x * (i+1))])
indices.extend([vertex_i + 1, vertex_i + 2])
vertex_i += 2
for i in range(grid_num_y):
vertices.extend([*(corner[0] + step_y * (i+1))])
vertices.extend([*(corner[3] + step_y * (i+1))])
indices.extend([vertex_i + 1, vertex_i + 2])
vertex_i += 2
GL.glEnableClientState(GL.GL_VERTEX_ARRAY)
GL.glVertexPointer(3, GL.GL_FLOAT, 0, vertices)
GL.glDrawElements(GL.GL_LINES, len(indices), GL.GL_UNSIGNED_BYTE, indices)
def make_grid_plane(x_axis, y_axis, half_size_x, half_size_y, grid_num_x, gird_num_y, center=mm.o_vec3()):
draw_grid_plane(x_axis, y_axis, half_size_x, half_size_y, grid_num_x, gird_num_y, center=mm.o_vec3())
def make_plane(x_axis, y_axis, half_size_x, half_size_y, center=mm.o_vec3()):
draw_plane(x_axis, y_axis, half_size_x, half_size_y, center)
def __init__(self, label="no name", translation=mm.o_vec3()):
super(JointNode, self).__init__(label)
self._translation = translation
def initialize(self):
self._root_position = mm.o_vec3()
self.set_local_rs(
[mm.i_se3() for _ in range(self._skeleton.get_len_nodes())])