From 61c1f87f00163e9e2e90469bd62be14f170dd806 Mon Sep 17 00:00:00 2001 From: liamgilligan Date: Sun, 24 Dec 2023 12:02:05 -0800 Subject: [PATCH] Updated Motor Configuration in Kinematics to encode torque, not position relative to CoM --- src/seahawk/seahawk_deck/thrust.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/seahawk/seahawk_deck/thrust.py b/src/seahawk/seahawk_deck/thrust.py index 81d95d9d..faf236fe 100644 --- a/src/seahawk/seahawk_deck/thrust.py +++ b/src/seahawk/seahawk_deck/thrust.py @@ -43,13 +43,14 @@ def __init__(self): super().__init__('thrust') self.motor_config = [ - [ 0, 0, 0, 0, 0.7071, 0.7071, -0.7071, -0.7071 ], # Fx - [ 0, 0, 0, 0, -0.7071, 0.7071, -0.7071, 0.7071 ], # Fy - [ 1, 1, 1, 1, 0, 0, 0, 0 ], # Fz - [ -0.19, -0.19, 0.19, 0.19, -0.105, -0.105, 0.105, 0.105 ], # Rx (mm) - [ -0.12, 0.12, -0.12, 0.12, -0.15, 0.15, -0.15, 0.15 ], # Ry (mm) - [ -0.047, -0.047, -0.047, -0.047, 0.038, 0.038, 0.038, 0.038 ], # Rz (mm) + [ 0, 0, 0, 0, 0.7071, 0.7071, -0.7071, -0.7071 ], # Fx (N) + [ 0, 0, 0, 0, -0.7071, 0.7071, -0.7071, 0.7071 ], # Fy (N) + [ 1, 1, 1, 1, 0, 0, 0, 0 ], # Fz (N) + [ 0.12, -0.12, 0.12, -0.12, -0.0268698, 0.0268698, -0.0268698, 0.0268698 ], # Rx (N*m) + [ -0.19, -0.19, 0.19, 0.19, -0.0268698, -0.0268698, 0.0268698, 0.0268698 ], # Ry (N*m) + [ 0, 0, 0, 0, -0.180311, 0.180311, 0.180311, -0.180311 ], # Rz (N*m) ] + self.inverse_config = np.linalg.pinv(self.motor_config, rcond=1e-15, hermitian=False) self.subscription = self.create_subscription(Twist, 'drive/twist', self._callback, 10)