ANGLES

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)

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
 

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"],
-)

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__)

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

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

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"],
+)

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)