def difference(self, node_A, node_B, frames_compare, num_comparison):
"""
Calculate the difference between the two given nodes.
"""
motion_idx_A = node_A["motion_idx"]
frames_A = node_A["frame_start"]
motion_idx_B = node_B["motion_idx"]
frames_B = node_B["frame_start"]
diff_pose = 0.0
diff_root_ee = 0.0
diff_trajectory = 0.0
for k in range(0, frames_compare + 1,
(frames_compare + 1) // num_comparison):
pose = self.motions[motion_idx_A].get_pose_by_frame(frames_A + k)
vel = self.motions[motion_idx_A].get_velocity_by_frame(frames_A +
k)
pose_j = self.motions[motion_idx_B].get_pose_by_frame(frames_B + k)
vel_j = self.motions[motion_idx_B].get_velocity_by_frame(frames_B +
k)
if k == 0:
T_ref = pose.get_facing_transform()
T_ref_j = pose_j.get_facing_transform()
diff_pose += similarity.pose_similarity(pose, pose_j, vel, vel_j,
self.w_joint_pos,
self.w_joint_vel,
self.w_joints)
diff_root_ee += similarity.root_ee_similarity(
pose,
pose_j,
vel,
vel_j,
self.w_root_pos,
self.w_root_vel,
self.w_ee_pos,
self.w_ee_vel,
T_ref,
T_ref_j,
)
if self.w_trajectory > 0.0:
R, p = conversions.T2Rp(pose.get_facing_transform())
R_j, p_j = conversions.T2Rp(pose_j.get_facing_transform())
if k > 0:
d = np.dot(R_prev.transpose(), p - p_prev)
d_j = np.dot(R_j_prev.transpose(), p_j - p_j_prev)
d = d - d_j
diff_trajectory += np.dot(d, d)
R_prev, p_prev = R, p
R_j_prev, p_j_prev = R_j, p_j
diff_pose /= num_comparison
diff_root_ee /= num_comparison
diff_trajectory /= num_comparison
diff = diff_pose + diff_root_ee + diff_trajectory
return diff
def compare_and_connect_edge(
node_id,
nodes,
motions,
frames_compare,
w_joints,
w_joint_pos,
w_joint_vel,
w_root_pos,
w_root_vel,
w_ee_pos,
w_ee_vel,
w_trajectory,
diff_threshold,
num_comparison,
verbose,
):
node = nodes[node_id]
res = []
num_nodes = len(nodes)
motion_idx = node["motion_idx"]
frame_end = node["frame_end"]
for j in range(num_nodes):
motion_idx_j = nodes[j]["motion_idx"]
frame_start_j = nodes[j]["frame_start"]
diff_pose = 0.0
diff_root_ee = 0.0
diff_trajectory = 0.0
for k in range(0, frames_compare + 1,
(frames_compare + 1) // num_comparison):
pose = motions[motion_idx].get_pose_by_frame(frame_end + k)
vel = motions[motion_idx].get_velocity_by_frame(frame_end + k)
pose_j = motions[motion_idx_j].get_pose_by_frame(frame_start_j + k)
vel_j = motions[motion_idx_j].get_velocity_by_frame(frame_start_j +
k)
if k == 0:
T_ref = pose.get_facing_transform()
T_ref_j = pose_j.get_facing_transform()
diff_pose += similarity.pose_similarity(pose, pose_j, vel, vel_j,
w_joint_pos, w_joint_vel,
w_joints)
diff_root_ee += similarity.root_ee_similarity(
pose,
pose_j,
vel,
vel_j,
w_root_pos,
w_root_vel,
w_ee_pos,
w_ee_vel,
T_ref,
T_ref_j,
)
if w_trajectory > 0.0:
R, p = conversions.T2Rp(pose.get_facing_transform())
R_j, p_j = conversions.T2Rp(pose_j.get_facing_transform())
if k > 0:
d = np.dot(R_prev.transpose(), p - p_prev)
d_j = np.dot(R_j_prev.transpose(), p_j - p_j_prev)
d = d - d_j
diff_trajectory += np.dot(d, d)
R_prev, p_prev = R, p
R_j_prev, p_j_prev = R_j, p_j
diff_pose /= num_comparison
diff_root_ee /= num_comparison
diff_trajectory /= num_comparison
diff = diff_pose + diff_root_ee + diff_trajectory
if diff <= diff_threshold:
res.append((diff, node_id, j))
return res