Used Gemini to convert the C++ tests in wpimath/.../kinematics to
pytest. For ease of use required a better constructor for
`MecanumDriveWheelPositions`
I did this conversion months ago (before robotpy landed and before the
reorg), so new tests might be missing. At least its better than nothing
🤷
Broke the huge pr that does almost everything into parts so its easier
to review.
---------
Co-authored-by: David Vo <auscompgeek@users.noreply.github.com>
Resync with `mostrobotpy`
This mostly involves the big "ignore almost everything in the HAL
project" and some fixups for the Addressable LED classes.
Required two small hand fixes to get it building over here with bazel,
and with more compiler warnings on.
I also manually zeroed out the `repo_url` field in the toml files to
avoid unnecessary churn whenever it goes from a release build to a
development build. I already did this with `version` field in there, and
will do a follow up PR that updates the copybara script to do it
automatically.
---------
Co-authored-by: Default email <default@default.com>
I tried to sync `mostrobotpy` with `allwpilib` and was getting a
compilation error I had not seen before when it tried to do the stub
generation (which `allwpilib` does not do).
Luckily, I was able to debug it here by writing some unit tests (i.e.,
having Gemini convert the C++ tests into python) that failed in a
similar way. The main problem was needing to write a custom constructor
for the class and adding a `force_type_casters`. I used `ChassisSpeed`
as my main example, but I did not copy all of the other custom code like
overriding the index operator, `__repr__` operator, feet helpers, etc. I
can gladly add those in.
In the future, we should start enforce a policy that if you add a C++ or
Java unit test, you also have to add a python test. That developer might
have gotten more stuck on the minutia of how to fix it, but this problem
would have at least been caught earlier before it landed.
This got missed in the reorg, and these values aren't actually used for
anything when building in `allwpilb`, but we might as well fix them here
to make the copybara process easier.
ChassisAccelerations and the drivetrain acceleration types are added in
both Java and C++. `ChassisAccelerations` is basically just
`ChassisSpeeds` but for accelerations!
`DifferentialDriveWheelAccelerations`, `MecanumDriveWheelAccelerations`,
and `SwerveModuleAccelerations` are the acceleration equivalent of the
drivetrain speeds types.
In Java, the `Kinematics` interface now has an additional generic
parameter `A` which represents the accelerations, and
`toChassisAccelerations` and `toWheelAccelerations` methods, which are
implemented the same way as `toChassisSpeeds` and `toWheelSpeeds`.
Protobuf and struct classes were also added for all four classes in Java
and C++.
---------
Signed-off-by: Zach Harel <zach@zharel.me>
Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
* Moved makeWhiteNoiseVector() to random.Normal.normal()
* Moved isControllable() and isDetectable() to system.LinearSystemUtil
* Renamed makeCostMatrix() to costMatrix() (Java)
* Renamed makeCovarianceMatrix() to covarianceMatrix() (Java)
* Renamed MakeCostMatrix() to CostMatrix() (C++)
* Renamed MakeCovMatrix() to CovarianceMatrix() (C++)
* Removed deprecated poseTo3dVector(), poseTo4dVector(), poseToVector()
* Removed clampInputMaxMagnitude()
* We don't use it, and Eigen has this functionality built in via `u =
u.array().min(u_max.array()).max(u_min.array());`
* Simplified implementation of desaturateInputVector()
After replacing the remaining include guards with `#pragma once`, I was
able to merge all the wpiformat configs into one file in the repo root.
This should make the config easier to reason about and maintain in the
future.
The spiraling issue occurs when the vision rotation standard deviation is very high relative to the odometry rotation standard deviation and the vision measurements have a large rotation error. (Scaling the rotation component of a twist without scaling the translation component causes the direction of overall translation to change, leading to spiraling around (either towards or away) the vision measurement instead of moving towards it.) Using a transform instead of a twist avoids this issue.
In general, scaling twist components is more mathematically correct than scaling transform components. However, although twists are correct for modeling uncertainty in an odometry-only pose estimate, they are not correct for the difference between the odometry-only pose estimate and a vision measurement. Since neither twists nor transforms are completely correct (and the pose estimator as a whole is not mathematically correct), but using transforms can guarantee that the pose estimate approaches the vision measurement (instead of potentially spiraling away), they are the least bad option.
