PX4 SITL support - #26
Conversation
Handle vehicle on ground
Signed-off-by: Ramon Roche <mrpollo@gmail.com>
- Remove quaternion unit-norm penalty in quat_dot (renormalize post-step) - Use solve_ivp Radau with max_step=dt and relaxed tolerances - Apply analytic motor dynamics during step to avoid stiffness from tau_m Results: large speedup vs RK45 once motors spin; stability maintained.
- Extract SIH sim params to rotorpy/vehicles/px4_params/sihsim_quadx.py - Make PX4Multirotor build a default hover initial_state when None - Factor step() into helpers for clarity and reuse - Update example to import params from new module Prepares for adding CF/CFBL/Hummingbird presets under px4_params.
- Add _quaternion_rotorpy_to_aerospace() static method for reusable quaternion conversion - Simplify _create_sensor_data() by using the new conversion method - Improve code modularity and maintainability - Add comprehensive documentation for coordinate frame transformations - Fix missing time import in __main__ section
… code duplication - Use Ardupilot._quaternion_rotorpy_to_aerospace() for consistent transformations - Add enu_to_geodetic() and geodetic_to_mavlink() static methods for coordinate conversion - Clean up unused scipy.spatial.transform import - Reduce code duplication while maintaining functionality - Enhance code modularity and maintainability with proper coordinate frame handling
The noisy acceleration from the imu was being sent on the HIL_STATE_QUATERNION message, which expects ground truth data
… split, cleanup printing statements
…quaternion and _send_hil_sensor methods
…d enable ground effect
|
I have got this to a good stage where I can fly around in position control mode, @spencerfolk marking as ready for review. |
mrpollo
left a comment
mrpollo
left a comment
There was a problem hiding this comment.
Thanks for pushing this forward. I don't have the bandwidth to continue working on my PR! The PX4 parts seem right, however, I can't comment on the rotorpy internals, nor the Ardupilot stuff.
|
@spencerfolk would love to have a review on this! |
|
@Tuxliri looks really good. I can take a closer look later this week and hopefully merge! Thanks! |
|
@Tuxliri I'm finally getting to this on the weekend. I've taken a closer look and made a few adjustments to match the rest of the rotorpy repo. You can see these commits here I wasn't able to push them directly to your branch. Could you take a look at these commits and run your tests to make sure I didn't break anything? The big thing I did was revert the changes to the dynamics integration. I know you changed it because PX4 was having trouble with the stiffness of the dynamics, but I'm hesitant to make changes to the dynamics integration until I can measure its affect on the performance of the simulator (w.r.t. speed) (#27). But perhaps with that unit test we could confirm the affect is minimal. If everything still works merge those commits into your fork and we can merge. |
spencerfolk
left a comment
spencerfolk
left a comment
There was a problem hiding this comment.
See above comment about checking my additional tweaks and merging my commits into this PR.
|
@spencerfolk I have merged your commits and I can successfully fly in the simulator but I would like to keep the ground handling in the dynamics (it was removed in |
|
@Tuxliri Ah yes I did take note of that. The reason I removed L301-324 is because I thought I saw ground handling also being handled elsewhere. I thought L301-324 was no longer needed. I think this function also handles ground interaction and is indeed ran if the Can you please double check that this is indeed the case and L301-324 is no longer needed? |
|
@spencerfolk you're right part of the ground handling in the |
Changed the tolerance from 2e-2 to 3e-3 to fix the test failing on Python 3.10
|
@spencerfolk hi, just pinging to check if you're okay with merging this! |
spencerfolk
left a comment
spencerfolk
left a comment
There was a problem hiding this comment.
I think this implementation makes sense. Thanks for removing the redundant ground handling! I'm a bit uneasy about having to relax the tolerance on the batched sim testing... but the relaxation is rather modest so it's ok.
3 participants
I have been working on getting rotorpy to work with PX4 SITL, based on PR #25. I was a bit confused in that PR as how to reconcile PX4 SIH dynamics and rotorpy's own dynamics.
I am opening this as a draft since I have got this to a point where there are still some quirks to fix in the ground handling for repeated flights.
As of now I am leaving the simulation of magnetometer, barometer, gps and air speed sensor to PX4's own simulation modules.
One thing to note is that I have had to change the integration solver to a stiff one, it seems that the motor dynamics was causing the IVP to be stiff and RK45 was struggling to solve it in a timely manner. Right now this is hardcoded but I can move it to be enabled by a parameter. It would make sense though in my opinion to keep the stiff solver as in general the dynamics of the motor have a much smaller time constant than the rigid body dynamics.This has been reverted.This can be tested by launching the PX4 SITL by building it using
make px4_sitland then runningPX4_SIMULATOR=none PX4_SYS_AUTOSTART=10040 ./build/px4_sitl_default/bin/px4and then launching thebasic_usage_px4.pyexample.I have also built a Docker image to run the px4 binary, it can be run with:
docker run --rm --net=host duckietown/dt-px4:1.16