From 0e357d71c03f158ed94beebf39876d562be67671 Mon Sep 17 00:00:00 2001 From: Nathan DuPont Date: Mon, 15 Apr 2024 01:09:12 -0700 Subject: [PATCH] ANGLES --- src/api/v1/api.py | 4 +- src/api/v1/robot.py | 23 +++- src/robot/v1/BUILD.bazel | 14 --- src/robot/v1/robot.py | 1 - src/robot/v1/structs.py | 29 ----- src/robot/v1/structs_test.py | 112 ----------------- src/util/BUILD.bazel | 15 +++ src/util/linalg.py | 61 ++++++++++ src/util/linalg_test.py | 229 +++++++++++++++++++++++++++++++++++ 9 files changed, 327 insertions(+), 161 deletions(-) create mode 100644 src/util/BUILD.bazel create mode 100644 src/util/linalg.py create mode 100644 src/util/linalg_test.py diff --git a/src/api/v1/api.py b/src/api/v1/api.py index b679c16..2e0e9d2 100644 --- a/src/api/v1/api.py +++ b/src/api/v1/api.py @@ -29,7 +29,9 @@ motors = [ DynamixelMotor(10, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0)), - DynamixelMotor(11, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0), True), + DynamixelMotor(11, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0)), + DynamixelMotor(12, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0), True), + DynamixelMotor(13, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0), True), ] ctrl = RobotControl("/dev/ttyUSB0", 1, motors) ctrl.init(1000000) diff --git a/src/api/v1/robot.py b/src/api/v1/robot.py index d83e1f5..4561830 100644 --- a/src/api/v1/robot.py +++ b/src/api/v1/robot.py @@ -391,12 +391,27 @@ def set_velocity(self, command: Position2D) -> bool: math.pi / 2 ) - for motor in self.motors: + # rear left + motor_0_speed = min(max(clamped_y - clamped_x - command.rotation.theta, -1), 1) + + # front left + motor_1_speed = min(max(clamped_y + clamped_x - command.rotation.theta, -1), 1) + + # front right + motor_2_speed = min(max(clamped_y - clamped_x - command.rotation.theta, -1), 1) + + # rear right + motor_3_speed = min(max(clamped_y + clamped_x - command.rotation.theta, -1), 1) + + speeds = [motor_0_speed, motor_1_speed, motor_2_speed, motor_3_speed] + + for i, motor in enumerate(self.motors): + speed = speeds[i] # TODO actual kinematics here - if clamped_y < 0: - motor.set_moving_speed((abs(clamped_y) * 1023) + 1023) + if speed < 0: + motor.set_moving_speed((abs(speed) * 1023) + 1023) else: - motor.set_moving_speed(clamped_y * 1023) + motor.set_moving_speed(speed * 1023) return True diff --git a/src/robot/v1/BUILD.bazel b/src/robot/v1/BUILD.bazel index 8cba23d..c1429aa 100644 --- a/src/robot/v1/BUILD.bazel +++ b/src/robot/v1/BUILD.bazel @@ -16,17 +16,3 @@ py_library( "@pypi//websockets:pkg", ], ) - -py_library( - name = "structs_src", - srcs = ["structs.py"], - deps = [ - "@pypi//numpy:pkg", - ], -) - -py_test( - name = "structs_test", - srcs = ["structs_test.py"], - deps = [":structs_src"], -) diff --git a/src/robot/v1/robot.py b/src/robot/v1/robot.py index d121d2d..81ef8c5 100644 --- a/src/robot/v1/robot.py +++ b/src/robot/v1/robot.py @@ -2,7 +2,6 @@ import math from typing import Optional import dynamixel_sdk as dynamixel -from dataclasses import dataclass import logging logger = logging.getLogger(__file__) diff --git a/src/robot/v1/structs.py b/src/robot/v1/structs.py index 7a1d922..056bd92 100644 --- a/src/robot/v1/structs.py +++ b/src/robot/v1/structs.py @@ -1,33 +1,4 @@ from dataclasses import dataclass -from typing import Union -import numpy as np - - -def vector2d(x: Union[int, float], y: Union[int, float]): - """ - Returns a 2D vector as a numpy array. - - Params: - x [Union[int, float]]: X component of the vector - y [Union[int, float]]: Y component of the vector - """ - return np.array([x, y]) - - -def rotation2d(theta: Union[int, float]): - """ - Return a 2D rotation matrix as a numpy array, provided an angle in radians. - From https://scipython.com/book/chapter-6-numpy/examples/creating-a-rotation-matrix-in-numpy/ - - Params: - theta [Union[int, float]]: Angle in radians - """ - c, s = np.cos(theta), np.sin(theta) - return np.array(((c, -s), (s, c))) - - -# TODO: Add some smallest angle function. Needs to keep sign, -# can likely be done with modulus @dataclass diff --git a/src/robot/v1/structs_test.py b/src/robot/v1/structs_test.py index 5a8128a..b047b3b 100644 --- a/src/robot/v1/structs_test.py +++ b/src/robot/v1/structs_test.py @@ -1,115 +1,3 @@ -import unittest -import math -import numpy as np -from src.robot.v1.structs import vector2d, rotation2d - - -class TestVector2D(unittest.TestCase): - def test_add(self): - total = vector2d(1, 2) + vector2d(3, 4) - self.assertEqual(total[0], 4) - self.assertEqual(total[1], 6) - - def test_iadd(self): - vec_a = vector2d(1, 2) - vec_a += vector2d(5, 5) - self.assertEqual(vec_a[0], 6) - self.assertEqual(vec_a[1], 7) - - def test_sub(self): - total = vector2d(5, 5) - vector2d(3, 4) - self.assertEqual(total[0], 2) - self.assertEqual(total[1], 1) - - def test_isub(self): - vec_a = vector2d(5, 5) - vec_a -= vector2d(1, 2) - self.assertEqual(vec_a[0], 4) - self.assertEqual(vec_a[1], 3) - - def test_45deg_rotation(self): - angle = math.pi / 4 - starting_vec = vector2d(1, 2) - - # Figure out the expected angles programmatically - r = math.sqrt(5) - expected_angle_cw = math.atan2(starting_vec[1], starting_vec[0]) - (math.pi / 4) - expected_vec_cw = vector2d( - r * math.cos(expected_angle_cw), r * math.sin(expected_angle_cw) - ) - - expected_angle_ccw = math.atan2(starting_vec[1], starting_vec[0]) + ( - math.pi / 4 - ) - expected_vec_ccw = vector2d( - r * math.cos(expected_angle_ccw), r * math.sin(expected_angle_ccw) - ) - - for theta, expected in [(angle, expected_vec_cw), (-angle, expected_vec_ccw)]: - rotation = rotation2d(theta) - - rotated = starting_vec.dot(rotation) - self.assertEqual(rotated.shape, (2,)) - self.assertAlmostEqual(rotated[0], expected[0]) - self.assertAlmostEqual(rotated[1], expected[1]) - - def test_90deg_rotation(self): - angle = math.pi / 2 - starting_vec = vector2d(1, 2) - expected_vec = vector2d(2, -1) - - for theta, expected in [(angle, expected_vec), (-angle, -expected_vec)]: - rotation = rotation2d(theta) - - rotated = starting_vec.dot(rotation) - self.assertEqual(rotated.shape, (2,)) - self.assertAlmostEqual(rotated[0], expected[0]) - self.assertAlmostEqual(rotated[1], expected[1]) - - def test_180deg_rotation(self): - angle = math.pi - starting_vec = vector2d(1, 2) - expected_vec = vector2d(-1, -2) - - for theta, expected in [(angle, expected_vec), (-angle, expected_vec)]: - rotation = rotation2d(theta) - - rotated = starting_vec.dot(rotation) - self.assertEqual(rotated.shape, (2,)) - self.assertAlmostEqual(rotated[0], expected[0]) - self.assertAlmostEqual(rotated[1], expected[1]) - - def test_270deg_rotation(self): - angle = (3 * math.pi) / 2 - starting_vec = vector2d(1, 2) - expected_vec = vector2d(-2, 1) - - for theta, expected in [(angle, expected_vec), (-angle, -expected_vec)]: - rotation = rotation2d(theta) - - rotated = starting_vec.dot(rotation) - self.assertEqual(rotated.shape, (2,)) - self.assertAlmostEqual(rotated[0], expected[0]) - self.assertAlmostEqual(rotated[1], expected[1]) - - def test_360deg_rotation(self): - angle = 2 * math.pi - starting_vec = vector2d(1, 2) - expected_vec = vector2d(1, 2) - - for theta, expected in [(angle, expected_vec), (-angle, expected_vec)]: - rotation = rotation2d(theta) - - rotated = starting_vec.dot(rotation) - self.assertEqual(rotated.shape, (2,)) - self.assertAlmostEqual(rotated[0], expected[0]) - self.assertAlmostEqual(rotated[1], expected[1]) - - -if __name__ == "__main__": - unittest.main() - - # @dataclass # class Vector2D: # x: float diff --git a/src/util/BUILD.bazel b/src/util/BUILD.bazel new file mode 100644 index 0000000..af408fe --- /dev/null +++ b/src/util/BUILD.bazel @@ -0,0 +1,15 @@ +load("@rules_python//python:defs.bzl", "py_library", "py_test") + +py_library( + name = "linalg_src", + srcs = ["linalg.py"], + deps = [ + "@pypi//numpy:pkg", + ], +) + +py_test( + name = "linalg_test", + srcs = ["linalg_test.py"], + deps = [":linalg_src"], +) diff --git a/src/util/linalg.py b/src/util/linalg.py new file mode 100644 index 0000000..7082389 --- /dev/null +++ b/src/util/linalg.py @@ -0,0 +1,61 @@ +from typing import Union +import numpy as np +import math + +M_PI = math.pi +M_PI_2 = M_PI / 2 +M_PI_3 = M_PI / 3 +M_PI_4 = M_PI / 4 + + +def vector2d(x: Union[int, float], y: Union[int, float]): + """ + Returns a 2D vector as a numpy array. + + Params: + x [Union[int, float]]: X component of the vector + y [Union[int, float]]: Y component of the vector + """ + return np.array([x, y]) + + +def rotation2d(theta: Union[int, float]): + """ + Return a 2D rotation matrix as a numpy array, provided an angle in radians. + From https://scipython.com/book/chapter-6-numpy/examples/creating-a-rotation-matrix-in-numpy/ + + Positive rotations move vectors clockwise, whereas negative rotations move + vectors counterclockwise + + Params: + theta [Union[int, float]]: Angle in radians + """ + c, s = np.cos(theta), np.sin(theta) + return np.array(((c, -s), (s, c))) + + +def smallest_angle(theta: Union[int, float]): + """ + Given an angle in radians, returns the smallest rotation for the angle, + between -PI and PI + + Params: + theta [Union[int, float]]: Angle in radians + """ + # Reduce the angle down to 0-PI + reduced_theta = abs(theta) % (M_PI) + + # If the value is between pi and 2pi (modulus 2pi to not count multiple + # rotations), we need to use 360 minus the angle's value + opposite_hemisphere = (abs(theta) // M_PI) % 2 != 0 + + if opposite_hemisphere: + # If the shortest angle is in the opposite hemisphere, we reverse + # the angle and take the opposite of the angle between 0-180. + # Ex. For 210deg, the shortest angle is actually -150deg, whereas + # if directly flipped, it would be reported as -30deg + return math.copysign(M_PI - reduced_theta, -theta) + else: + # If we are in the same hemisphere, we can just remove any extra + # rotations and keep the same sign. + return math.copysign(reduced_theta, theta) diff --git a/src/util/linalg_test.py b/src/util/linalg_test.py new file mode 100644 index 0000000..5ce87ce --- /dev/null +++ b/src/util/linalg_test.py @@ -0,0 +1,229 @@ +import unittest +import math +from src.util.linalg import ( + vector2d, + rotation2d, + smallest_angle, + M_PI, + M_PI_2, + M_PI_3, + M_PI_4, +) + + +class TestVector2D(unittest.TestCase): + def test_add(self): + total = vector2d(1, 2) + vector2d(3, 4) + self.assertEqual(total[0], 4) + self.assertEqual(total[1], 6) + + def test_iadd(self): + vec_a = vector2d(1, 2) + vec_a += vector2d(5, 5) + self.assertEqual(vec_a[0], 6) + self.assertEqual(vec_a[1], 7) + + def test_sub(self): + total = vector2d(5, 5) - vector2d(3, 4) + self.assertEqual(total[0], 2) + self.assertEqual(total[1], 1) + + def test_isub(self): + vec_a = vector2d(5, 5) + vec_a -= vector2d(1, 2) + self.assertEqual(vec_a[0], 4) + self.assertEqual(vec_a[1], 3) + + def test_scalar_multiply(self): + total = vector2d(1, 2) * 3 + self.assertEqual(total[0], 3) + self.assertEqual(total[1], 6) + + def test_scalar_divide(self): + total = vector2d(8, 4) / 2 + self.assertEqual(total[0], 4) + self.assertEqual(total[1], 2) + + def test_45deg_rotation(self): + angle = M_PI_4 + starting_vec = vector2d(1, 2) + + # Figure out the expected angles programmatically + r = math.sqrt(5) + expected_angle_cw = math.atan2(starting_vec[1], starting_vec[0]) - M_PI_4 + expected_vec_cw = vector2d( + r * math.cos(expected_angle_cw), r * math.sin(expected_angle_cw) + ) + + expected_angle_ccw = math.atan2(starting_vec[1], starting_vec[0]) + M_PI_4 + expected_vec_ccw = vector2d( + r * math.cos(expected_angle_ccw), r * math.sin(expected_angle_ccw) + ) + + for theta, expected in [(angle, expected_vec_cw), (-angle, expected_vec_ccw)]: + rotation = rotation2d(theta) + + rotated = starting_vec.dot(rotation) + self.assertEqual(rotated.shape, (2,)) + self.assertAlmostEqual(rotated[0], expected[0]) + self.assertAlmostEqual(rotated[1], expected[1]) + + def test_90deg_rotation(self): + angle = M_PI_2 + starting_vec = vector2d(1, 2) + expected_vec = vector2d(2, -1) + + for theta, expected in [(angle, expected_vec), (-angle, -expected_vec)]: + rotation = rotation2d(theta) + + rotated = starting_vec.dot(rotation) + self.assertEqual(rotated.shape, (2,)) + self.assertAlmostEqual(rotated[0], expected[0]) + self.assertAlmostEqual(rotated[1], expected[1]) + + def test_180deg_rotation(self): + angle = M_PI + starting_vec = vector2d(1, 2) + expected_vec = vector2d(-1, -2) + + for theta, expected in [(angle, expected_vec), (-angle, expected_vec)]: + rotation = rotation2d(theta) + + rotated = starting_vec.dot(rotation) + self.assertEqual(rotated.shape, (2,)) + self.assertAlmostEqual(rotated[0], expected[0]) + self.assertAlmostEqual(rotated[1], expected[1]) + + def test_270deg_rotation(self): + angle = 3 * M_PI_2 + starting_vec = vector2d(1, 2) + expected_vec = vector2d(-2, 1) + + for theta, expected in [(angle, expected_vec), (-angle, -expected_vec)]: + rotation = rotation2d(theta) + + rotated = starting_vec.dot(rotation) + self.assertEqual(rotated.shape, (2,)) + self.assertAlmostEqual(rotated[0], expected[0]) + self.assertAlmostEqual(rotated[1], expected[1]) + + def test_360deg_rotation(self): + angle = 2 * M_PI + starting_vec = vector2d(1, 2) + expected_vec = vector2d(1, 2) + + for theta, expected in [(angle, expected_vec), (-angle, expected_vec)]: + rotation = rotation2d(theta) + + rotated = starting_vec.dot(rotation) + self.assertEqual(rotated.shape, (2,)) + self.assertAlmostEqual(rotated[0], expected[0]) + self.assertAlmostEqual(rotated[1], expected[1]) + + +class TestRotation2D(unittest.TestCase): + def test_add_90deg_90deg(self): + """ + Test that adding two 90 degree rotations adds to a + 180 degree rotation. + """ + rot_a = rotation2d(M_PI_2) + rot_b = rotation2d(M_PI_2) + expected_total = rotation2d(M_PI) + + total = rot_a.dot(rot_b) + self.assertTrue((total == expected_total).all()) + + def test_add_neg90deg_neg90deg(self): + """ + Test that adding two -90 degree rotations adds to a + -180 degree rotation. + """ + rot_a = rotation2d(-M_PI_2) + rot_b = rotation2d(-M_PI_2) + expected_total = rotation2d(-M_PI) + + total = rot_a.dot(rot_b) + self.assertTrue((total == expected_total).all()) + + def test_add_90deg_180deg(self): + """ + Test that adding a 90 and 180 degree rotation adds to + a 270 degree rotation + """ + rot_a = rotation2d(M_PI_2) + rot_b = rotation2d(M_PI) + expected_total = rotation2d(3 * M_PI_2) + + total = rot_a.dot(rot_b) + self.assertTrue((total == expected_total).all()) + + +class TestSmallestAngle(unittest.TestCase): + def test_small_positive_angle(self): + """ + Test a small, positive angle (between 0 and PI) to confirm the + same angle gets returned + """ + angle = smallest_angle(M_PI_4) + self.assertAlmostEqual(angle, M_PI_4) + + def test_small_negative_angle(self): + """ + Test a small, negative angle (between 0 and -PI) to confirm the + same angle gets returned + """ + angle = smallest_angle(-M_PI_4) + self.assertAlmostEqual(angle, -M_PI_4) + + def test_mid_positive_angle(self): + """ + Test a medium, positive angle (between PI and 2PI) to confirm that + the smaller negative angle is returned + """ + angle = smallest_angle(5 * M_PI_3) + self.assertAlmostEqual(angle, -M_PI_3) + + def test_mid_negative_angle(self): + """ + Test a medium, negative angle (between -PI and -2PI) to confirm that + the smaller positive angle is returned + """ + angle = smallest_angle(-5 * M_PI_3) + self.assertAlmostEqual(angle, M_PI_3) + + def test_large_positive_angle(self): + """ + Test a large, positive angle (between 2PI and 3PI) to confirm + that the same positive angle is returned + """ + angle = smallest_angle(5 * M_PI_2) + self.assertAlmostEqual(angle, M_PI_2) + + def test_large_negative_angle(self): + """ + Test a large, negative angle (between -2PI and -3PI) to confirm + that the same negative angle is returned + """ + angle = smallest_angle(-5 * M_PI_2) + self.assertAlmostEqual(angle, -M_PI_2) + + def test_multi_rollover_positive_angle(self): + """ + Test a large, positive angle (0 and PI, + n*2PI) to confirm + that the reduced positive angle is returned + """ + angle = smallest_angle(M_PI_3 + (14 * 2 * M_PI)) + self.assertAlmostEqual(angle, M_PI_3) + + def test_multi_rollover_negative_angle(self): + """ + Test a large, negative angle (0 and -PI, + n*-2PI) to confirm + that the reduced negative angle is returned + """ + angle = smallest_angle(-M_PI_3 + (14 * -2 * M_PI)) + self.assertAlmostEqual(angle, -M_PI_3) + + +if __name__ == "__main__": + unittest.main()