Teo vector verifcation

src/seahawk/seahawk_deck/thrust.py

@@ -23,7 +23,6 @@
 cabrillorobotics@gmail.com
 '''
 import sys 
-
 import rclpy
 
 from rclpy.node import Node 
@@ -42,18 +41,16 @@ def __init__(self):
         """Initialize this node"""
         super().__init__('thrust')
 
-        root3 = 1/math.sqrt(3)
-
-
         self.motor_config = [
-            [root3, root3, root3, root3, -root3, -root3, -root3, -root3], 
-            [-root3, root3, -root3, root3, -root3, root3, -root3, root3], 
-            [root3, root3, -root3, -root3, root3, root3, -root3, -root3],
-            [root3, -root3, -root3, root3, root3, -root3, -root3, root3],
-            [-2*root3, -2*root3, 2*root3, 2*root3, 2*root3, 2*root3, -2*root3, -2*root3],
-            [-1/root3, 1/root3, -1/root3, 1/root3, 1/root3, -1/root3, 1/root3, -1/root3]
+            [     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(motor_config, rcond=1e-15, hermitian=False)
+
+        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)
         self.motor_pub = self.create_publisher(Float32MultiArray, 'drive/motors', 10)
@@ -87,9 +84,27 @@ def _callback(self, twist_msg):
             twist_msg.angular.y,
             twist_msg.angular.z
         ]
+        motor_msg.data = [
+            0.0,  # Motor 0 thrust 
+            0.0,  # Motor 1 thrust
+            0.0,  # Motor 2 thrust
+            0.0,  # Motor 3 thrust
+            0.0,  # Motor 4 thrust
+            0.0,  # Motor 5 thrust
+            0.0,  # Motor 6 thrust
+            0.0,  # Motor 7 thrust
+        ]
 
         # Multiply twist with inverse of motor config to get motor effort values
-        motor_msg.data = np.matmul(self.inverse_config, twist_array)
+        motor_efforts = np.matmul(self.inverse_config, twist_array)
+        motor_msg.data[0] = motor_efforts[0]
+        motor_msg.data[1] = motor_efforts[1]
+        motor_msg.data[2] = motor_efforts[2]
+        motor_msg.data[3] = motor_efforts[3]
+        motor_msg.data[4] = motor_efforts[4]
+        motor_msg.data[5] = motor_efforts[5]
+        motor_msg.data[6] = motor_efforts[6]
+        motor_msg.data[7] = motor_efforts[7]
 
         self.motor_pub.publish(motor_msg)
 

src/seahawk/seahawk_deck/thrustverify.py

@@ -0,0 +1,130 @@
+"""
+thrustverify.py
+
+Calculate predicted thrust and angular vector based on motor status and output it on /drive/predict
+
+Copyright (C) 2022-2023 Cabrillo Robotics Club
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU Affero General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU Affero General Public License for more details.
+
+You should have received a copy of the GNU Affero General Public License
+along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+Cabrillo Robotics Club
+6500 Soquel Drive Aptos, CA 95003
+cabrillorobotics@gmail.com
+"""
+import sys
+
+import rclpy
+
+from rclpy.node import Node
+
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Float32MultiArray
+
+class ThrustVerify(Node):
+    """Caculates expected input, based on current motor status"""
+
+    def __init__(self):
+        super().__init__("thrust")
+
+        self.subscription = self.create_subscription(
+            Float32MultiArray, "drive/motors", self.motor_math, 10
+        )
+        self.vector_predict = self.create_publisher(
+            Twist, "drive/predict", 10
+        )
+        self.motor_config = [
+            [     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)
+        ]
+
+    def motor_math(self, msg):
+        # motor 0-7 within list index
+        motor_msg = msg
+
+        # Linear Thrust Vector -- Operable
+        x = (
+              self.motor_config[0][4] * motor_msg.data[4]
+            + self.motor_config[0][5] * motor_msg.data[5]
+            + self.motor_config[0][6] * motor_msg.data[6]
+            + self.motor_config[0][7] * motor_msg.data[7]
+        )
+        y = (
+              self.motor_config[1][4] * motor_msg.data[4]
+            + self.motor_config[1][5] * motor_msg.data[5]
+            + self.motor_config[1][6] * motor_msg.data[6]
+            + self.motor_config[1][7] * motor_msg.data[7]
+        )
+        z = (
+              self.motor_config[2][0] * motor_msg.data[0]
+            + self.motor_config[2][1] * motor_msg.data[1]
+            + self.motor_config[2][2] * motor_msg.data[2]
+            + self.motor_config[2][3] * motor_msg.data[3]
+        )
+
+        # Angular Thrust Vector -- Inoperable
+        rx = (
+              self.motor_config[3][0] * motor_msg.data[0]
+            + self.motor_config[3][1] * motor_msg.data[1]
+            + self.motor_config[3][2] * motor_msg.data[2]
+            + self.motor_config[3][3] * motor_msg.data[3]
+            + self.motor_config[3][4] * motor_msg.data[4]
+            + self.motor_config[3][5] * motor_msg.data[5]
+            + self.motor_config[3][6] * motor_msg.data[6]
+            + self.motor_config[3][7] * motor_msg.data[7]
+        )
+        ry = (
+              self.motor_config[4][0] * motor_msg.data[0]
+            + self.motor_config[4][1] * motor_msg.data[1]
+            + self.motor_config[4][2] * motor_msg.data[2]
+            + self.motor_config[4][3] * motor_msg.data[3]
+            + self.motor_config[4][4] * motor_msg.data[4]
+            + self.motor_config[4][5] * motor_msg.data[5]
+            + self.motor_config[4][6] * motor_msg.data[6]
+            + self.motor_config[4][7] * motor_msg.data[7]
+        )
+        rz = (
+              self.motor_config[5][0] * motor_msg.data[0]
+            + self.motor_config[5][1] * motor_msg.data[1]
+            + self.motor_config[5][2] * motor_msg.data[2]
+            + self.motor_config[5][3] * motor_msg.data[3]
+            + self.motor_config[5][4] * motor_msg.data[4]
+            + self.motor_config[5][5] * motor_msg.data[5]
+            + self.motor_config[5][6] * motor_msg.data[6]
+            + self.motor_config[5][7] * motor_msg.data[7]
+        )
+
+        predicted_vectors = Twist()
+
+        predicted_vectors.linear.x = x
+        predicted_vectors.linear.y = y
+        predicted_vectors.linear.z = z
+        predicted_vectors.angular.x = rx
+        predicted_vectors.angular.y = ry
+        predicted_vectors.angular.z = rz
+
+        self.vector_predict.publish(predicted_vectors)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    rclpy.spin(ThrustVerify())
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main(sys.argv)