Teo vector verifcation #177
Open
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
|
This fixex #172 |
OrionOth
reviewed
Nov 17, 2023
src/seahawk/seahawk_deck/thrust.py
Outdated
Comment on lines
98
to
107
| # Multiply twist with inverse of motor config to get motor effort values | ||
| 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] |
There was a problem hiding this comment.
Is it possible to refactor this to use a loop?
OrionOth
requested changes
Nov 17, 2023
Comment on lines
58
to
66
| # 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] |
There was a problem hiding this comment.
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?
Comment on lines
49
to
56
| 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] | ||
| ] |
There was a problem hiding this comment.
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!)
src/seahawk/seahawk_deck/thrust.py
Outdated
Comment on lines
45
to
52
| self.motor_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] | ||
| ] |
There was a problem hiding this comment.
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!)
…ion relative to CoM
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Adds new file to verify vectors (thrustverify.py), while modifying thrust.py. Trustverify.py takes in published thruster data from thrust.py, and predicts what input vector would have created those values. Such predicted vector is published to "drive/predict", in order to verify the robot is firing its thrusters in the predicted manner.