def test_apply_to_vector_non_unit(self):
q = Quaternion.from_x_rotation(np.pi)
# zero length
v = Vector3([0., 0., 0.])
self.assertTrue(
np.allclose(
q * v,
quaternion.apply_to_vector(
quaternion.create_from_x_rotation(np.pi), [0., 0., 0.])))
# >1 length
v = Vector3([2., 0., 0.])
self.assertTrue(
np.allclose(
q * v,
quaternion.apply_to_vector(
quaternion.create_from_x_rotation(np.pi), [2., 0., 0.])))
v = Vector3([0., 2., 0.])
self.assertTrue(
np.allclose(
q * v,
quaternion.apply_to_vector(
quaternion.create_from_x_rotation(np.pi), [0., 2., 0.])))
v = Vector3([0., 0., 2.])
self.assertTrue(
np.allclose(
q * v,
quaternion.apply_to_vector(
quaternion.create_from_x_rotation(np.pi), [0., 0., 2.])))
def test_m44_q_equivalence(self):
"""Test for equivalance of matrix and quaternion rotations.
Create a matrix and quaternion, rotate each by the same values
then convert matrix<->quaternion and check the results are the same.
"""
m = matrix44.create_from_x_rotation(np.pi / 2.)
mq = quaternion.create_from_matrix(m)
q = quaternion.create_from_x_rotation(np.pi / 2.)
qm = matrix44.create_from_quaternion(q)
self.assertTrue(
np.allclose(np.dot([1., 0., 0., 1.], m), [1., 0., 0., 1.]))
self.assertTrue(
np.allclose(np.dot([1., 0., 0., 1.], qm), [1., 0., 0., 1.]))
self.assertTrue(
np.allclose(quaternion.apply_to_vector(q, [1., 0., 0., 1.]),
[1., 0., 0., 1.]))
self.assertTrue(
np.allclose(quaternion.apply_to_vector(mq, [1., 0., 0., 1.]),
[1., 0., 0., 1.]))
np.testing.assert_almost_equal(q, mq, decimal=5)
np.testing.assert_almost_equal(m, qm, decimal=5)
def test_apply_to_vector_non_unit(self):
q = quaternion.create_from_x_rotation(np.pi)
# zero length
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 0.]), [0., 0., 0.]))
# >1 length
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [2., 0., 0.]), [2., 0., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 2., 0.]), [0.,-2., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 2.]), [0., 0.,-2.]))
def test_apply_to_vector_non_unit(self):
q = quaternion.create_from_x_rotation(np.pi)
# zero length
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 0.]), [0., 0., 0.]))
# >1 length
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [2., 0., 0.]), [2., 0., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 2., 0.]), [0.,-2., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 2.]), [0., 0.,-2.]))
def test_apply_to_vector_z(self):
quat = quaternion.create_from_z_rotation(np.pi / 2.)
self.assertTrue(
np.allclose(quaternion.apply_to_vector(quat, [1., 0., 0.]),
[0., -1., 0.]))
self.assertTrue(
np.allclose(quaternion.apply_to_vector(quat, [0., 1., 0.]),
[1., 0., 0.]))
self.assertTrue(
np.allclose(quaternion.apply_to_vector(quat, [0., 0., 1.]),
[0., 0., 1.]))
def rotateAboutOrigin(self, xyz, t, p):
rp = p - math.pi / 2
rt = -t
q1 = quaternion.create_from_z_rotation(rp)
q2 = quaternion.create_from_x_rotation(rt)
v = vector3.create(xyz[0], xyz[1], xyz[2])
v = quaternion.apply_to_vector(q1, v)
v = quaternion.apply_to_vector(q2, v)
return v
def test_apply_to_vector_non_unit(self):
q = Quaternion.from_x_rotation(np.pi)
# zero length
v = Vector3([0., 0., 0.])
self.assertTrue(np.allclose(q * v, quaternion.apply_to_vector(quaternion.create_from_x_rotation(np.pi), [0., 0., 0.])))
# >1 length
v = Vector3([2., 0., 0.])
self.assertTrue(np.allclose(q * v, quaternion.apply_to_vector(quaternion.create_from_x_rotation(np.pi), [2., 0., 0.])))
v = Vector3([0., 2., 0.])
self.assertTrue(np.allclose(q * v, quaternion.apply_to_vector(quaternion.create_from_x_rotation(np.pi), [0., 2., 0.])))
v = Vector3([0., 0., 2.])
self.assertTrue(np.allclose(q * v, quaternion.apply_to_vector(quaternion.create_from_x_rotation(np.pi), [0., 0., 2.])))
def roll(self, value):
"""Rotate using the forward direction
"""
rotation = Quaternion.from_axis_rotation(self._forward,
value * self._speed)
self._up = normalize(quaternion.apply_to_vector(rotation, self._up))
return self
def yaw(self, value):
"""Rotate using up direction
"""
rotation = Quaternion.from_axis_rotation(self._up, value * self._speed)
self._forward = normalize(
quaternion.apply_to_vector(rotation, self._forward))
return self
def octasphere(ndivisions: int):
"""Creates a unit sphere using octagon subdivision.
Creates a unit sphere using octagon subdivision. Modified slightly from the original code
by Philip Rideout (https://prideout.net/blog/octasphere).
"""
n = 2**ndivisions + 1
num_verts = n * (n + 1) // 2
verts = empty((num_verts, 3))
j = 0
for i in range(n):
theta = pi * 0.5 * i / (n - 1)
point_a = [0, sin(theta), cos(theta)]
point_b = [cos(theta), sin(theta), 0]
num_segments = n - 1 - i
j = compute_geodesic(verts, j, point_a, point_b, num_segments)
assert len(verts) == num_verts
num_faces = (n - 2) * (n - 1) + n - 1
faces = empty((num_faces, 3), dtype='int')
f, j0 = 0, 0
for col_index in range(n - 1):
col_height = n - 1 - col_index
j1 = j0 + 1
j2 = j0 + col_height + 1
j3 = j0 + col_height + 2
for row in range(col_height - 1):
faces[f + 0] = [j0 + row, j1 + row, j2 + row]
faces[f + 1] = [j2 + row, j1 + row, j3 + row]
f = f + 2
row = col_height - 1
faces[f] = [j0 + row, j1 + row, j2 + row]
f = f + 1
j0 = j2
euler_angles = array([
[0, 0, 0],
[0, 1, 0],
[0, 2, 0],
[0, 3, 0],
[1, 0, 0],
[1, 0, 1],
[1, 0, 2],
[1, 0, 3],
]) * pi * 0.5
quats = (quaternion.create_from_eulers(e) for e in euler_angles)
offset, combined_verts, combined_faces = 0, [], []
for quat in quats:
rotated_verts = [quaternion.apply_to_vector(quat, v) for v in verts]
rotated_faces = faces + offset
combined_verts.append(rotated_verts)
combined_faces.append(rotated_faces)
offset = offset + len(verts)
return vstack(combined_verts), vstack(combined_faces)
def pitch(self, value):
""" Rotate using cross product between up and forward vectors (side).
"""
side = normalize(np.cross(self._up, self._forward))
rotation = Quaternion.from_axis_rotation(side, value * self._speed)
self._forward = normalize(
quaternion.apply_to_vector(rotation, self._forward))
self._up = normalize(np.cross(self._forward, side))
return self
def test_m44_q_equivalence(self):
"""Test for equivalance of matrix and quaternion rotations.
Create a matrix and quaternion, rotate each by the same values
then convert matrix<->quaternion and check the results are the same.
"""
m = matrix44.create_from_x_rotation(np.pi / 2.)
mq = quaternion.create_from_matrix(m)
q = quaternion.create_from_x_rotation(np.pi / 2.)
qm = matrix44.create_from_quaternion(q)
self.assertTrue(np.allclose(np.dot([1.,0.,0.,1.], m), [1.,0.,0.,1.]))
self.assertTrue(np.allclose(np.dot([1.,0.,0.,1.], qm), [1.,0.,0.,1.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [1.,0.,0.,1.]), [1.,0.,0.,1.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(mq, [1.,0.,0.,1.]), [1.,0.,0.,1.]))
np.testing.assert_almost_equal(q, mq, decimal=5)
np.testing.assert_almost_equal(m, qm, decimal=5)
def test_apply_to_vector_z(self):
# 180 degree turn around Z axis
q = quaternion.create_from_z_rotation(np.pi)
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [1., 0., 0.]), [-1., 0., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 1., 0.]), [0.,-1., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 1.]), [0., 0., 1.]))
# 90 degree rotation around Z axis
q = quaternion.create_from_z_rotation(np.pi / 2.)
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [1., 0., 0.]), [0., 1., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 1., 0.]), [-1., 0., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 1.]), [0., 0., 1.]))
# -90 degree rotation around Z axis
q = quaternion.create_from_z_rotation(-np.pi / 2.)
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [1., 0., 0.]), [0.,-1., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 1., 0.]), [1., 0., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 1.]), [0., 0., 1.]))
def test_apply_to_vector_z(self):
# 180 degree turn around Z axis
q = quaternion.create_from_z_rotation(np.pi)
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [1., 0., 0.]), [-1., 0., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 1., 0.]), [0.,-1., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 1.]), [0., 0., 1.]))
# 90 degree rotation around Z axis
q = quaternion.create_from_z_rotation(np.pi / 2.)
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [1., 0., 0.]), [0., 1., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 1., 0.]), [-1., 0., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 1.]), [0., 0., 1.]))
# -90 degree rotation around Z axis
q = quaternion.create_from_z_rotation(-np.pi / 2.)
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [1., 0., 0.]), [0.,-1., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 1., 0.]), [1., 0., 0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [0., 0., 1.]), [0., 0., 1.]))
def test_operators_vector4(self):
q = Quaternion.from_x_rotation(0.5)
v = Vector4([1.,0.,0.,1.])
# add
self.assertRaises(ValueError, lambda: q + v)
# subtract
self.assertRaises(ValueError, lambda: q - v)
# multiply
self.assertTrue(np.array_equal(q * v, quaternion.apply_to_vector(quaternion.create_from_x_rotation(0.5), [1.,0.,0.,1.])))
# divide
self.assertRaises(ValueError, lambda: q / v)
def orient(self, x, y):
""" Orient the current camera with the current x, y
The function will use two quaternions for computing the final rotation. By using
the current up and side vectors of the camera. The angle used for the rotations
are x and y passed by parameters. The function will normalize the new axis vectors
for the camera by the current rotation.
Remark: there is no translation like in Orbit functionality.
"""
side = normalize(np.cross(self._up, self._forward))
rotationYaw = Quaternion.from_axis_rotation(
self._up, -x * self._speed * self._sensitivity)
rotationPitch = Quaternion.from_axis_rotation(
side, y * self._speed * self._sensitivity)
rotation = rotationYaw * rotationPitch
self._forward = normalize(
quaternion.apply_to_vector(rotation, self._forward))
return self
def compute_geodesic(dst, index, point_a, point_b, num_segments):
"""Given two points on a unit sphere, returns a sequence of surface
points that lie between them along a geodesic curve."""
angle_between_endpoints = acos(dot(point_a, point_b))
rotation_axis = cross(point_a, point_b)
dst[index] = point_a
index = index + 1
if num_segments == 0:
return index
dtheta = angle_between_endpoints / num_segments
for point_index in range(1, num_segments):
theta = point_index * dtheta
q = quaternion.create_from_axis_rotation(rotation_axis, theta)
dst[index] = quaternion.apply_to_vector(q, point_a)
index = index + 1
dst[index] = point_b
return index + 1
def test_apply_to_vector_unit_x(self):
result = quaternion.apply_to_vector([0., 0., 0., 1.], [1., 0., 0.])
np.testing.assert_almost_equal(result, [1., 0., 0.], decimal=5)
def load(self, md5anim, frame):
# we don't need to upload the parents values
# so just leave as 'int'
# the parent can be negative (root is -1), so it must be signed
self.parents = numpy.empty(md5anim.hierarchy.num_joints, dtype='int')
self.positions = numpy.empty((md5anim.hierarchy.num_joints, 3),
dtype='float32')
self.orientations = numpy.empty((md5anim.hierarchy.num_joints, 4),
dtype='float32')
for index, (hierarchy_joint, base_frame_joint) in enumerate(
zip(md5anim.hierarchy, md5anim.base_frame)):
# set the parent now as we can't set it in the named tuple
self.parents[index] = hierarchy_joint.parent
# get the current joint
joint = self.joint(index)
# begin with the original base frame values
joint.position[:] = base_frame_joint.position
joint.orientation[:] = base_frame_joint.orientation
# overlay with values from our frame
# we know which values to get from the joint's start_index
# and the joint's flag
frame_index = hierarchy_joint.start_index
if hierarchy_joint.flags & 1:
joint.position[0] = frame.value(frame_index)
frame_index += 1
if hierarchy_joint.flags & 2:
joint.position[1] = frame.value(frame_index)
frame_index += 1
if hierarchy_joint.flags & 4:
joint.position[2] = frame.value(frame_index)
frame_index += 1
if hierarchy_joint.flags & 8:
joint.orientation[0] = frame.value(frame_index)
frame_index += 1
if hierarchy_joint.flags & 16:
joint.orientation[1] = frame.value(frame_index)
frame_index += 1
if hierarchy_joint.flags & 32:
joint.orientation[2] = frame.value(frame_index)
frame_index += 1
# compute the W component of the quaternion
joint.orientation[3] = compute_quaternion_w(
joint.orientation[0], joint.orientation[1],
joint.orientation[2])
# parents should always be an bone we've
# previously calculated
assert joint.parent < index
# check if the joint has a parent
if joint.parent >= 0:
# get the parent joint
parent = self.joint(joint.parent)
# make this joint relative to the parent
# rotate our position by our parents
rotated_position = quaternion.apply_to_vector(
parent.orientation, joint.position)
# add our parent's position
joint.position[:] = parent.position + rotated_position
# multiply our orientation by our parent's
rotated_orientation = quaternion.cross(parent.orientation,
joint.orientation)
# normalise our orientation
joint.orientation[:] = quaternion.normalise(
rotated_orientation)
def test_apply_to_vector_z(self):
quat = quaternion.create_from_z_rotation(np.pi / 2.)
self.assertTrue(np.allclose(quaternion.apply_to_vector(quat,[1.,0.,0.]), [0.,-1.,0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(quat,[0.,1.,0.]), [1.,0.,0.]))
self.assertTrue(np.allclose(quaternion.apply_to_vector(quat,[0.,0.,1.]), [0.,0.,1.]))
def test_apply_to_vector_unit_x(self):
result = quaternion.apply_to_vector([0., 0., 0., 1.], [1., 0., 0.])
np.testing.assert_almost_equal(result, [1., 0., 0.], decimal=5)
def compute_angles(transforms, rotations):
angles = []
#########################################
### Elbows - simple angle calculation ###
#########################################
elbow_angles = []
for i in range(2): # Iterate over joint chains - get right and left
# Create vectors from transforms
vector_shoulder_elbow = np.array(
[transforms[i][2][0], transforms[i][2][1], transforms[i][2][2]])
vector_elbow_wrist = np.array(
[transforms[i][3][0], transforms[i][3][1], transforms[i][3][2]])
# Rotate vector_elbow_wrist to match vector_shoulder_elbow coordinate system
inverse_quaternion = quaternion.inverse(rotations[i][2])
vector_elbow_wrist = quaternion.apply_to_vector(
inverse_quaternion, vector_elbow_wrist)
# Angle between 2 vectors
angle = np.arccos(
np.dot(vector_shoulder_elbow, vector_elbow_wrist) /
(np.linalg.norm(vector_shoulder_elbow) *
np.linalg.norm(vector_elbow_wrist)))
# Limit angle
if angle > 3 * math.pi / 4:
angle = 3 * math.pi / 4
elif angle < 0:
angle = 0
elbow_angles.append(angle)
###########################################################################################
### Shoulders, 2 angles per shoulder - projecting upper arm on two perpendicular planes ###
###########################################################################################
shoulder_angles = []
for i in range(2): # Iterate over joint chains - get right and left
# Create vectors from transformations
vector_hip_neck = np.array(
[transforms[i][0][0], transforms[i][0][1], transforms[i][0][2]])
vector_neck_shoulder = np.array(
[transforms[i][1][0], transforms[i][1][1], transforms[i][1][2]])
vector_shoulder_elbow = np.array(
[transforms[i][2][0], transforms[i][2][1], transforms[i][2][2]])
# Rotate vector_shoulder_elbow to match vector_neck_shoulder coordinate system
inverse_quaternion = quaternion.inverse(rotations[i][1])
vector_shoulder_elbow = quaternion.apply_to_vector(
inverse_quaternion, vector_shoulder_elbow)
# Rotate vector_hip_neck to match vector_neck_shoulder coordinate system
vector_hip_neck = quaternion.apply_to_vector(rotations[i][0],
vector_hip_neck)
# Calculate normal vector for plane set by vector_hip_neck and vector_neck_shoulder
vector_normal_forward = np.cross(vector_hip_neck, vector_neck_shoulder)
# Initialize second normal vector
vector_normal_side = vector_neck_shoulder
# Project vector_shoulder_elbow to plane set by vector_normal_forward
vector_shoulder_elbow_parallel = vector_shoulder_elbow - (
(np.dot(vector_shoulder_elbow, vector_normal_forward) /
math.pow(np.linalg.norm(vector_normal_forward), 2)) *
vector_normal_forward)
# Project vector_shoulder_elbow to plane set by vector_normal_side
vector_shoulder_elbow_perpendicular = vector_shoulder_elbow - (
(np.dot(vector_shoulder_elbow, vector_normal_side) / math.pow(
np.linalg.norm(vector_normal_side), 2)) * vector_normal_side)
if i == 1:
vector_shoulder_elbow_perpendicular = -vector_shoulder_elbow_perpendicular
# Angles between 2 vectors
angle_parallel = np.arccos(
np.dot(vector_normal_side, vector_shoulder_elbow_parallel) /
(np.linalg.norm(vector_normal_side) *
np.linalg.norm(vector_shoulder_elbow_parallel)))
angle_perpendicular = np.arccos(
np.dot(vector_normal_forward, vector_shoulder_elbow_perpendicular)
/ (np.linalg.norm(vector_normal_forward) *
np.linalg.norm(vector_shoulder_elbow_perpendicular)))
# Fix ambiquity of results below and above zero-level
if vector_shoulder_elbow[1] > 0:
angle_parallel += math.pi / 2
angle_perpendicular = math.fabs(angle_perpendicular +
(math.pi / 2))
# Set angles to match the simulation
else:
angle_parallel = math.fabs(angle_parallel - (math.pi / 2))
angle_perpendicular = math.fabs(angle_perpendicular -
(math.pi / 2))
# In simulation, right goes down from zero while left goes up from zero
if i == 0:
angle_parallel = -angle_parallel
# Set angle_parallel to zero if hand is above parallel
if vector_shoulder_elbow[1] > 0:
if vector_shoulder_elbow[0] > 0 and i == 0:
angle_parallel = 0
elif vector_shoulder_elbow[0] < 0 and i == 1:
angle_parallel = 0
# Limit parallel angle
if angle_parallel > 0.75:
angle_parallel = 0.75
elif angle_parallel < -0.75:
angle_parallel = -0.75
# Set perpendicular angle to zero if it's unexpectedly high or low - usually if elbows are farther from Kinect than shoulders
if angle_perpendicular > math.pi + math.pi / 4:
angle_perpendicular = 0
elif angle_perpendicular < -math.pi / 4:
angle_perpendicular = 0
# Limit perpendicular angle
elif angle_perpendicular > math.pi:
angle_perpendicular = math.pi
elif angle_perpendicular < 0:
angle_perpendicular = 0
shoulder_angles.append(angle_parallel)
shoulder_angles.append(angle_perpendicular)
######################
### Bicep rotation ###
######################
bicep_angles = []
for i in range(2): # Iterate over joint chains - get right and left
# Create vectors from transformations
vector_shoulder_elbow = np.array(
[transforms[i][2][0], transforms[i][2][1], transforms[i][2][2]])
vector_elbow_hand = np.array(
[transforms[i][3][0], transforms[i][3][1], transforms[i][3][2]])
# Rotate vector_shoulder_elbow to match vector_neck_shoulder coordinate system
inverse_quaternion = quaternion.inverse(rotations[i][1])
vector_shoulder_elbow = quaternion.apply_to_vector(
inverse_quaternion, vector_shoulder_elbow)
# Calculate a unit vector from vector_elbow_hand
vector_elbow_hand_lenght = np.linalg.norm(vector_elbow_hand)
vector_elbow_hand_unit = vector_elbow_hand / vector_elbow_hand_lenght
# Calculate bicep rotation angle
angle = 2 * math.sin(vector_elbow_hand_unit[1])
if vector_shoulder_elbow[1] > 0 and i == 1:
angle = angle - math.pi
elif vector_shoulder_elbow[1] > 0 and i == 0:
angle = -angle + math.pi
elif vector_shoulder_elbow[1] < 0 and i == 0:
angle = -angle
bicep_angles.append(angle)
#################
### Head tilt ###
#################
head_angles = []
# Create vectors from transformations
vector_neck_head = np.array(
[transforms[0][5][0], transforms[0][5][1], transforms[0][5][2]])
# Calculate a unit vector from vector_neck_head
vector_neck_head_lenght = np.linalg.norm(vector_neck_head)
vector_neck_head_unit = vector_neck_head / vector_neck_head_lenght
# Calculate head rotation angle
angle = math.sin(vector_neck_head_unit[0])
# Limit angle
if angle > 0.3:
angle = 0.3
elif angle < -0.3:
angle = -0.3
head_angles.append(angle)
##################
### Waist lean ###
##################s
waist_angles = []
# Create vector up from shoulder line
vector_normal_up = np.cross(vector_normal_side, vector_normal_forward)
# Calculate a unit vector from vector_normal_up
vector_normal_up_lenght = np.linalg.norm(vector_normal_up)
vector_normal_up_unit = vector_normal_up / vector_normal_up_lenght
# Calculate waist lean angle
angle = math.sin(vector_neck_head_unit[0])
# Limit angle
if angle > 0.3:
angle = 0.3
elif angle < -0.3:
angle = -0.3
waist_angles.append(angle)
# Test print
outputStr = '{}\n'.format(vector_normal_up)
rospy.loginfo(outputStr)
angles.append(elbow_angles)
angles.append(shoulder_angles)
angles.append(bicep_angles)
angles.append(head_angles)
angles.append(waist_angles)
return angles