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_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): 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 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