def test_inverse_equivalence(self):
q = [5.77350000e-01, 5.77350000e-01, 5.77350000e-01, 6.12323400e-17]
result = matrix33.create_from_quaternion(quaternion.inverse(q))
expected = matrix33.inverse(matrix33.create_from_quaternion(q))
np.testing.assert_almost_equal(result, expected, decimal=5)
q = quaternion.create_from_x_rotation(0.5)
result = matrix33.create_from_inverse_of_quaternion(q)
expected = matrix33.inverse(matrix33.create_from_quaternion(q))
np.testing.assert_almost_equal(result, expected, decimal=5)
def test_inverse_equivalence(self):
q = [5.77350000e-01, 5.77350000e-01, 5.77350000e-01, 6.12323400e-17]
result = matrix33.create_from_quaternion(quaternion.inverse(q))
expected = matrix33.inverse(matrix33.create_from_quaternion(q))
np.testing.assert_almost_equal(result, expected, decimal=5)
q = quaternion.create_from_x_rotation(0.5)
result = matrix33.create_from_inverse_of_quaternion(q)
expected = matrix33.inverse(matrix33.create_from_quaternion(q))
np.testing.assert_almost_equal(result, expected, decimal=5)
def test_inverse_non_unit(self):
q = [1, 2, 3, 4]
result = quaternion.inverse(q)
expected = quaternion.conjugate(q) / quaternion.length(q)
np.testing.assert_almost_equal(result, expected, decimal=5)
def test_inverse_rotation(self):
result = quaternion.inverse(
[5.77350000e-01, 5.77350000e-01, 5.77350000e-01, 6.12323400e-17])
np.testing.assert_almost_equal(
result, [-0.577351, -0.577351, -0.577351, 6.12324e-17], decimal=5)
def test_inverse(self):
result = quaternion.inverse([0., 0., 0., 1.])
np.testing.assert_almost_equal(result, [0., 0., 0., 1.], decimal=5)
def test_inverse_non_unit(self):
q = [1, 2, 3, 4]
result = quaternion.inverse(q)
expected = quaternion.conjugate(q) / quaternion.length(q)
np.testing.assert_almost_equal(result, expected, decimal=5)
def test_inverse_rotation(self):
result = quaternion.inverse([5.77350000e-01, 5.77350000e-01, 5.77350000e-01, 6.12323400e-17])
np.testing.assert_almost_equal(result, [-0.577351, -0.577351, -0.577351, 6.12324e-17], decimal=5)
def test_inverse(self):
result = quaternion.inverse([0., 0., 0., 1.])
np.testing.assert_almost_equal(result, [0., 0., 0., 1.], decimal=5)
def test_inverse(self):
q = Quaternion.from_x_rotation(np.pi / 2.0)
self.assertTrue(np.allclose(q.inverse, quaternion.inverse(q)))
def test_create_from_inverse_quaternion(self):
q = Quaternion.from_x_rotation(0.5)
m = Matrix44.from_inverse_of_quaternion(q)
expected = matrix44.create_from_quaternion(
quaternion.inverse(quaternion.create_from_x_rotation(0.5)))
np.testing.assert_almost_equal(np.array(m), expected, decimal=5)
def test_create_from_inverse_quaternion(self):
q = Quaternion.from_x_rotation(0.5)
m = Matrix33.from_inverse_of_quaternion(q)
expected = matrix33.create_from_quaternion(quaternion.inverse(quaternion.create_from_x_rotation(0.5)))
np.testing.assert_almost_equal(np.array(m), expected, decimal=5)
def test_inverse(self):
q = Quaternion.from_x_rotation(np.pi / 2.0)
self.assertTrue(np.allclose(q.inverse, quaternion.inverse(q)))
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
