Calculate Chassis velocities using pseudo-inverse using normal equations and Cholesky decomposition - #71
Open
JulianBarossi wants to merge 5 commits into
Open
Conversation
…ns and cholesky decomposition
MichaelYKersey
left a comment
MichaelYKersey
left a comment
Contributor
There was a problem hiding this comment.
I will trust that the actual math correct, but add some comments so someone has some ideas of what to search up/look at if they want to understand in the future.
added 4 commits
February 7, 2026 15:39
…y decomposition changed commit to the correct branch
| */ | ||
| float modulus_range(float p_value, float p_lower, float p_upper); | ||
|
|
||
| bool isSafe(const std::array<swerve_module_state, module_count>& modules) |
Contributor
There was a problem hiding this comment.
use type const& not const type&
| // https://www.youtube.com/watch?v=C7LEuhS4H94&t=8s | ||
| // https://math.mit.edu/icg/resources/teaching/18.085-spring2015/LeastSquares.pdf | ||
| // https://tobydriscoll.net/fnc-julia/leastsq/normaleqns.html | ||
| constexpr float M_PI = 3.14159265358979323846f; |
Contributor
There was a problem hiding this comment.
c++ lib has it's own pi const, use that
Comment on lines
+142
to
+147
| auto const state = module.get_actual_state_cache(); | ||
| float const speed = state.propulsion_velocity; | ||
| float const angle_degrees = state.steer_angle; | ||
| float const angle_rad = angle_degrees * deg_to_rad; | ||
| float const vix = speed * std::cos(angle_rad); | ||
| float const viy = speed * std::sin(angle_rad); |
Contributor
There was a problem hiding this comment.
might be good to make a state to velocity vector function if it doesn't already exist
Comment on lines
+377
to
+385
| auto const& module = *p_modules[i]; | ||
| auto const state = module.get_actual_state_cache(); | ||
| auto const& position = module.settings.position; | ||
|
|
||
| // convert deg/sec to rad/sec | ||
| float omega_radians = estimates.rotational_vel * deg_to_rad; | ||
|
|
||
| float est_vx = estimates.translation.x - omega_radians * position.y; | ||
| float est_vy = estimates.translation.y + omega_radians * position.x; |
Contributor
There was a problem hiding this comment.
these math calculations (module velocity vector & chassis speeds to module sate or vector) should already exist in some other drive math function, if not then make one.
Comment on lines
+321
to
+329
| float angleDiff(float a, float b) | ||
| { | ||
|
|
||
| float d = std::fmod(a - b + M_PI, 2.0 * M_PI); | ||
| if (d < 0) | ||
| d += 2.0 * M_PI; | ||
|
|
||
| return d - M_PI; | ||
| } |
Contributor
There was a problem hiding this comment.
Is there a reason to noy use modulus_range
This file contains hidden or 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
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
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.
chassis_velocities [calc_estimated_chassis_velocities] function