def apply_axial_constraint2(q, ref_q, axis, min_angle, max_angle):
""" get delta orientation
do axis decomposition
restrict angle between min and max
"""
q = normalize(q)
inv_ref_q = quaternion_inverse(ref_q)
inv_ref_q = normalize(inv_ref_q)
delta_q = quaternion_multiply(inv_ref_q, q)
delta_q = normalize(delta_q)
swing_q, twist_q = swing_twist_decomposition(delta_q, axis)
swing_q = normalize(swing_q)
twist_q = normalize(twist_q)
v, a = quaternion_to_axis_angle(twist_q)
print("apply axial", q)
v2, a2 = quaternion_to_axis_angle(swing_q)
sign = 1
if np.sum(v) < 0:
sign = -1
a *= sign
print("twist swing", v, np.degrees(a), v2, np.degrees(a2))
print("boundary", min_angle, max_angle)
a = max(a, min_angle)
a = min(a, max_angle)
new_twist_q = quaternion_about_axis(a, axis)
new_twist_q = normalize(new_twist_q)
delta_q = quaternion_multiply(swing_q, new_twist_q)
delta_q = normalize(delta_q)
q = quaternion_multiply(ref_q, delta_q)
q = normalize(q)
return q
def apply_constraint_with_swing_global(self, node, frame, o, eps=0.01):
old_q = np.array(frame[o:o + 4])
next_node = self.bones[node].child
parent_m = self.skeleton.nodes[node].get_global_matrix(frame)[:3, :3]
node_m = self.skeleton.nodes[next_node].get_global_matrix(
frame)[:3, :3]
node_q = quaternion_from_matrix(node_m)
node_q = normalize(node_q)
# remove twist rotation
swing_q, twist_q = self.skeleton.nodes[
node].joint_constraint.split_global(parent_m, node_q)
frame[o:o + 4] = twist_q
# apply swing_q to parent
parent_q = np.array(frame[o - 4:o])
new_parent_q = quaternion_multiply(parent_q, swing_q)
new_parent_q = normalize(new_parent_q)
frame[o - 4:o] = new_parent_q
new_node_pos = self.skeleton.nodes[node].children[
0].get_global_position(frame)
diff = np.linalg.norm(new_node_pos - self.target)
return
# calculate rotation fix if necessary
if diff > eps:
delta_q = orient_joint_to_target(self.skeleton, node, frame,
new_node_pos, self.target)
aligned_parent_q = quaternion_multiply(delta_q, new_parent_q)
aligned_parent_q = normalize(aligned_parent_q)
frame[o - 4:o] = aligned_parent_q
def add_reduced_frames(skeleton, a, delta, joints):
""" returns c = a + delta where delta are the parameters of the joints list"""
c = np.array(a)
o = 0
for idx, j in enumerate(skeleton.animated_joints):
if j not in joints:
continue
if j == skeleton.root:
dest = 0
c[:3] = a[dest:dest+3] + delta[o:o+3]
q_dest = dest+3
q_o = o+3
q_a = a[q_dest:q_dest + 4]
q_delta = delta[q_o:q_o + 4]
q_prod = quaternion_multiply(q_a, q_delta)
c[q_dest:q_dest + 4] = q_prod / np.linalg.norm(q_prod)
o += 7
else:
dest = idx* 4 + 3
q_a = a[dest:dest + 4]
q_delta = delta[o:o + 4]
q_prod = quaternion_multiply(q_a, q_delta)
c[dest:dest + 4] = q_prod / np.linalg.norm(q_prod)
o += 4
return c
def align_joint(new_skeleton,
free_joint_name,
local_target_axes,
global_src_up_vec,
global_src_x_vec,
joint_cos_map,
apply_spine_fix=False):
# first align the twist axis
q, axes = align_axis(local_target_axes, "y", global_src_up_vec)
q = normalize(q)
# then align the swing axis
qx, axes = align_axis(axes, "x", global_src_x_vec)
q = quaternion_multiply(qx, q)
q = normalize(q)
# handle cases when twist axis alignment was lost
dot = np.dot(axes["y"], global_src_up_vec)
if dot <= -1:
q180 = quaternion_about_axis(np.deg2rad(180), global_src_x_vec)
q180 = normalize(q180)
q = quaternion_multiply(q180, q)
q = normalize(q)
elif abs(dot) != 1.0:
qy, axes = align_axis(axes, "y", global_src_up_vec)
q = quaternion_multiply(qy, q)
q = normalize(q)
return q
def align_root_joint(new_skeleton, free_joint_name, axes, global_src_up_vec, global_src_x_vec,joint_cos_map, max_iter_count=10):
# handle special case for the root joint
# apply only the y axis rotation of the Hip to the Game_engine node
q = [1, 0, 0, 0]
#apply first time
qx, axes = align_axis_in_place(axes, "x", global_src_x_vec) # first find rotation to align x axis
q = quaternion_multiply(qx, q)
q = normalize(q)
qy, axes = align_axis_in_place(axes, "y", global_src_up_vec) # then add a rotation to let the y axis point up
q = quaternion_multiply(qy, q)
q = normalize(q)
#apply second time
qx, axes = align_axis_in_place(axes, "x", global_src_x_vec) # first find rotation to align x axis
q = quaternion_multiply(qx, q)
q = normalize(q)
qy, axes = align_axis_in_place(axes, "y", global_src_up_vec) # then add a rotation to let the y axis point up
q = quaternion_multiply(qy, q)
q = normalize(q)
# print("handle special case for pelvis")
# handle special case of applying the x axis rotation of the Hip to the pelvis
node = new_skeleton.nodes[free_joint_name]
t_pose_global_m = node.get_global_matrix(new_skeleton.reference_frame)[:3, :3]
global_original = np.dot(t_pose_global_m, joint_cos_map[free_joint_name]["y"])
global_original = normalize(global_original)
qoffset = find_rotation_between_vectors(OPENGL_UP_AXIS, global_original)
q = quaternion_multiply(q, qoffset)
q = normalize(q)
return q
def compute_mean_quaternion(sigma_pts, init_mean):
#returns the quaternion mean of the quaternion state in the sigma_pts using gradient descent
num_pts = sigma_pts.shape[1]
# error_quat = np.empty(sigma_pts.shape)
error_vec = np.empty([3, num_pts], dtype=np.float64)
mean_error_vec = np.ones([
3,
], dtype=np.float64)
iteration = 0
while (np.linalg.norm(mean_error_vec) > 0.001 and iteration < 50):
iteration += 1
prev_mean = tf.quaternion_inverse(init_mean)
for i in range(0, num_pts):
error_quat = tf.quaternion_multiply(sigma_pts[:, i], prev_mean)
error_vec[:, i] = tf.rotvec_from_quaternion(error_quat)
mean_error_vec = np.mean(error_vec, 1)
init_mean = tf.quaternion_multiply(
tf.quaternion_from_rotvec(mean_error_vec), init_mean)
# print iteration
return init_mean, error_vec
def align_root_joint(axes, global_src_x_vec, max_iter_count=10):
# handle special case for the root joint
# apply only the y axis rotation of the Hip to the Game_engine node
not_aligned = True
q = [1, 0, 0, 0]
iter_count = 0
while not_aligned:
qx, axes = align_axis(
axes, "x", global_src_x_vec) # first find rotation to align x axis
q = quaternion_multiply(qx, q)
q = normalize(q)
qy, axes = align_axis(
axes, "y",
OPENGL_UP_AXIS) # then add a rotation to let the y axis point up
q = quaternion_multiply(qy, q)
q = normalize(q)
dot_y = np.dot(axes["y"], OPENGL_UP_AXIS)
dot_y = min(1, max(dot_y, -1))
a_y = math.acos(dot_y)
dot_x = np.dot(axes["x"], global_src_x_vec)
dot_x = min(1, max(dot_x, -1))
a_x = math.acos(dot_x)
iter_count += 1
not_aligned = a_y > 0.1 or a_x > 0.1 and iter_count < max_iter_count
return q
def axes_to_q(g_twist, g_swing, flip=False):
q = [1, 0, 0, 0]
q_y = quaternion_from_vector_to_vector(Y, g_twist)
q_y = normalize(q_y)
q = quaternion_multiply(q_y, q)
X_prime = rotate_vector(q_y, X)
X_prime = normalize(X_prime)
q_x = quaternion_from_vector_to_vector(X_prime, g_swing)
q_x = normalize(q_x)
q = quaternion_multiply(q_x, q)
q = normalize(q)
Y_prime = rotate_vector(q, Y)
dot = np.dot(Y_prime, g_twist)
#dot = min(dot,1)
dot = max(dot, -1)
if dot == -1:
q180 = quaternion_about_axis(np.deg2rad(180), g_swing)
q180 = normalize(q180)
q = quaternion_multiply(q180, q)
q = normalize(q)
elif abs(dot) != 1.0:
q_y = quaternion_from_vector_to_vector(Y_prime, g_twist)
q = quaternion_multiply(q_y, q)
q = normalize(q)
return q
def apply_axial_constraint(q, ref_q, axis, k1, k2):
""" lee 2000 p. 48"""
q0 = ref_q
w = axis
#rel_q = quaternion_multiply(quaternion_inverse(q0), q)
#rel_q = normalize(rel_q)
w_prime = rotate_vector_by_quaternion(w, q)
v = np.cross(w, w_prime)
phi = acos(np.dot(w, w_prime))
#remove rotation around v
# psi_q = exp(-phi*v)
inv_phi_q = quaternion_about_axis(-phi, v)
psi_q = quaternion_multiply(inv_phi_q, ref_q)
# get angle around w from psi_q
w_prime2 = rotate_vector_by_quaternion(w, psi_q)
w_prime2 = normalize(w_prime2)
psi = acos(np.dot(w, w_prime2))
if k1 < psi > k2: # apply
delta_psi1 = k1 - psi
delta_psi2 = k2 - psi
delta_psi = min(delta_psi1, delta_psi2)
delta_q = quaternion_about_axis(delta_psi, w)
q = quaternion_multiply(delta_q, q)
print("apply", psi)
return q
def apply_constraint_with_swing_and_lee(self, node, frame, o, eps=0.01):
old_q = frame[o:o + 4]
old_node_pos = self.skeleton.nodes[node].children[
0].get_global_position(frame)
delta_q, new_q = self.skeleton.nodes[node].joint_constraint.split(
old_q)
frame[o:o + 4] = new_q
new_node_pos = self.skeleton.nodes[node].children[
0].get_global_position(frame)
diff = np.linalg.norm(new_node_pos - old_node_pos)
if diff > eps:
parent_q = frame[o - 4:o]
new_parent_q = quaternion_multiply(parent_q, delta_q)
new_parent_q = normalize(new_parent_q)
frame[o - 4:o] = new_parent_q
new_node_pos = self.skeleton.nodes[node].children[
0].get_global_position(frame)
diff = np.linalg.norm(new_node_pos - old_node_pos)
if diff > eps:
parent_q = frame[o - 4:o]
root = None
if self.skeleton.nodes[node].parent is not None:
root = self.skeleton.nodes[node].parent.node_name
end_effector = self.skeleton.nodes[node].children[0].node_name
print("apply lee tolani", root, node, end_effector, diff)
local_axis = self.skeleton.nodes[node].joint_constraint.axis
#frame[o:o + 4] = calculate_limb_joint_rotation(self.skeleton, root, node, end_effector, local_axis, frame, self.target)
delta_q = calculate_limb_root_rotation(self.skeleton, root,
end_effector, frame,
self.target)
new_parent_q = quaternion_multiply(parent_q, delta_q)
new_parent_q = normalize(new_parent_q)
frame[o - 4:o] = new_parent_q
def qv_mult(q1, v1):
v1 = transformations.unit_vector(v1)
q2 = list(v1)
q2.append(0.0)
return transformations.quaternion_multiply(
transformations.quaternion_multiply(q1, q2),
transformations.quaternion_conjugate(q1))[:3]
def __predict_nominal_state(self, imu_measurement: np.array):
p = self.nominal_state[:3].reshape(-1, 1)
q = self.nominal_state[3:7]
v = self.nominal_state[7:10].reshape(-1, 1)
a_b = self.nominal_state[10:13].reshape(-1, 1)
w_b = self.nominal_state[13:16]
g = self.nominal_state[16:19].reshape(-1, 1)
w_m = imu_measurement[1:4].copy()
a_m = imu_measurement[4:7].reshape(-1, 1).copy()
dt = imu_measurement[0] - self.last_predict_time
"""
dp/dt = v
dv/dt = R(a_m - a_b) + g
dq/dt = 0.5 * q x(quaternion product) (w_m - w_b)
a_m and w_m are the measurements of IMU.
a_b and w_b are biases of acc and gyro, respectively.
R = R{q}, which bring the point from local coordinate to global coordinate.
"""
w_m -= w_b
a_m -= a_b
# use the zero-order integration to integrate the quaternion.
# q_{n+1} = q_n x q{(w_m - w_b) * dt}
angle = la.norm(w_m)
axis = w_m / angle
R_w = tr.rotation_matrix(0.5 * dt * angle, axis)
q_w = tr.quaternion_from_matrix(R_w, True)
q_half_next = tr.quaternion_multiply(q, q_w)
R_w = tr.rotation_matrix(dt * angle, axis)
q_w = tr.quaternion_from_matrix(R_w, True)
q_next = tr.quaternion_multiply(q, q_w)
if q_next[0] < 0: # force the quaternion has a positive real part.
q_next *= -1
# use RK4 method to integration velocity and position.
# integrate velocity first.
R = tr.quaternion_matrix(q)[:3, :3]
R_half_next = tr.quaternion_matrix(q_half_next)[:3, :3]
R_next = tr.quaternion_matrix(q_next)[:3, :3]
v_k1 = R @ a_m + g
v_k2 = R_half_next @ a_m + g
# v_k3 = R_half_next @ a_m + g # yes. v_k2 = v_k3.
v_k3 = v_k2
v_k4 = R_next @ a_m + g
v_next = v + dt * (v_k1 + 2 * v_k2 + 2 * v_k3 + v_k4) / 6
# integrate position
p_k1 = v
p_k2 = v + 0.5 * dt * v_k1 # k2 = v_next_half = v + 0.5 * dt * v' = v + 0.5 * dt * v_k1(evaluate at t0)
p_k3 = v + 0.5 * dt * v_k2 # v_k2 is evaluated at t0 + 0.5*delta
p_k4 = v + dt * v_k3
p_next = p + dt * (p_k1 + 2 * p_k2 + 2 * p_k3 + p_k4) / 6
self.nominal_state[:3] = p_next.reshape(3, )
self.nominal_state[3:7] = q_next
self.nominal_state[7:10] = v_next.reshape(3, )
def rotate(self, yaw, pitch, roll):
self.yaw += yaw
self.pitch += pitch
self.roll += roll
q1 = transformations.quaternion_about_axis(self.pitch, (1, 0, 0))
q2 = transformations.quaternion_about_axis(self.yaw, (0, 0, 1))
q3 = transformations.quaternion_about_axis(self.roll, (0, 1, 0))
self.orientation = transformations.quaternion_multiply(transformations.quaternion_multiply(q1, q2), q3)
def getBlendedPose(self, poses, weights, additiveBlending=True):
import transformations as tm
REST_QUAT = np.asarray([1, 0, 0, 0], dtype=np.float32)
if isinstance(poses[0], basestring):
f_idxs = [self.getPoseNames().index(pname) for pname in poses]
else:
f_idxs = poses
if not additiveBlending:
# normalize weights
if not isinstance(weights, np.ndarray):
weights = np.asarray(weights, dtype=np.float32)
t = sum(weights)
if t < 1:
# Fill up rest with neutral pose (neutral pose is assumed to be first frame)
weights = np.asarray(weights + [1.0 - t], dtype=np.float32)
f_idxs.append(0)
weights /= t
#print zip([self.getPoseNames()[_f] for _f in f_idxs],weights)
result = emptyPose(self.nBones)
m = np.identity(4, dtype=np.float32)
m1 = np.identity(4, dtype=np.float32)
m2 = np.identity(4, dtype=np.float32)
if len(f_idxs) == 1:
for b_idx in xrange(self.nBones):
m[:3, :4] = self.getAtFramePos(f_idxs[0], True)[b_idx]
q = tm.quaternion_slerp(REST_QUAT,
tm.quaternion_from_matrix(m, True),
float(weights[0]))
result[b_idx] = tm.quaternion_matrix(q)[:3, :4]
else:
for b_idx in xrange(self.nBones):
m1[:3, :4] = self.getAtFramePos(f_idxs[0], True)[b_idx]
m2[:3, :4] = self.getAtFramePos(f_idxs[1], True)[b_idx]
q1 = tm.quaternion_slerp(REST_QUAT,
tm.quaternion_from_matrix(m1, True),
float(weights[0]))
q2 = tm.quaternion_slerp(REST_QUAT,
tm.quaternion_from_matrix(m2, True),
float(weights[1]))
quat = tm.quaternion_multiply(q2, q1)
for i, f_idx in enumerate(f_idxs[2:]):
i += 2
m[:3, :4] = self.getAtFramePos(f_idx, True)[b_idx]
q = tm.quaternion_slerp(REST_QUAT,
tm.quaternion_from_matrix(m, True),
float(weights[i]))
quat = tm.quaternion_multiply(q, quat)
result[b_idx] = tm.quaternion_matrix(quat)[:3, :4]
return Pose(self.name + '-blended', result)
def point_rotation_by_quaternion(point, q):
"""
Rotate a 3D point by quaternion
:param point: [x, y, z]
:param q: [qw, qx, qy, qz]
:return rotated_point: [x, y, ]
"""
r = [0, point[0], point[1], point[2]]
q_conj = [q[0], -1 * q[1], -1 * q[2], -1 * q[3]]
return quaternion_multiply(quaternion_multiply(q, r), q_conj)[1:]
def on_timer(self, event):
# auto rotating
qy = tr.quaternion_about_axis(0.005, [0,0,1])
qx = tr.quaternion_about_axis(0.005, [0,1,0])
q = tr.quaternion_multiply(qx,qy)
self.rot = tr.quaternion_multiply(self.rot, q)
self.model = tr.quaternion_matrix(self.rot)
self.program_data['u_model'] = self.model
self.program_axis['u_model'] = self.model
self.program_plane['u_model'] = self.model
self.update()
def on_timer(self, event):
# auto rotating
qy = tr.quaternion_about_axis(0.005, [0, 0, 1])
qx = tr.quaternion_about_axis(0.005, [0, 1, 0])
q = tr.quaternion_multiply(qx, qy)
self.rot = tr.quaternion_multiply(self.rot, q)
self.model = tr.quaternion_matrix(self.rot)
self.program_data['u_model'] = self.model
self.program_axis['u_model'] = self.model
self.program_plane['u_model'] = self.model
self.update()
def quaternion_rotate_vector(q, vector):
""" src: http://math.stackexchange.com/questions/40164/how-do-you-rotate-a-vector-by-a-unit-quaternion
Args:
q:
vector:
Returns:
"""
tmp_result = quaternion_multiply(q, vector)
return quaternion_multiply(tmp_result, quaternion_conjugate(q))[1:]
def find_rotation_analytically(new_skeleton,
free_joint_name,
target,
frame,
joint_cos_map,
is_root=False,
max_iter_count=10,
twist_angle=None):
global_src_up_vec = target[0]
if twist_angle is None:
global_src_x_vec = target[1]
else:
global_src_x_vec = None
local_target_axes = dict(joint_cos_map[free_joint_name])
if is_root:
q = align_root_joint(new_skeleton, free_joint_name, local_target_axes,
global_src_up_vec, global_src_x_vec,
joint_cos_map, max_iter_count)
else:
# first align the bone vectors
q = [1, 0, 0, 0]
qy, axes = align_axis_in_place(local_target_axes, "y",
global_src_up_vec)
q = quaternion_multiply(qy, q)
q = normalize(q)
# then align the twisting angles
if global_src_x_vec is not None:
qx, axes = align_axis_in_place(axes, "x", global_src_x_vec)
q = quaternion_multiply(qx, q)
q = normalize(q)
#if "FK" in free_joint_name:
# q = to_local_cos(new_skeleton, free_joint_name, frame, q)
if new_skeleton.nodes[free_joint_name].parent is not None:
#if "upLeg" in free_joint_name: # it does not work for the legs for some reason
# q = to_local_cos(new_skeleton, new_skeleton.nodes[free_joint_name].parent.node_name, frame, q)
#else:
q = to_local_cos(new_skeleton,
new_skeleton.nodes[free_joint_name].parent.node_name,
frame, q)
if twist_angle is not None:
# separate rotation
local_twist_axis = np.array(joint_cos_map[free_joint_name]["y"])
swing_q, twist_q = swing_twist_decomposition(q, local_twist_axis)
# replace
twist_q = quaternion_about_axis(-twist_angle, local_twist_axis)
q = quaternion_multiply(swing_q, twist_q)
q = normalize(q)
return q
def norm(self):
""":obj:`tuple` of :obj:`numpy.ndarray`: The normalized vectors for qr and qd, respectively.
"""
qr_c = quaternion_conjugate(self._qr)
qd_c = quaternion_conjugate(self._qd)
qr_norm = np.linalg.norm(quaternion_multiply(self._qr, qr_c))
qd_norm = np.linalg.norm(
quaternion_multiply(self._qr, qd_c) +
quaternion_multiply(self._qd, qr_c))
return (qr_norm, qd_norm)
def set_orientation(self, orientation):
print("rotate frames")
current_q = self.mv.frames[self.frame_idx, 3:7]
delta_q = quaternion_multiply(orientation,
quaternion_inverse(current_q))
delta_q /= np.linalg.norm(delta_q)
delta_m = quaternion_matrix(delta_q)[:3, :3]
for idx in range(self.mv.n_frames):
old_t = self.mv.frames[idx, :3]
old_q = self.mv.frames[idx, 3:7]
self.mv.frames[idx, :3] = np.dot(delta_m, old_t)[:3]
self.mv.frames[idx, 3:7] = quaternion_multiply(delta_q, old_q)
def align_joint(local_target_axes, up_vec, x_vec):
q = [1, 0, 0, 0]
qy, axes = align_axis_in_place(local_target_axes, "y", up_vec)
q = quaternion_multiply(qy, q)
q = normalize(q)
# then align the twisting angles
if x_vec is not None:
qx, axes = align_axis_in_place(axes, "x", x_vec)
q = quaternion_multiply(qx, q)
q = normalize(q)
# print("set twist angle", free_joint_name, twist_angle)
return q
def get_pose_by_trajectory(self, node, last_pose, trj_type, trj_value,
direction, orientation):
pose = np.copy(node.pose)
last_root_pos = last_pose[:3]
root_velocity = node.velocity[:3]
if trj_type == "euler":
current_pos = last_root_pos + np.linalg.norm(
root_velocity) * direction
pose[:3] = current_pos
pose[3:7] = quaternion_multiply(pose[3:7], node.velocity[3:7])
pose[3:7] = quaternion_multiply(
pose[3:7], quaternion_from_euler(orientation, 0, 0))
return pose
def getBlendedPose(self, poses, weights, additiveBlending=True):
import transformations as tm
REST_QUAT = np.asarray([1,0,0,0], dtype=np.float32)
if isinstance(poses[0], basestring):
f_idxs = [self.getPoseNames().index(pname) for pname in poses]
else:
f_idxs = poses
if not additiveBlending:
# normalize weights
if not isinstance(weights, np.ndarray):
weights = np.asarray(weights, dtype=np.float32)
t = sum(weights)
if t < 1:
# Fill up rest with neutral pose (neutral pose is assumed to be first frame)
weights = np.asarray(weights + [1.0-t], dtype=np.float32)
f_idxs.append(0)
weights /= t
#print zip([self.getPoseNames()[_f] for _f in f_idxs],weights)
result = emptyPose(self.nBones)
m = np.identity(4, dtype=np.float32)
m1 = np.identity(4, dtype=np.float32)
m2 = np.identity(4, dtype=np.float32)
if len(f_idxs) == 1:
for b_idx in xrange(self.nBones):
m[:3, :4] = self.getAtFramePos(f_idxs[0], True)[b_idx]
q = tm.quaternion_slerp(REST_QUAT, tm.quaternion_from_matrix(m, True), float(weights[0]))
result[b_idx] = tm.quaternion_matrix( q )[:3,:4]
else:
for b_idx in xrange(self.nBones):
m1[:3, :4] = self.getAtFramePos(f_idxs[0], True)[b_idx]
m2[:3, :4] = self.getAtFramePos(f_idxs[1], True)[b_idx]
q1 = tm.quaternion_slerp(REST_QUAT, tm.quaternion_from_matrix(m1, True), float(weights[0]))
q2 = tm.quaternion_slerp(REST_QUAT, tm.quaternion_from_matrix(m2, True), float(weights[1]))
quat = tm.quaternion_multiply(q2, q1)
for i,f_idx in enumerate(f_idxs[2:]):
i += 2
m[:3, :4] = self.getAtFramePos(f_idx, True)[b_idx]
q = tm.quaternion_slerp(REST_QUAT, tm.quaternion_from_matrix(m, True), float(weights[i]))
quat = tm.quaternion_multiply(q, quat)
result[b_idx] = tm.quaternion_matrix( quat )[:3,:4]
return Pose(self.name+'-blended', result)
def lamePath(dist, ang, side=50.8, r=3, oset=10, compAng=numpy.radians(0)):
q = numpy.array([[0., 0., -0.71, 0.71], [0, 0, 1, 0], [0., 0., 0.71,
0.71]])
if numpy.round(ang % 3.141, 2) > .01: d = side / numpy.tan(ang)
else: d = 0
do = oset * numpy.cos(ang)
zo = oset * numpy.sin(ang)
s = numpy.array([[-do, side / 2, -zo], [d + do, side / 2, side + zo]])
s1 = numpy.array([[d + do, -side / 2, zo + side], [-do, -side / 2, -zo]])
top = [[d, ((side / 2) + oset), side], [d, -((side / 2) + oset), side]]
topAng = (numpy.pi / 2) - ang
botAng = -topAng
topAng = thresh(topAng, numpy.radians(50))
botAng = thresh(botAng, numpy.radians(50))
qBot = transformations.quaternion_multiply(
q[1], transformations.quaternion_about_axis(botAng, [0, 1, 0]))
print '\n\n\QBOT', qBot
angVec = [numpy.cos(ang), 0, numpy.sin(ang)]
q[0] = transformations.quaternion_multiply(
q[0], transformations.quaternion_about_axis(-compAng, angVec))
q[1] = transformations.quaternion_multiply(
q[1], transformations.quaternion_about_axis(topAng, [0, 1, 0]))
q[2] = transformations.quaternion_multiply(
q[2], transformations.quaternion_about_axis(compAng, angVec))
q = unitVector(q)
s = numpy.hstack((s, [q[0], q[0]]))
s2 = numpy.hstack((s1, [q[2], q[2]]))
top = numpy.hstack((top, [q[1], q[1]]))
top = numpy.array([s, top, s2])
numpy.set_printoptions(precision=3, suppress=True)
top[:, :, 0] -= numpy.min(top[:, :, 0]) + dist
bf = 1.375
bottom = [[-dist + (bf * do), -((side / 2) + oset), side],
[-dist + (bf * do), (side / 2) + oset, side]]
bottom = numpy.hstack((bottom, [qBot, qBot]))
return (top, bottom)
def quaternion_fromto_test(tb):
q1 = tb.utils.ArrayToQuat([0, 0, 0, 1])
q2 = tb.utils.ArrayToQuat([0, 0, 0, 1])
q3 = tb.utils.QuaternionFromTo(q1, q2)
q4 = tb.utils.MultiplyQuaternion(q1, q3)
print q3
print q4
print transformations.quaternion_multiply(tb.utils.QuatToArray(q2),
tb.utils.QuatToArray(q3))
raw_input("Press enter to cont...")
def virtual(worktarget=[945, 0, 0], cartesians=[945, 0, 1375]):
# returns a virtual target that the robot can actually reach
# virtual target will be pointed towards worktarget and located at cartesians
# Quaternion [0,0,1,0] has z vector [0,0,-1]
z0 = [0, 0, -1]
zq0 = [0, 0, 1, 0]
a = [[0], [0], [1], [1]]
if (len(cartesians) == 3) & (len(worktarget) == 3):
#unit direction vector from defined robot position to worktarget position
dvec = unitv((worktarget - numpy.array(cartesians)))
if (~parallel(dvec, z0)):
rotvec = numpy.cross(dvec, z0)
rotangle = numpy.arcsin(norm(rotvec) / (norm(z0) * norm(dvec)))
rotquat = transformations.quaternion_about_axis(-rotangle, rotvec)
virtualresult = [
cartesians,
transformations.quaternion_multiply(rotquat, zq0)
]
else:
virtualresult = [cartesians, zq0]
return virtualresult
else:
return False
def graspExecute(limb, W, H, Ang):
endEffPos = pixelToWorld(W, H)
quat1 = transformations.quaternion_about_axis(Ang, (0, 0, 1))
quat2 = transformations.quaternion_about_axis(np.pi, (1, 0, 0))
quat = transformations.quaternion_multiply(quat1, quat2)
# print(quat)
top_grasp_joint = ik_service_client(limb='right',
use_advanced_options=True,
p_x=endEffPos[0],
p_y=endEffPos[1],
p_z=0.5,
q_x=quat[0],
q_y=quat[1],
q_z=quat[2],
q_w=quat[3])
down_grasp_joint = ik_service_client(limb='right',
use_advanced_options=True,
p_x=endEffPos[0],
p_y=endEffPos[1],
p_z=0.1,
q_x=quat[0],
q_y=quat[1],
q_z=quat[2],
q_w=quat[3])
move(limb, positions=top_grasp_joint, move_speed=0.2)
rospy.sleep(2)
move(limb, positions=down_grasp_joint, move_speed=0.2)
rospy.sleep(2)
move(limb, positions=top_grasp_joint, move_speed=0.2)
def _create_actions_from_unity_format(self):
if self._constraints_dict is None:
return
p = self._constraints_dict["startPosition"]
q = self._constraints_dict["startOrientation"]
start_pose = dict()
start_pose["position"] = np.array([p["x"], p["y"], p["z"]])
q = np.array([q["w"], q["x"], q["y"], q["z"]])
q = quaternion_multiply([0, 0, 1, 0], q)
start_pose["orientation"] = np.degrees(euler_from_quaternion(q))
print("start rotation", start_pose["orientation"])
self._controller.start_pose = start_pose
self._action_sequence = []
print("create objects from constraints")
scene = self._parent.app_manager.getDisplayedScene()
a = [None, []]
a[0] = self._constraints_dict["name"]
a[1] = []
for c in self._constraints_dict["frameConstraints"]:
p = c["position"]
p = np.array([p["x"], p["y"], p["z"]])
# q = c["orientation"]
# c["orientation"] = np.array([q["w"], q["x"], q["y"], q["z"]])
annotation = c["keyframe"]
joint_name = c["joint"]
scene_object = PositionConstraintObject(p, 2.5)
scene.addObject(scene_object)
a[1].append(
ConstraintDefinition(scene_object,
joint_name,
annotation=annotation))
self._action_sequence.append(a)
self.update_action_sequence_list()
def transform_quaternion_frames_legacy(quat_frames,
angles,
offset,
rotation_order=None):
""" Applies a transformation on the root joint of a list quaternion frames.
Parameters
----------
*quat_frames: np.ndarray
\tList of frames where the rotation is represented as euler angles in degrees.
*angles: list of floats
\tRotation angles in degrees
*offset: np.ndarray
\tTranslation
"""
if rotation_order is None:
rotation_order = DEFAULT_ROTATION_ORDER
offset = np.array(offset)
if round(angles[0], 3) == 0 and round(angles[2], 3) == 0:
rotation_q = quaternion_about_axis(np.deg2rad(angles[1]), [0, 1, 0])
else:
rotation_q = euler_to_quaternion(angles, rotation_order)
rotation_matrix = euler_angles_to_rotation_matrix(angles,
rotation_order)[:3, :3]
for frame in quat_frames:
ot = frame[:3]
oq = frame[3:7]
frame[:3] = np.dot(rotation_matrix, ot) + offset
frame[3:7] = quaternion_multiply(rotation_q, oq)
return quat_frames
def apply_joint_rotation_offset(self,
joint_name,
euler,
frame_range=None,
blend_window_size=None):
motion = self.get_motion()
print("euler ", euler)
delta_q = quaternion_from_euler(*np.radians(euler))
if motion.frames.ndim == 1:
motion.frames = motion.frames.reshape((1, len(motion.frames)))
j_offset = self.get_skeleton().animated_joints.index(
joint_name) * 4 + 3
if frame_range is None:
start_frame = 0
end_frame = motion.n_frames
else:
start_frame, end_frame = frame_range
end_frame = min(motion.n_frames, end_frame + 1)
for idx in range(start_frame, end_frame):
old_q = motion.frames[idx, j_offset:j_offset + 4]
motion.frames[idx, j_offset:j_offset + 4] = quaternion_multiply(
delta_q, old_q)
if frame_range is not None and blend_window_size is not None:
joint_list = [joint_name]
offset = self.skeleton.nodes[
joint_name].quaternion_frame_index * 4 + 3
joint_index_list = list(range(offset, offset + 4))
motion.frames = apply_blending(self.skeleton, motion.frames,
joint_list, joint_index_list,
start_frame, end_frame - 1,
blend_window_size)
def get_rotation_from_pc(self, src_name, target_name, src_pc, src_frame):
child_name = self.src_child_map[src_name]
global_src_up_vec, global_src_x_vec = self.estimate_src_joint_cos(
src_name, child_name, target_name, src_pc)
if "Spine" in src_name:
print("axis", src_name, global_src_up_vec, global_src_up_vec)
q = [1, 0, 0, 0]
local_target_axes = self.src_cos_map[src_name]
qy, axes = align_axis(local_target_axes, "y", global_src_up_vec)
q = quaternion_multiply(qy, q)
q = normalize(q)
if global_src_x_vec is not None:
qx, axes = align_axis(axes, "x", global_src_x_vec)
q = quaternion_multiply(qx, q)
q = normalize(q)
return to_local_cos(self.src_skeleton, src_name, src_frame, q)
def to_local_cos2(self, joint_name, frame, q):
# bring into parent coordinate system
parent_joint = self.skeleton.nodes[joint_name].parent.node_name
pm = self.skeleton.nodes[parent_joint].get_global_matrix(frame)#[:3, :3]
inv_p = quaternion_inverse(quaternion_from_matrix(pm))
normalize(inv_p)
return quaternion_multiply(inv_p, q)
def rotate_around(self, angle, axis):
'''
Rotate the render object around the axis.
Raises an error if self.orientation is None.
'''
ori = self.orientation
if ori is None:
raise NotImplemented("This RenderObject has no orientation")
self.orientation = quaternion_multiply(ori, quaternion_about_axis(angle, axis))
def rotate(self, roll, pitch, yaw):
'''
Rotate the render object around the axes.
Raises an error if self.orientation is None.
'''
ori = self.orientation
if ori is None:
raise
self.orientation = quaternion_multiply(quaternion_from_euler(roll, pitch, yaw), ori)
def on_mouse_move(self, event):
# drag to rotate
if event.is_dragging:# and not self.timer.running:
# drag delta
dx = event.pos[0]-self.last_drag_pos[0]
dy = event.pos[1]-self.last_drag_pos[1]
dampen = 0.05
# quaternion rotation
qy = tr.quaternion_about_axis(dx*dampen, [0,-1,0])
qx = tr.quaternion_about_axis(dy*dampen, [-1,0,0])
q = tr.quaternion_multiply(qx,qy)
self.rot = tr.quaternion_multiply(self.rot, q)
# rotate model
self.model = tr.quaternion_matrix(self.rot)
# update model
self.program_data['u_model'] = self.model
self.program_axis['u_model'] = self.model
self.program_plane['u_model'] = self.model
self.update()
self.last_drag_pos = event.pos
pass
def plus(x1, x2):
"""Returns the SO3 quantity that represents first rotating
by x1 and then by x2 (i.e. the composition of x2 on x1, call
it "x1+x2"). The output will be in the same representation as the
inputs, but the inputs must be in the same representation as each other.
>>> R1 = trns.random_rotation_matrix()[:3, :3]
>>> R2 = trns.random_rotation_matrix()[:3, :3]
>>> R3 = R2.dot(R1) # rotating vector v by R3 is R2*R1*v
>>> R = plus(R1, R2)
>>> print(np.allclose(R, R3))
True
>>> M1 = make_affine(R1, [4, 4, 4])
>>> M2 = make_affine(R2, [5, 5, 5])
>>> M3 = make_affine(R2.dot(R1), R2.dot([4, 4, 4]) + [5, 5, 5])
>>> M = plus(M1, M2)
>>> print(np.allclose(M, M3))
True
>>> print(np.allclose(M[:3, :3], R))
True
>>> q1 = trns.quaternion_from_matrix(R1)
>>> q2 = trns.quaternion_from_matrix(R2)
>>> q3 = trns.quaternion_multiply(q2, q1)
>>> q = plus(q1, q2)
>>> print(np.allclose(q, q3))
True
>>> print(np.allclose(trns.quaternion_matrix(q)[:3, :3], R))
True
>>> R1plusR2plusR3 = reduce(plus, [R1, R2, R3]) # using plus in succession
>>> print(rep(R1plusR2plusR3))
rotmat
"""
xrep = rep(x1)
xrep2 = rep(x2)
if xrep != xrep2:
raise ValueError('Values to "add" are not in the same SO3 representation.')
if xrep == 'quaternion':
return normalize(trns.quaternion_multiply(x2, x1))
elif xrep in ['rotmat', 'tfmat']:
return x2.dot(x1)
elif xrep == 'rotvec':
# Adding rotvecs is only valid for small perturbations...
# Perform operation on actual SO3 manifold instead:
x1 = matrix_from_rotvec(x1)
x2 = matrix_from_rotvec(x2)
return get_rotvec(x2.dot(x1))
else:
raise ValueError('Values to "add" are not on SO3.')
def minus(x1, x2):
"""Returns the SO3 quantity representing the minimal rotation from
orientation x1 to orientation x2 (i.e. the inverse composition of x2 on x1,
call it "x2-x1"). The output will be in the same representation as the
inputs, but the inputs must be in the same representation as each other.
>>> A = trns.rotation_matrix(.2, [0, 1, 0]) # rotation of 0.2 rad about +y axis
>>> B = trns.rotation_matrix(-.2, [0, 1, 0]) # rotation of -0.2 rad about +y axis
>>> AtoB = minus(A, B) # this is "B - A", the rotation from A to B
>>> angle, axis = angle_axis(AtoB) # should be a rotation of (-0.2)-(0.2) = -0.4 rad about +y axis
>>> print(angle, axis)
(0.39999999999999997, array([-0., -1., -0.]))
>>> # The above is 0.4 rad about -y axis, which is equivalent to the expected -0.4 rad about the +y axis.
>>> # As usual, the angle is always returned between 0 and pi, with the accordingly correct axis.
>>> # The rotations need not be about the same axis of course:
>>> NtoA = trns.random_quaternion() # some rotation from frame N to frame A
>>> NtoB = trns.random_quaternion() # some rotation from frame N to frame B
>>> AtoB = minus(NtoA, NtoB) # We say "AtoB = NtoB - NtoA"
>>> NtoAtoB = plus(NtoA, AtoB) # NtoAtoB == NtoB and we say "NtoAtoB = NtoA + AtoB"
>>> print(np.allclose(NtoAtoB, NtoB))
True
>>> # Evidently, plus and minus are inverse operations.
>>> # "x1 + (x2 - x1) = x2" reads as "(N to x1) plus (x1 to x2) = (N to x2)"
>>> A[:3, 3] = trns.random_vector(3)
>>> B[:3, 3] = trns.random_vector(3)
>>> C1 = B.dot(npl.inv(A))
>>> C2 = minus(A, B)
>>> print(np.allclose(C1, C2))
True
"""
xrep = rep(x1)
xrep2 = rep(x2)
if xrep != xrep2:
raise ValueError('Values to "subtract" are not in the same SO3 representation.')
if xrep == 'quaternion':
return normalize(trns.quaternion_multiply(x2, trns.quaternion_inverse(x1)))
elif xrep == 'rotmat':
return x2.dot(x1.T)
elif xrep == 'tfmat':
return x2.dot(npl.inv(x1)) # inverse of a tfmat is not its transpose
elif xrep == 'rotvec':
# Subtracting rotvecs is only valid for small perturbations...
# Perform operation on actual SO3 manifold instead:
x1 = matrix_from_rotvec(x1)
x2 = matrix_from_rotvec(x2)
return get_rotvec(x2.dot(x1.T))
else:
raise ValueError('Values to "subtract" are not on SO3.')
def slerp(x1, x2, fraction):
"""Spherical Linear intERPolation. Returns an SO3 quantity representing an orientation
between x1 and x2. The fraction of the path from x1 to x2 is the fraction input, and it
must be between 0 and 1. The output will be in the same representation as the inputs, but
the inputs must be in the same representation as each other.
>>> x1 = make_affine(trns.random_rotation_matrix()[:3, :3], [1, 1, 1])
>>> x2 = make_affine(trns.random_rotation_matrix()[:3, :3], [2, 2, 2])
>>> nogo = slerp(x1, x2, 0)
>>> print(np.allclose(nogo, x1))
True
>>> allgo = slerp(x1, x2, 1)
>>> print(np.allclose(allgo, x2))
True
>>> first_25 = slerp(x1, x2, 0.25) # 0 to 25 percent from x1 to x2
>>> last_75 = minus(first_25, x2) # 25 to 75 percent from x1 to x2
>>> wholeway = plus(first_25, last_75)
>>> print(np.allclose(wholeway, x2))
True
>>> x1 = trns.quaternion_from_matrix(x1)
>>> x2 = trns.quaternion_from_matrix(x2)
>>> mine = slerp(x1, x2, 0.3)
>>> his = trns.quaternion_slerp(x1, x2, 0.3) # only works on quaternions
>>> print(np.allclose(mine, his))
True
"""
xrep = rep(x1)
xrep2 = rep(x2)
if xrep != xrep2:
raise ValueError('Values to slerp between are not in the same SO3 representation.')
if xrep == 'quaternion':
r12 = get_rotvec(trns.quaternion_multiply(x2, trns.quaternion_inverse(x1)))
x12 = normalize(quaternion_from_rotvec(fraction * r12))
elif xrep == 'rotmat':
r12 = get_rotvec(x2.dot(x1.T))
x12 = matrix_from_rotvec(fraction * r12)
elif xrep == 'tfmat':
M12 = x2.dot(npl.inv(x1))
r12 = get_rotvec(M12)
x12 = make_affine(matrix_from_rotvec(fraction * r12), fraction * M12[:3, 3])
elif xrep == 'rotvec':
R1 = matrix_from_rotvec(x1)
R2 = matrix_from_rotvec(x2)
x12 = fraction * get_rotvec(R2.dot(R1.T))
else:
raise ValueError("One or both values to slerp between are not on SO3.")
return plus(x1, x12)
def computeCameraPoseFromAircraft(image, cam, ref,
yaw_bias=0.0, roll_bias=0.0,
pitch_bias=0.0, alt_bias=0.0):
lla, ypr, ned2body = image.get_aircraft_pose()
aircraft_lat, aircraft_lon, aircraft_alt = lla
#print "aircraft quat =", world2body
msl = aircraft_alt + image.alt_bias + alt_bias
(camera_yaw, camera_pitch, camera_roll) = cam.get_mount_params()
body2cam = transformations.quaternion_from_euler(camera_yaw * d2r,
camera_pitch * d2r,
camera_roll * d2r,
'rzyx')
ned2cam = transformations.quaternion_multiply(ned2body, body2cam)
(yaw, pitch, roll) = transformations.euler_from_quaternion(ned2cam, 'rzyx')
ned = navpy.lla2ned( aircraft_lat, aircraft_lon, aircraft_alt,
ref[0], ref[1], ref[2] )
#print "aircraft=%s ref=%s ned=%s" % (image.get_aircraft_pose(), ref, ned)
return (ned.tolist(), [yaw/d2r, pitch/d2r, roll/d2r])
def minus(q1, q2):
"""Returns a quaternion representing the minimal rotation from
orientation q1 to orientation q2 (i.e. the inverse composition
of q2 on q1, call it "q2-q1").
>>> NtoA = trns.random_quaternion() # some rotation from frame N to frame A
>>> NtoB = trns.random_quaternion() # some rotation from frame N to frame B
>>> AtoB = minus(NtoA, NtoB) # We say "AtoB = NtoB - NtoA"
>>> NtoAtoB = plus(NtoA, AtoB) # NtoAtoB == NtoB and we say "NtoAtoB = NtoA + AtoB"
>>> print(np.allclose(NtoAtoB, NtoB))
True
>>> # Evidently, plus and minus are inverse operations.
>>> # "q1 + (q2 - q1) = q2" reads as "(N to q1) plus (q1 to q2) = (N to q2)"
>>> q = plus(NtoA, minus(NtoA, NtoB))
>>> print(np.allclose(q, NtoB))
True
"""
return normalize(trns.quaternion_multiply(q2, trns.quaternion_inverse(q1)))
def plus(q1, q2):
"""Returns the quaternion that represents first rotating
by q1 and then by q2 (i.e. the composition of q2 on q1,
call it "q1+q2").
>>> q1 = trns.random_quaternion()
>>> q2 = trns.random_quaternion()
>>> q12 = plus(q1, q2)
>>>
>>> p1 = trns.random_vector(3)
>>> p2 = qapply_points(q1, p1)
>>> p3a = qapply_points(q2, p2)
>>>
>>> p3b = qapply_points(q12, p1)
>>> print(np.allclose(p3a, p3b))
True
"""
return normalize(trns.quaternion_multiply(q2, q1))
def ode( self , t , Q ) : w = Q[:3] q = Q[3:] q = q / np.linalg.norm( q ) qm = tr.inverse_matrix( tr.quaternion_matrix(q) ) if self.drawstate & GRAVITY : self.G = np.resize( np.dot( qm , self.g ) , 3 ) N = np.cross( self.r , self.G ) else : N = np.zeros(3) # print self.G , N , np.linalg.norm(self.G) , np.linalg.norm(w) QP = np.empty(7,np.float64) QP[:3] = np.dot( self.Mi , ( N + np.cross( np.dot(self.M,w) , w ) ) ) qw = np.empty(4,np.float64) qw[0] = 0 qw[1:] = w QP[3:] = tr.quaternion_multiply( q , qw ) / 2.0 return QP
bgain = 0.001
yawEst = []
yawINS = []
pitchEst = []
pitchINS = []
rollEst = []
rollINS = []
yaw_meas = []
omegas = []
yaw_crct = []
for i,t in enumerate(time):
# integrate ins
omega = np.array([0,p[i], q[i], r[i]])
omega_bias = np.array([0,p[i]-biasEst[0], q[i]-biasEst[1], r[i]-biasEst[2]])
q_ins = q_est + 0.5*dt*trans.quaternion_multiply(q_est, omega_bias)
q_insOnly = q_insOnly + 0.5*dt*trans.quaternion_multiply(q_insOnly, omega)
yawINS.append(trans.euler_from_quaternion(q_insOnly)[2])
pitchINS.append(trans.euler_from_quaternion(q_insOnly)[0])
rollINS.append(trans.euler_from_quaternion(q_insOnly)[1])
# generate truth quaternion
#quat_meas = trans.quaternion_from_euler(yaw[i],pitch[i],roll[i])
q_meas = trans.quaternion_from_euler(roll[i],pitch[i],yaw[i])
yaw_meas.append(trans.euler_from_quaternion(q_meas)[0])
q_error = trans.quaternion_multiply(q_meas,trans.quaternion_inverse(q_ins))
q_error = q_error / np.linalg.norm(q_error)
q_error = np.hstack((1,q_error[1:4]))
import robot import transformations as tr import numpy as np r = robot.Robot() print "joints" print r.getJoints() r.setCartesian([[800,0,1000], [0,0,1,0]]) #quat [0,0,1,0] is tool # flange pointed down quat_down = [0,0,1,0] quat_stuff = tr.quaternion_multiply(quat_down, tr.quaternion_about_axis(np.radians(30), [0,1,0])) #rotate 30 degrees # around the Y axis r.setCartesian([[800,0,1000], quat_stuff]) r.close()
flight_mode = 'auto'
body2cam = transformations.quaternion_from_euler( cam_yaw * d2r,
cam_pitch * d2r,
cam_roll * d2r,
'rzyx')
#print 'att:', [yaw_rad, pitch_rad, roll_rad]
ned2body = transformations.quaternion_from_euler(yaw_rad,
pitch_rad,
roll_rad,
'rzyx')
body2ned = transformations.quaternion_inverse(ned2body)
#print 'ned2body(q):', ned2body
ned2cam_q = transformations.quaternion_multiply(ned2body, body2cam)
ned2cam = np.matrix(transformations.quaternion_matrix(np.array(ned2cam_q))[:3,:3]).T
#print 'ned2cam:', ned2cam
R = ned2proj.dot( ned2cam )
rvec, jac = cv2.Rodrigues(R)
ned = navpy.lla2ned( lat_deg, lon_deg, altitude_m,
ref[0], ref[1], ref[2] )
#print 'ned:', ned
tvec = -np.matrix(R) * np.matrix(ned).T
R, jac = cv2.Rodrigues(rvec)
# is this R the same as the earlier R?
PROJ = np.concatenate((R, tvec), axis=1)
#print 'PROJ:', PROJ
#print lat_deg, lon_deg, altitude, ref[0], ref[1], ref[2]
#print ned
def createAnimationTrack(self, skel=None, name=None):
"""
Create an animation track from the motion stored in this BHV file.
"""
def _bvhJointName(boneName):
# Remove the tail from duplicate bone names (added by the BVH parser)
import re
if not boneName:
return boneName
r = re.search("(.*)_\d+$", boneName)
if r:
return r.group(1)
return boneName
def _createAnimation(jointsData, name, frameTime, nFrames):
nJoints = len(jointsData)
#nFrames = len(jointsData[0])
# Interweave joints animation data, per frame with joints in breadth-first order
animData = np.hstack(jointsData).reshape(nJoints*nFrames,3,4)
framerate = 1.0/frameTime
return animation.AnimationTrack(name, animData, nFrames, framerate)
if name is None:
name = self.name
if skel is None:
jointsData = [joint.matrixPoses for joint in self.getJoints() if not joint.isEndConnector()]
# We leave out end effectors as they should not have animation data
return _createAnimation(jointsData, name, self.frameTime, self.frameCount)
elif isinstance(skel, list):
# skel parameter is a list of joint or bone names
jointsOrder = skel
jointsData = []
for jointName in jointsOrder:
jointName = _bvhJointName(jointName)
if jointName and self.getJointByCanonicalName(jointName) is not None:
poseMats = self.getJointByCanonicalName(jointName).matrixPoses.copy()
jointsData.append(poseMats)
else:
jointsData.append(animation.emptyTrack(self.frameCount))
return _createAnimation(jointsData, name, self.frameTime, self.frameCount)
else:
# skel parameter is a Skeleton
jointsData = []
for bone in skel.getBones():
if len(bone.reference_bones) > 0:
# Combine the rotations of reference bones to influence this bone
bvhJoints = []
for bonename in bone.reference_bones:
jointname = _bvhJointName(bonename)
joint = self.getJointByCanonicalName(jointname)
if joint:
bvhJoints.append(joint)
if len(bvhJoints) == 0:
poseMats = animation.emptyTrack(self.frameCount)
elif len(bvhJoints) == 1:
poseMats = bvhJoints[0].matrixPoses.copy()
else: # len(bvhJoints) >= 2:
# Combine the rotations using quaternions to simplify math and normalizing (rotations only)
poseMats = animation.emptyTrack(self.frameCount)
m = np.identity(4, dtype=np.float32)
for f_idx in xrange(self.frameCount):
m[:3,:4] = bvhJoints[0].matrixPoses[f_idx]
q1 = tm.quaternion_from_matrix(m, True)
m[:3,:4] = bvhJoints[1].matrixPoses[f_idx]
q2 = tm.quaternion_from_matrix(m, True)
quat = tm.quaternion_multiply(q2, q1)
for bvhJoint in bvhJoints[2:]:
m[:3,:4] = bvhJoint.matrixPoses[f_idx]
q = tm.quaternion_from_matrix(m, True)
quat = tm.quaternion_multiply(q, quat)
poseMats[f_idx] = tm.quaternion_matrix( quat )[:3,:4]
jointsData.append(poseMats)
else:
# Map bone to joint by bone name
jointName = _bvhJointName(bone.name)
joint = self.getJointByCanonicalName(jointName)
if joint:
jointsData.append( joint.matrixPoses.copy() )
else:
jointsData.append(animation.emptyTrack(self.frameCount))
return _createAnimation(jointsData, name, self.frameTime, self.frameCount)
def FindVariance(quaternions,delta_t=0.1,motion_frequency=3,plot=False,filt_type='ellip',method='kevin',attitude='azelbore'):
'''
Purpose: Find the variance of a set of quaternions. Low Frequency components
are assumed to be invalid and will be discarded. Intended for analyzing
high-frequency variations in a set of rotation quaternions, specifically for
the DayStar platform.
Inputs: quaternions - List of quaternion arrays, formatted 'SXYZ' I think.
delta_t - (optional) Time between quaternion observations [s].
Assumed (and must be) uniform for all frames
motion_frequency - (optional) The Frequency we want to filter below
plot - (optional) Set to generate plots of the Fourier filterig as
we do it, to see how the signal changes
variable - (optional), the name of the signal being filtered. Used for
plot labeling.
{filt_type} - (optional) Specify filter type. Either:
* 'ellip' - try a scipy elliptical filter
* 'brick' - try a simple brick wall filter
Outputs: var - The computed variance of all observations. Meant to be some
indication of DayStar performance
**NOTE**
MOTION_FREQUENCY KEYWORD MUST BE GREATER THAN 1 FOR ELLIPTICAL FILTER.
It is an inverse ratio of the full spectrum that we wish to cut off. So a
'4' will cutoff (1/4) of the frequency spectrum.
'''
# Hey, do this later smarter
quats=[]
qtmp=quaternions[0]
for q in quaternions:
# Get rotation from epoch by multiplying each quaternion by the sum
# of all previous quaternions
qtmp=transform.quaternion_multiply(qtmp,q)
if method == 'kevin':
quats.append(np.array(np.hstack([qtmp[3],qtmp[0:3]])))
else:
quats.append(np.array(np.hstack([qtmp[0:3],qtmp[3]])))
if attitude == 'azelbore':
# 3D frame unit vectors
x = np.array([1, 0, 0])
y = np.array([0, 1, 0])
z = np.array([0, 0, 1])
Y = []
P = []
R = []
for q in quats:
# Find rotation matrix from quaternion:
M = quat2dcm(q)
# Rotate the x and y axes of the 3D frame
xhat = np.dot(M,x)
yhat = np.dot(M,y)
# Find azimuth and elevation:
az = np.arctan2(xhat[1], xhat[0])
el = np.arcsin(xhat[2])
# Find unrotated boresite y-axis:
yhatp = np.array([-np.sin(az), np.cos(az), 0])
# Find the boresight rotation:
phi = np.arccos(np.dot(yhat,yhatp))
if yhat[2] < 0:
phi = -phi
# Store data
Y.append(az)
P.append(-el)
R.append(phi)
y_filt = high_pass(Y, cutoff=motion_frequency, delta=delta_t, plot=plot,
variable='Yaw', filt_type=filt_type, color='blue') #radians
p_filt = high_pass(P, cutoff=motion_frequency, delta=delta_t, plot=plot,
variable='Pitch', filt_type=filt_type, color='purple') #radians
r_filt = high_pass(R, cutoff=motion_frequency, delta=delta_t, plot=plot,
variable='Roll', filt_type=filt_type, color='green') #radians
else:
[Y,P,R]=quat2ypr(quats,method=method)
y_filt = high_pass(Y,cutoff=motion_frequency,delta=delta_t,plot=plot,
variable='yaw',filt_type=filt_type,color='blue') #radians
p_filt = high_pass(P,cutoff=motion_frequency,delta=delta_t,plot=plot,
variable='pitch',filt_type=filt_type,color='purple') #radians
r_filt = high_pass(R,cutoff=motion_frequency,delta=delta_t,plot=plot,
variable='roll',filt_type=filt_type,color='green') #radians
# Project filtered results on 2d plane:
#project2d(y_filt,p_filt,r_filt)
project2dquat(quats)
project2d(y_filt,p_filt,r_filt)
#Standard deviation
#obs_std = np.sqrt(np.std(r_filt)**2 + np.std(p_filt)**2 + np.std(y_filt)**2)
y_std = 3600*(np.std(y_filt)*180/math.pi)
p_std = 3600*(np.std(p_filt)*180/math.pi)
r_std = 3600*(np.std(r_filt)*180/math.pi)
y_var = y_std**2
p_var = p_std**2
r_var = r_std**2
#var = 3600*(obs_std*180/math.pi)**2 # arcseconds
return y_var,p_var,r_var,y_filt,p_filt,r_filt
