Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Teo vector verifcation #177

Open
wants to merge 14 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from 4 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
41 changes: 29 additions & 12 deletions src/seahawk/seahawk_deck/thrust.py
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@
cabrillorobotics@gmail.com
'''
import sys

import math
import rclpy

from rclpy.node import Node
Expand All @@ -42,18 +42,15 @@ 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],
[0, 0, 0, 0, -0.7071, 0.7071, -0.7071, 0.7071],
[1, 1, 1, 1, 0, 0, 0, 0],
[-.19, -.19, .19, .19, -.105, -.105, .105, .105],
[-.12, .12, -.12, .12, -.15, .15, -.15, .15],
[-.047,-.047,-.047, -.047, 0.038, 0.038, 0.038, 0.038]
]
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)
Expand Down Expand Up @@ -87,9 +84,29 @@ 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]

print(np.matmul(self.motor_config, motor_efforts))

self.motor_pub.publish(motor_msg)

Expand Down
79 changes: 79 additions & 0 deletions src/seahawk/seahawk_deck/thrustverify.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,79 @@
'''
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

import numpy as np

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(Float32MultiArray, 'drive/predict', 10)

def motor_math(self, msg):
# motor 0-7 within list index
motor_msg = msg

config = [
[0, 0, 0, 0, 0.7071, 0.7071, -0.7071, -0.7071],
[0, 0, 0, 0, -0.7071, 0.7071, -0.7071, 0.7071],
[1, 1, 1, 1, 0, 0, 0, 0],
[-.19, -.19, .19, .19, -.105, -.105, .105, .105],
[-.12, .12, -.12, .12, -.15, .15, -.15, .15],
[-.047,-.047,-.047, -.047, 0.038, 0.038, 0.038, 0.038]
]
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you add a brief code comment describing what exactly this is, and how we got it? We can save specifics for markdown documentation (should probably write that too!)

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These latest changes (757736b, 8bce7db), while they do help, don't particularly resolve describing what config is exactly. It's good you are describing the components of it, however.


# Linear Thrust Vector
x = config[0][0]*motor_msg.data[4] + config[0][1]*motor_msg.data[5] + config[0][2]*motor_msg.data[6] + config[0][3]*motor_msg.data[7]
y = config[1][0]*motor_msg.data[4] + config[1][1]*motor_msg.data[5] + config[1][2]*motor_msg.data[6] + config[1][3]*motor_msg.data[7]
z = config[2][0]*motor_msg.data[0] + config[2][1]*motor_msg.data[1] + config[2][2]*motor_msg.data[2] + config[2][3]*motor_msg.data[3]

# Angular Thrust Vector
rx = config[3][0]*motor_msg.data[0] + config[3][1]*motor_msg.data[1] + config[3][2]*motor_msg.data[2] + config[3][3]*motor_msg.data[3] + config[3][4]*motor_msg.data[4] + config[3][5]*motor_msg.data[5] + config[3][6]*motor_msg.data[6] + config[3][7]*motor_msg.data[7]
ry = config[4][0]*motor_msg.data[0] + config[4][1]*motor_msg.data[1] + config[4][2]*motor_msg.data[2] + config[4][3]*motor_msg.data[3] + config[4][4]*motor_msg.data[4] + config[4][5]*motor_msg.data[5] + config[4][6]*motor_msg.data[6] + config[4][7]*motor_msg.data[7]
rz = config[5][0]*motor_msg.data[0] + config[5][1]*motor_msg.data[1] + config[5][2]*motor_msg.data[2] + config[5][3]*motor_msg.data[3] + config[5][4]*motor_msg.data[4] + config[5][5]*motor_msg.data[5] + config[5][6]*motor_msg.data[6] + config[5][7]*motor_msg.data[7]
Copy link
Member

@OrionOth OrionOth Nov 17, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is basically unreadable, is it possible to do this in a clearer and more programmatic (rather than purely mathematical) fashion?
The line length is very long too, if you don't take my above advice can you at least format it so its broken out onto separate lines?


predicted_vectors = Float32MultiArray()
predicted_vectors.data = [x, y, z, rx, ry, 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)