Improved Kinematics #187
Merged
Improved Kinematics #187
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
The list in the motor message (data) needed to be initialized to all zeroes, and each value from the result of the matrix multiplication must be assigned to that list. This is because numpy's matmul returns an ndarray and not a list, and therefore cannot be directly copied. TODO: Use the function on the resultant ndarray to do direct assignment.
Data came from the Blue Robotics T200 Motor Technical Details, link will be in commit comment. Data will be updated with more accurate numbers after testing.
…ers for previous function.
The thrust limit scalar determines maximum thrust in direction, maximum pilot input controls percentage of that maximum. TODO: Implement current scaling and select the smaller of the scalars between current and thrust limits to determine thrust maximum
…eter When the center_of_mass_offset parameter is updated, the motor_config matrix will regenerate and a new inverse_config matrix will be generated from that.
Note: In production, we will have to gather our own thrust->current data, since the provided Blue Robotics data won't accurately reflect our setup. Also, we should not consider the hard limits to be our actual limit, and instead use some percentage of that maximum, such as 80% or 90%
List comprehensions are generally faster and more readable, see Gudio blog post and diff, respectively. Also removed unnecessary initilization of the motor_msg data array.
|
When I try to run the nodes I get this: |
Fixed in be5a597! |
steph1111
reviewed
Jan 16, 2024
steph1111
reviewed
Jan 16, 2024
steph1111
reviewed
Jan 16, 2024
steph1111
reviewed
Jan 16, 2024
| return min([(self.MAX_FWD_THRUST / thrust) if thrust > 0 | ||
| else ((self.MAX_REV_THRUST / thrust) if thrust < 0 | ||
| else float('inf')) | ||
| for thrust in motor_values]) | ||
|
|
||
| def _callback(self, twist_msg): |
There was a problem hiding this comment.
Double underscore for private methords
steph1111
reviewed
Jan 16, 2024
| self.__params[5] * sum(mv), | ||
| self.__params[6] * len(mv) - limit] | ||
|
|
||
| def get_current_scalar_value(self, mv: list, limit: float) -> float: |
There was a problem hiding this comment.
Consider adding a link to the paper you wrote on the methodology. Add the pdf to the doc directory
steph1111
reviewed
Jan 16, 2024
steph1111
reviewed
Jan 16, 2024
steph1111
reviewed
Jan 16, 2024
steph1111
approved these changes
Jan 17, 2024
There was a problem hiding this comment.
The revisions made to the Thrust node in this PR are extremely impressive. These enhancements are a substantial improvement over the previous implementation. The updated methodology which uses a much more mathematically sound way of converting Twist to motor values will significantly improve the drive-ability of the robot. From reviewing this code, reading the paper you wrote, and talking to you over the last few months, it is clear that the underlying mathematics of the kinematics, current limiting, and thrust limiting code are well thought out and thoroughly researched. After viewing the simulation in rviz I am confident that this should be merged
Also very noice list comprehensions :D
Updates CoM offset member by calling self.get_parameter(...).value, and moves the other function calls within the try block
Removed as member was never accessed outside of parameter setting callback. Also replaced try/catch with a check for paramter validity and an early return.
|
I'm getting this error: |
Updated according to Isaac's new values given. Also reformatted the motor thrust matrix to match the new formatting.
|
This worked! Nice job. |
mike-matera
approved these changes
Feb 2, 2024
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.
This PR changes the Kinematics node in four major ways:
1. Improved Kinematics methodology
This PR changes the process by which the robot determines how much thrust each motor produces to a mathematically and physically sound method. It does this by encoding the resultant thrust and torque each motor will produce into a matrix and generating its Moore-Penrose (pseudo)inverse. When this inverse matrix is multiplied by the pilot's desired twist vector, we are given a list of thrusts in newtons that each motor should produce in order to achieve the multiplied twist vector.
Moore-Penrose inverses have multiple important properties in respect to the problem of kinematics:
2. Variable Center of Mass
While the Moore-Penrose inverse method described above allows for precise control of the robot, it's main limitation is it requires that the position of the Center of Mass be accurate. One could imagine that as the Center of Mass moves away from its expected location (in practice due to lifting a heavy object) the actual torque produced by each motor will stray further and further from those prescribed by the matrix math (due to a changing radius).
In order to remedy this, the ability for a moving Center of Mass using ROS parameters was added. While this could potentially be controlled automatically in the future, the intended use was for the pilot to be able to trim the Center of Mass of the robot during flight.
While the kinematics allows for arbitrary Center of Mass shifts, it is probably most advantageous to only give the pilot the ability to shift the center of mass along the line connecting the normal Center of Mass position to the Claw, as that is line the Center of Mass will actually shift along when lifting an object (a CoM reset button would also prove useful). Regardless of the implementation, the kinematics allows for it, so this can be tweaked in the future.
3. Thrust Limiting
The Blue Robotics T200 have several limitations, one of which being their thrust limits.
The motors have separate forward and reverse thrust limit, and we must take that into account. The methodology is to find a scaling factor for each motor that would scale it to its maximum thrust in the direction it is going. We then select the minimum of those possible scalars, as any of the other scaling factors will scale some of the other motors past their maximums.
4. Current Limiting
The robot has two main current limits which must be taken into account:
To do this we employ an identical method as we do for thrust limiting. We calculate the maximum scaling factors such that all the motors together draw 70A, the motors on the first ESC draw 40A, and the motors on the second ESC also draw 40A. We then select the minimum of those scaling factors.
Of course, to calculate any of those scaling factors is slightly more complicated. In the case of the thrust limiting, it is very easy as thrust is proportional to thrust (the proof of this is left to the reader as an exercise). However, there is a nonlinear relationship between current draw and thrust. We model that relationship with a 5th degree polynomial.
So for a given list of motor thrusts, we can do some algebraic manipulation to receive another 5th degree polynomial function, except this time it is a function of the desired scaling factor. We can then subtract the current limit we are trying to reach, and find the root of that function. We are going to receive multiple roots for this, so we select the smallest positive, real root.
Conclusions
These changes will make the kinematics far more accurate, maximize our thrust envelope, and can account for parametric uncertainties - all while being relatively computationally inexpensive.
However, there will undoubtedly be changes and tweaks needed. Here is a short list of things which will be necessary to change in the future:
There are certainly other issues which will arise, however the code in this branch works around the limitations that are known to the developer. Any changes that must be made likely won't be too difficult to fix (famous last words?). Please feel free to ask any questions you might have regarding the code or the methodology.