def append_and_blend(motion1, motion2, blend_length=0):
assert isinstance(motion1, motion_class.Motion)
assert isinstance(motion2, motion_class.Motion)
assert motion1.skel.num_joints() == motion2.skel.num_joints()
combined_motion = copy.deepcopy(motion1)
combined_motion.name = f"{motion1.name}+{motion2.name}"
if motion1.num_frames() == 0:
for i in range(motion2.num_frames()):
combined_motion.poses.append(motion2.poses[i])
if hasattr(motion1, "velocities"):
combined_motion.velocities.append(motion2.velocities[i])
return combined_motion
frame_target = motion2.time_to_frame(blend_length)
frame_source = motion1.time_to_frame(motion1.length() - blend_length)
# Translate and rotate motion2 to location of frame_source
pose1 = motion1.get_pose_by_frame(frame_source)
pose2 = motion2.get_pose_by_frame(0)
R1, p1 = conversions.T2Rp(pose1.get_root_transform())
R2, p2 = conversions.T2Rp(pose2.get_root_transform())
# Translation to be applied
dp = p1 - p2
dp = dp - math.projectionOnVector(dp, motion1.skel.v_up_env)
axis = motion1.skel.v_up_env
# Rotation to be applied
Q1 = conversions.R2Q(R1)
Q2 = conversions.R2Q(R2)
_, theta = quaternion.Q_closest(Q1, Q2, axis)
dR = conversions.A2R(axis * theta)
motion2 = translate(motion2, dp)
motion2 = rotate(motion2, dR)
t_total = motion1.fps * frame_source
t_processed = 0.0
poses_new = []
for i in range(motion2.num_frames()):
dt = 1 / motion2.fps
t_total += dt
t_processed += dt
pose_target = motion2.get_pose_by_frame(i)
# Blend pose for a moment
if t_processed <= blend_length:
alpha = t_processed / float(blend_length)
pose_source = motion1.get_pose_by_time(t_total)
pose_target = blend(pose_source, pose_target, alpha)
poses_new.append(pose_target)
del combined_motion.poses[frame_source + 1:]
for i in range(len(poses_new)):
combined_motion.add_one_frame(0, copy.deepcopy(poses_new[i].data))
return combined_motion
def pose_similarity(
pose1,
pose2,
vel1=None,
vel2=None,
w_joint_pos=0.9,
w_joint_vel=0.1,
w_joints=None,
apply_root_correction=True,
):
"""
This only measure joint angle difference (i.e. root translation will
not be considered).
If 'apply_root_correction' is True, then pose2 will be rotated
automatically
in a way that its root rotation is closest to the root rotation of pose1,
where'root_correction_axis' defines the geodesic curve.
"""
assert pose1.skel.num_joints() == pose2.skel.num_joints()
skel = pose1.skel
if vel1 is not None:
assert vel2 is not None
assert vel1.skel.num_joints() == vel2.skel.num_joints()
if w_joints is None:
w_joints = np.ones(skel.num_joints())
""" joint angle difference """
diff_pos = 0.0
if w_joint_pos > 0.0:
root_idx = skel.get_index_joint(skel.root_joint)
v_up_env = pose1.skel.v_up_env
for j in range(skel.num_joints()):
R1, p1 = conversions.T2Rp(pose1.get_transform(j, local=True))
R2, p2 = conversions.T2Rp(pose2.get_transform(j, local=True))
if apply_root_correction and j == root_idx:
Q1 = conversions.R2Q(R1)
Q2 = conversions.R2Q(R2)
Q2, _ = quaternion.Q_closest(Q1, Q2, v_up_env)
R2 = conversions.Q2R(Q2)
# TODO: Verify if logSO3 is same as R2A
dR = conversions.R2A(np.dot(np.transpose(R1), R2))
diff_pos += w_joints[j] * np.dot(dR, dR)
""" joint angular velocity difference """
diff_vel = 0.0
if vel1 is not None and w_joint_vel > 0.0:
skel = vel1.skel
for j in range(skel.num_joints()):
dw = vel2.get_angular(j, local=True) - vel1.get_angular(j,
local=True)
diff_vel += w_joints[j] * np.dot(dw, dw)
return (w_joint_pos * diff_pos +
w_joint_vel * diff_vel) / skel.num_joints()
def _state_body(self,
agent,
T_ref=None,
include_com=True,
include_p=True,
include_Q=True,
include_v=True,
include_w=True,
return_stacked=True):
if T_ref is None:
T_ref = agent.get_facing_transform(self.get_ground_height())
R_ref, p_ref = conversions.T2Rp(T_ref)
R_ref_inv = R_ref.transpose()
link_states = []
link_states.append(agent.get_root_state())
ps, Qs, vs, ws = agent.get_link_states()
for j in agent._joint_indices:
link_states.append((ps[j], Qs[j], vs[j], ws[j]))
state = []
for i, s in enumerate(link_states):
p, Q, v, w = s[0], s[1], s[2], s[3]
if include_p:
p_rel = np.dot(R_ref_inv, p - p_ref)
state.append(
p_rel) # relative position w.r.t. the reference frame
if include_Q:
Q_rel = conversions.R2Q(np.dot(R_ref_inv, conversions.Q2R(Q)))
Q_rel = quaternion.Q_op(Q_rel, op=["normalize", "halfspace"])
state.append(
Q_rel) # relative rotation w.r.t. the reference frame
if include_v:
v_rel = np.dot(R_ref_inv, v)
state.append(
v_rel) # relative linear vel w.r.t. the reference frame
if include_w:
w_rel = np.dot(R_ref_inv, w)
state.append(
w_rel) # relative angular vel w.r.t. the reference frame
if include_com:
if i == 0:
p_com = agent._link_masses[i] * p
v_com = agent._link_masses[i] * v
else:
p_com += agent._link_masses[i] * p
v_com += agent._link_masses[i] * v
if include_com:
p_com /= agent._link_total_mass
v_com /= agent._link_total_mass
state.append(np.dot(R_ref_inv, p_com - p_ref))
state.append(np.dot(R_ref_inv, v_com))
if return_stacked:
return np.hstack(state)
else:
return state
def project_rotation_1D(R, axis):
"""
Project a 3D rotation matrix to the closest 1D rotation
when a rotational axis is given
"""
Q, angle = quaternion.Q_closest(
conversions.R2Q(R), [1.0, 0.0, 0.0, 0.0], axis,
)
return angle
def test_R2Q(self):
Q = test_utils.get_random_Q()
R = conversions.Q2R(Q)
np.testing.assert_almost_equal(
conversions.R2Q(np.array([R]))[0], conversions.R2Q(R)
)
np.testing.assert_almost_equal(
conversions.Q2R(np.array([Q]))[0], conversions.Q2R(Q)
)
Q_test = conversions.R2Q(R)
for q, q_test in zip(Q, Q_test):
self.assertAlmostEqual(q, q_test)
# Test if batched input conversion is same as single input
R_batched = np.array([test_utils.get_random_R() for _ in range(2)])
Q_batched = conversions.R2Q(R_batched)
for R, Q in zip(R_batched, Q_batched):
np.testing.assert_array_equal(conversions.R2Q(R), Q)
def create_ground(self):
''' Create Plane '''
if np.allclose(np.array([0.0, 0.0, 1.0]), self._v_up):
R_plane = constants.eye_R()
else:
R_plane = math.R_from_vectors(np.array([0.0, 0.0, 1.0]),
self._v_up)
self._plane_id = \
self._pb_client.loadURDF(
"plane_implicit.urdf",
[0, 0, 0],
conversions.R2Q(R_plane),
useMaximalCoordinates=True)
self._pb_client.changeDynamics(self._plane_id,
linkIndex=-1,
lateralFriction=0.9)
''' Dynamics parameters '''
assert np.allclose(np.linalg.norm(self._v_up), 1.0)
gravity = -9.8 * self._v_up
self._pb_client.setGravity(gravity[0], gravity[1], gravity[2])
self._pb_client.setTimeStep(self._dt_sim)
self._pb_client.setPhysicsEngineParameter(numSubSteps=2)
self._pb_client.setPhysicsEngineParameter(numSolverIterations=10)