mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-02-05 02:34:32 +08:00
feature: Integrating the RAPTOR foundation policy (#26082)
* moving raptor bump compiles and raptor mode appears hovering with RAPTOR seems to work Using Raptor to execute offboard commands works (using multirobot f03825a5795a77c5a095f799eeb8e0b646fe7176 to feed the trajectory_setpoint). Requires more testing simplified rotmat runtime inference frequency multiple arming request response reflects actual readiness adjusting to fit IMU gyro ratemax relaxing control timing warning thresholds for SITL Using mode registration to signal if offboard commands should be forwarded to trajectory_setpoint instead of just hardcoding vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD adopting new "request_offboard_setpoint" in raptor module replace offboard seems good mc_raptor: overwrite offboard parameter separate raptor config addendum Raptor off by default RAPTOR readme Loading raptor checkpoint from tar works. check if load was successful refactoring: cutting out the pure C interface to allow direct testing of the policy input/output behavior from the file, without fully loading it into memory first adapter not needed anymore ripping out test observation mode (not used in a long time) fixing warnings bump RLtools to fix the remaining warnings Loading RAPTOR checkpoint from sdcard seems to work on FMU-6C embedding Raptor policy into flash works again also printing checkpoint name when using the embedded policy cleaner handling of the checkpoint name back to reading from file ripping out visual odometry checks cleaner more debug but no success bump rlt bump pre next rebase we can publish the no angvel update because we latch onto it with the scheduled work item anyways this kind of runs on the 6c still bad SIH almost flying saving stale traj setpoint yaw new error. timestamp not the problem anymore bump rlt; SIH works with executor shaping up bumping blob (include tar checkpoint) cleaning up fixing formatting update readme * moving raptor bump compiles and raptor mode appears hovering with RAPTOR seems to work Using Raptor to execute offboard commands works (using multirobot f03825a5795a77c5a095f799eeb8e0b646fe7176 to feed the trajectory_setpoint). Requires more testing simplified rotmat runtime inference frequency multiple arming request response reflects actual readiness adjusting to fit IMU gyro ratemax relaxing control timing warning thresholds for SITL Using mode registration to signal if offboard commands should be forwarded to trajectory_setpoint instead of just hardcoding vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD adopting new "request_offboard_setpoint" in raptor module replace offboard seems good mc_raptor: overwrite offboard parameter separate raptor config addendum Raptor off by default RAPTOR readme Loading raptor checkpoint from tar works. check if load was successful refactoring: cutting out the pure C interface to allow direct testing of the policy input/output behavior from the file, without fully loading it into memory first adapter not needed anymore ripping out test observation mode (not used in a long time) fixing warnings bump RLtools to fix the remaining warnings Loading RAPTOR checkpoint from sdcard seems to work on FMU-6C embedding Raptor policy into flash works again also printing checkpoint name when using the embedded policy cleaner handling of the checkpoint name back to reading from file ripping out visual odometry checks cleaner more debug but no success bump rlt bump pre next rebase we can publish the no angvel update because we latch onto it with the scheduled work item anyways this kind of runs on the 6c still bad SIH almost flying saving stale traj setpoint yaw new error. timestamp not the problem anymore bump rlt; SIH works with executor shaping up bumping blob (include tar checkpoint) cleaning up fixing formatting update readme updating gitignore * fixing format and declaring submodules as cmake dependencies * adding uORB message documentation * fixing comment alignment * Adding option to restrict mc_raptor to not listen to the trajectory_setpoint (use the position and yaw at activation time as reference instead) * bump RLtools; relax timing thresholds and adding real world readme * smooth traj tracking performance * Measuring trajectory_setpoint timing (providing stats in raptor_status); reverting accidental .gitignore modification * More ideomatic way of setting the path to the policy checkpoint * Reset trajectory_setpoint on raptor mode activation * Adding internal trajectory generation (feeding trajectory_setpoint over Mavlink is too noisy). Quite agile trajectory tracking, good performance * stable flight * Update msg/versioned/RaptorInput.msg Co-authored-by: Hamish Willee <hamishwillee@gmail.com> * adopting message formatting conventions * sort raptor.px4board * Archiving RegisterExtComponentRequestV1.msg * Add message versioning for VehicleStatus v2 and RegisterExtComponentRequest v2 * fixing formatting * making internal reference configurable via command * RAPTOR docs wip * raptor internal reference documentation * Finishing RAPTOR docs first draft * adding logging instructions * Fixing missing command documentation test error * fixing format * adding motor layout warning * raptor minimal subedit - prettier, images etc * Improve intro * Fix up Neural_Networks version * Mentioning "Adaptive" in the RAPTOR documentation's title * Adding clarifications about the internal reference trajectory generator * Removing "foundation policy" wording * Fixing new-line check * Removing redundant (evident through directory hierarchy) raptor_ from filenames * Unifying Neural Network docs (mc_nn_control and mc_raptor) under the "Neural Network" topic * Fix to standard structure * Making the distinction between mc_nn_control and mc_raptor more clear and fixing the comparison table * Removing trajectory_setpoint forwarding flag from external mode registration request and from the vehicle status * Trivial layout and wording fixes * fixing docs error --------- Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
This commit is contained in:
Jonas Eschmann
GitHub
6
.gitmodules
vendored
6
.gitmodules
vendored
@@ -103,3 +103,9 @@
|
||||
[submodule "src/drivers/ins/sbgecom/sbgECom"]
|
||||
path = src/drivers/ins/sbgecom/sbgECom
|
||||
url = https://github.com/PX4/sbgECom.git
|
||||
[submodule "src/modules/mc_raptor/blob"]
|
||||
path = src/modules/mc_raptor/blob
|
||||
url = https://github.com/rl-tools/px4-blob
|
||||
[submodule "src/lib/rl_tools/rl_tools"]
|
||||
path = src/lib/rl_tools/rl_tools
|
||||
url = https://github.com/rl-tools/rl-tools.git
|
||||
|
||||
10
.vscode/cmake-variants.yaml
vendored
10
.vscode/cmake-variants.yaml
vendored
@@ -6,6 +6,16 @@ CONFIG:
|
||||
buildType: RelWithDebInfo
|
||||
settings:
|
||||
CONFIG: px4_sitl_default
|
||||
px4_sitl_raptor:
|
||||
short: px4_sitl_raptor
|
||||
buildType: RelWithDebInfo
|
||||
settings:
|
||||
CONFIG: px4_sitl_raptor
|
||||
px4_sitl_raptor_debug:
|
||||
short: px4_sitl_raptor_debug
|
||||
buildType: Debug
|
||||
settings:
|
||||
CONFIG: px4_sitl_raptor
|
||||
px4_sitl_spacecraft:
|
||||
short: px4_sitl_spacecraft
|
||||
buildType: RelWithDebInfo
|
||||
|
||||
@@ -41,3 +41,9 @@ if param compare -s MC_NN_EN 1
|
||||
then
|
||||
mc_nn_control start
|
||||
fi
|
||||
|
||||
|
||||
if param compare -s MC_RAPTOR_ENABLE 1
|
||||
then
|
||||
mc_raptor start
|
||||
fi
|
||||
|
||||
95
boards/px4/fmu-v6c/raptor.px4board
Normal file
95
boards/px4/fmu-v6c/raptor.px4board
Normal file
@@ -0,0 +1,95 @@
|
||||
CONFIG_BOARD_ARCHITECTURE="cortex-m7"
|
||||
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
|
||||
CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS6"
|
||||
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS5"
|
||||
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
|
||||
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
|
||||
CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
|
||||
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
|
||||
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
|
||||
CONFIG_COMMON_DISTANCE_SENSOR=y
|
||||
CONFIG_COMMON_LIGHT=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_COMMON_OPTICAL_FLOW=y
|
||||
CONFIG_COMMON_TELEMETRY=y
|
||||
CONFIG_DRIVERS_ACTUATORS_VERTIQ_IO=y
|
||||
CONFIG_DRIVERS_ADC_BOARD_ADC=y
|
||||
CONFIG_DRIVERS_BAROMETER_MS5611=y
|
||||
CONFIG_DRIVERS_BATT_SMBUS=y
|
||||
CONFIG_DRIVERS_CAMERA_CAPTURE=y
|
||||
CONFIG_DRIVERS_CAMERA_TRIGGER=y
|
||||
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
|
||||
CONFIG_DRIVERS_DSHOT=y
|
||||
CONFIG_DRIVERS_GNSS_SEPTENTRIO=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_HEATER=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI055=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA228=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA238=y
|
||||
CONFIG_DRIVERS_PWM_OUT=y
|
||||
CONFIG_DRIVERS_PX4IO=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_LIB_RL_TOOLS=y
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
CONFIG_MODULES_ESC_BATTERY=y
|
||||
CONFIG_MODULES_EVENTS=y
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
||||
CONFIG_MODULES_FW_ATT_CONTROL=n
|
||||
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
|
||||
CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
|
||||
CONFIG_MODULES_FW_MODE_MANAGER=n
|
||||
CONFIG_MODULES_FW_RATE_CONTROL=n
|
||||
CONFIG_MODULES_GIMBAL=y
|
||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||
CONFIG_MODULES_LAND_DETECTOR=y
|
||||
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
|
||||
CONFIG_MODULES_LOAD_MON=y
|
||||
CONFIG_MODULES_LOGGER=y
|
||||
CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
|
||||
CONFIG_MODULES_MANUAL_CONTROL=y
|
||||
CONFIG_MODULES_MAVLINK=y
|
||||
CONFIG_MODULES_MC_ATT_CONTROL=y
|
||||
CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
|
||||
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
|
||||
CONFIG_MODULES_MC_POS_CONTROL=y
|
||||
CONFIG_MODULES_MC_RAPTOR=y
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=y
|
||||
CONFIG_MODULES_NAVIGATOR=y
|
||||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
||||
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_NUM_MISSION_ITMES_SUPPORTED=1000
|
||||
CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
|
||||
CONFIG_SYSTEMCMDS_BSONDUMP=y
|
||||
CONFIG_SYSTEMCMDS_DMESG=y
|
||||
CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
|
||||
CONFIG_SYSTEMCMDS_I2CDETECT=y
|
||||
CONFIG_SYSTEMCMDS_LED_CONTROL=y
|
||||
CONFIG_SYSTEMCMDS_MFT=y
|
||||
CONFIG_SYSTEMCMDS_MTD=y
|
||||
CONFIG_SYSTEMCMDS_NSHTERM=y
|
||||
CONFIG_SYSTEMCMDS_PARAM=y
|
||||
CONFIG_SYSTEMCMDS_PERF=y
|
||||
CONFIG_SYSTEMCMDS_REBOOT=y
|
||||
CONFIG_SYSTEMCMDS_SD_BENCH=y
|
||||
CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
|
||||
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
|
||||
CONFIG_SYSTEMCMDS_TOP=y
|
||||
CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
|
||||
CONFIG_SYSTEMCMDS_UORB=y
|
||||
CONFIG_SYSTEMCMDS_VER=y
|
||||
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
|
||||
CONFIG_USE_IFCI_CONFIGURATION=y
|
||||
89
boards/px4/sitl/raptor.px4board
Normal file
89
boards/px4/sitl/raptor.px4board
Normal file
@@ -0,0 +1,89 @@
|
||||
CONFIG_BOARD_ETHERNET=y
|
||||
CONFIG_BOARD_ROOT_PATH="."
|
||||
CONFIG_BOARD_TESTING=y
|
||||
CONFIG_COMMON_SIMULATION=y
|
||||
CONFIG_DRIVERS_CAMERA_TRIGGER=y
|
||||
CONFIG_DRIVERS_GNSS_SEPTENTRIO=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_OSD_MSP_OSD=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_EKF2_VERBOSE_STATUS=y
|
||||
CONFIG_EXAMPLES_DYN_HELLO=y
|
||||
CONFIG_EXAMPLES_FAKE_GPS=y
|
||||
CONFIG_EXAMPLES_FAKE_IMU=y
|
||||
CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y
|
||||
CONFIG_EXAMPLES_HELLO=y
|
||||
CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y
|
||||
CONFIG_EXAMPLES_PX4_SIMPLE_APP=y
|
||||
CONFIG_EXAMPLES_WORK_ITEM=y
|
||||
CONFIG_FIGURE_OF_EIGHT=y
|
||||
CONFIG_LIB_RL_TOOLS=y
|
||||
CONFIG_MAVLINK_DIALECT="development"
|
||||
CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
|
||||
CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
|
||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
CONFIG_MODULES_EVENTS=y
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
||||
CONFIG_MODULES_FW_ATT_CONTROL=y
|
||||
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
|
||||
CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
|
||||
CONFIG_MODULES_FW_MODE_MANAGER=y
|
||||
CONFIG_MODULES_FW_RATE_CONTROL=y
|
||||
CONFIG_MODULES_GIMBAL=y
|
||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||
CONFIG_MODULES_GYRO_FFT=y
|
||||
CONFIG_MODULES_LAND_DETECTOR=y
|
||||
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
|
||||
CONFIG_MODULES_LOAD_MON=y
|
||||
CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
|
||||
CONFIG_MODULES_LOGGER=y
|
||||
CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
|
||||
CONFIG_MODULES_MANUAL_CONTROL=y
|
||||
CONFIG_MODULES_MAVLINK=y
|
||||
CONFIG_MODULES_MC_ATT_CONTROL=y
|
||||
CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
|
||||
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
|
||||
CONFIG_MODULES_MC_POS_CONTROL=y
|
||||
CONFIG_MODULES_MC_RAPTOR=y
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=y
|
||||
CONFIG_MODULES_NAVIGATOR=y
|
||||
CONFIG_MODULES_PAYLOAD_DELIVERER=y
|
||||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_REPLAY=y
|
||||
CONFIG_MODULES_ROVER_ACKERMANN=y
|
||||
CONFIG_MODULES_ROVER_DIFFERENTIAL=y
|
||||
CONFIG_MODULES_ROVER_MECANUM=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
CONFIG_MODULES_SIMULATION_GZ_BRIDGE=y
|
||||
CONFIG_MODULES_SIMULATION_GZ_MSGS=y
|
||||
CONFIG_MODULES_SIMULATION_GZ_PLUGINS=y
|
||||
CONFIG_MODULES_SIMULATION_SENSOR_AGP_SIM=y
|
||||
CONFIG_MODULES_SPACECRAFT=n
|
||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
||||
CONFIG_MODULES_UUV_ATT_CONTROL=y
|
||||
CONFIG_MODULES_UUV_POS_CONTROL=y
|
||||
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=y
|
||||
CONFIG_NUM_MISSION_ITMES_SUPPORTED=10000
|
||||
CONFIG_PLATFORM_POSIX=y
|
||||
CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
|
||||
CONFIG_SYSTEMCMDS_BSONDUMP=y
|
||||
CONFIG_SYSTEMCMDS_DYN=y
|
||||
CONFIG_SYSTEMCMDS_FAILURE=y
|
||||
CONFIG_SYSTEMCMDS_LED_CONTROL=y
|
||||
CONFIG_SYSTEMCMDS_PARAM=y
|
||||
CONFIG_SYSTEMCMDS_PERF=y
|
||||
CONFIG_SYSTEMCMDS_SD_BENCH=y
|
||||
CONFIG_SYSTEMCMDS_SHUTDOWN=y
|
||||
CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
|
||||
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
|
||||
CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
|
||||
CONFIG_SYSTEMCMDS_UORB=y
|
||||
CONFIG_SYSTEMCMDS_VER=y
|
||||
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
|
||||
BIN
docs/assets/advanced/neural_networks/raptor/method.jpg
Normal file
BIN
docs/assets/advanced/neural_networks/raptor/method.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 550 KiB |
File diff suppressed because one or more lines are too long
|
After Width: | Height: | Size: 81 KiB |
File diff suppressed because one or more lines are too long
|
After Width: | Height: | Size: 57 KiB |
BIN
docs/assets/advanced/neural_networks/raptor/visual_abstract.jpg
Normal file
BIN
docs/assets/advanced/neural_networks/raptor/visual_abstract.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 204 KiB |
@@ -822,9 +822,11 @@
|
||||
- [Camera Integration/Architecture](camera/camera_architecture.md)
|
||||
- [Computer Vision](advanced/computer_vision.md)
|
||||
- [Motion Capture (VICON, Optitrack, NOKOV)](tutorials/motion-capture.md)
|
||||
- [Neural Networks](advanced/neural_networks.md)
|
||||
- [Neural Network Module Utilities](advanced/nn_module_utilities.md)
|
||||
- [TensorFlow Lite Micro (TFLM)](advanced/tflm.md)
|
||||
- [Neural Networks](neural_networks/index.md)
|
||||
- [MC NN Control Module (Generic)](neural_networks/mc_neural_network_control.md)
|
||||
- [Neural Network Module Utilities](neural_networks/nn_module_utilities.md)
|
||||
- [TensorFlow Lite Micro (TFLM)](neural_networks/tflm.md)
|
||||
- [RAPTOR Adaptive RL NN Module](neural_networks/raptor.md)
|
||||
- [Installing driver for Intel RealSense R200](advanced/realsense_intel_driver.md)
|
||||
- [Switching State Estimators](advanced/switching_state_estimators.md)
|
||||
- [Out-of-Tree Modules](advanced/out_of_tree_modules.md)
|
||||
|
||||
@@ -1,119 +1 @@
|
||||
# Neural Networks
|
||||
|
||||
<Badge type="tip" text="PX4 v1.17" /> <Badge type="warning" text="Experimental" />
|
||||
|
||||
::: warning
|
||||
This is an experimental module.
|
||||
Use at your own risk.
|
||||
:::
|
||||
|
||||
The Multicopter Neural Network (NN) module ([mc_nn_control](../modules/modules_controller.md#mc-nn-control)) is an example module that allows you to experiment with using a pre-trained neural network on PX4.
|
||||
It might be used, for example, to experiment with controllers for non-traditional drone morphologies, computer vision tasks, and so on.
|
||||
|
||||
The module integrates a pre-trained neural network based on the [TensorFlow Lite Micro (TFLM)](../advanced/tflm.md) module.
|
||||
The module is trained for the [X500 V2](../frames_multicopter/holybro_x500v2_pixhawk6c.md) multicopter frame.
|
||||
While the controller is fairly robust, and might work on other platforms, we recommend [Training your own Network](#training-your-own-network) if you use a different vehicle.
|
||||
Note that after training the network you will need to update and rebuild PX4.
|
||||
|
||||
TLFM is a mature inference library intended for use on embedded devices.
|
||||
It has support for several architectures, so there is a high likelihood that you can build it for the board you want to use.
|
||||
If not, there are other possible NN frameworks, such as [Eigen](https://eigen.tuxfamily.org/index.php?title=Main_Page) and [Executorch](https://pytorch.org/executorch-overview).
|
||||
|
||||
This document explains how you can include the module in your PX4 build, and provides a broad overview of how it works.
|
||||
The other documents in the section provide more information about the integration, allowing you to replace the NN with a version trained on different data, or even to replace the TLFM library altogether.
|
||||
|
||||
If you are looking for more resources to learn about the module, a website has been created with links to a youtube video and a workshop paper. A full master's thesis will be added later. [A Neural Network Mode for PX4 on Embedded Flight Controllers](https://ntnu-arl.github.io/px4-nns/).
|
||||
|
||||
## Neural Network PX4 Firmware
|
||||
|
||||
::: warning
|
||||
This module requires Ubuntu 24.04 or newer (it is not supported in Ubuntu 22.04).
|
||||
:::
|
||||
|
||||
The module has been tested on a number of configurations, which can be build locally using the commands:
|
||||
|
||||
```sh
|
||||
make px4_sitl_neural
|
||||
```
|
||||
|
||||
```sh
|
||||
make px4_fmu-v6c_neural
|
||||
```
|
||||
|
||||
```sh
|
||||
make mro_pixracerpro_neural
|
||||
```
|
||||
|
||||
You can add the module to other board configurations by modifying their `default.px4board file` configuration to include these lines:
|
||||
|
||||
```sh
|
||||
CONFIG_LIB_TFLM=y
|
||||
CONFIG_MODULES_MC_NN_CONTROL=y
|
||||
```
|
||||
|
||||
:::tip
|
||||
The `mc_nn_control` module takes up roughly 50KB, and many of the `default.px4board file` are already close to filling all the flash on their boards. To make room for the neural control module you can remove the include statements for other modules, such as FW, rover, VTOL and UUV.
|
||||
:::
|
||||
|
||||
## Example Module Overview
|
||||
|
||||
The example module replaces the entire controller structure as well as the control allocator, as shown in the diagram below:
|
||||
|
||||
|
||||
|
||||
In the [controller diagram](../flight_stack/controller_diagrams.md) you can see the [uORB message](../middleware/uorb.md) flow.
|
||||
We hook into this flow by subscribing to messages at particular points, using our neural network to calculate outputs, and then publishing them into the next point in the flow.
|
||||
We also need to stop the module publishing the topic to be replaced, which is covered in [Neural Network Module: System Integration](nn_module_utilities.md)
|
||||
|
||||
### Input
|
||||
|
||||
The input can be changed to whatever you want.
|
||||
Set up the input you want to use during training and then provide the same input in PX4.
|
||||
In the Neural Control module the input is an array of 15 numbers, and consists of these values in this order:
|
||||
|
||||
- [3] Local position error. (goal position - current position)
|
||||
- [6] The first 2 rows of a 3 dimensional rotation matrix.
|
||||
- [3] Linear velocity
|
||||
- [3] Angular velocity
|
||||
|
||||
All the input values are collected from uORB topics and transformed into the correct representation in the `PopulateInputTensor()` function.
|
||||
PX4 uses the NED frame representation, while the Aerial Gym Simulator, in which the NN was trained, uses the ENU representation.
|
||||
Therefore two rotation matrices are created in the function and all the inputs are transformed from the NED representation to the ENU one.
|
||||
|
||||
|
||||
|
||||
ENU and NED are just rotation representations, the translational difference is only there so both can be seen in the same figure.
|
||||
|
||||
### Output
|
||||
|
||||
The output consists of 4 values, the motor forces, one for each motor.
|
||||
These are transformed in the `RescaleActions()` function.
|
||||
This is done because PX4 expects normalized motor commands while the Aerial Gym Simulator uses physical values.
|
||||
So the output from the network needs to be normalized before they can be sent to the motors in PX4.
|
||||
|
||||
The commands are published to the [ActuatorMotors](../msg_docs/ActuatorMotors.md) topic.
|
||||
The publishing is handled in `PublishOutput(float* command_actions)` function.
|
||||
|
||||
:::tip
|
||||
If the neural control mode is too aggressive or unresponsive the [MC_NN_THRST_COEF](../advanced_config/parameter_reference.md#MC_NN_THRST_COEF) parameter can be tuned.
|
||||
Decrease it for more thrust.
|
||||
:::
|
||||
|
||||
## Training your own Network
|
||||
|
||||
The network is currently trained for the [X500 V2](../frames_multicopter/holybro_x500v2_pixhawk6c.md).
|
||||
But the controller is somewhat robust, so it could work directly on other platforms, but performing system identification and training a new network is recommended.
|
||||
|
||||
Since the Aerial Gym Simulator is open-source you can download it and train your own networks as long as you have access to an NVIDIA GPU.
|
||||
If you want to train a control network optimized for your platform you can follow the instructions in the [Aerial Gym Documentation](https://ntnu-arl.github.io/aerial_gym_simulator/9_sim2real/).
|
||||
|
||||
You should do one system identification flight for this and get an approximate inertia matrix for your platform.
|
||||
On the `sys-id` flight you need ESC telemetry, you can read more about that in [DSHOT](../peripherals/dshot.md).
|
||||
|
||||
Then do the following steps:
|
||||
|
||||
- Do a hover flight
|
||||
- Read of the logs what RPM is required for the drone to hover.
|
||||
- Use the weight of each motor, length of the motor arms, total weight of the platform with battery to calculate an approximate inertia matrix for the platform.
|
||||
- Insert these values into the Aerial Gym configuration and train your network.
|
||||
- Convert the network as explained in [TFLM](tflm.md).
|
||||
<Redirect to="../neural_networks/mc_neural_network_control" />
|
||||
|
||||
21
docs/en/neural_networks/index.md
Normal file
21
docs/en/neural_networks/index.md
Normal file
@@ -0,0 +1,21 @@
|
||||
# Neural Network Control
|
||||
|
||||
PX4 supports the following mechanisms for using neural networks for multirotor control:
|
||||
|
||||
- [MC Neural Networks Control](../neural_networks/mc_neural_network_control.md)<Badge type="warning" text="Experimental" /> — A generic neural network module that you can modify to use different underlying neural network and training models and compile into the firmware.
|
||||
- [RAPTOR: A Neural Network Module for Adaptive Quadrotor Control](../neural_networks/raptor.md)<Badge type="warning" text="Experimental" /> — An adaptive RL NN module that works well with different Quad configurations without additional training.
|
||||
|
||||
Generally you will select the former if you wish to experiment with custom neural network architectures and train them using PyTorch or TensorFlow, and the latter if you want to use a pre-trained neural-network controller that works out-of-the-box (without training for your particular platform) or if you train your own policies using [RLtools](https://rl.tools).
|
||||
|
||||
Note that both modules are experimental and provided for experimentation.
|
||||
The table below provides more detail on the differences.
|
||||
|
||||
| Use Case | [`mc_raptor`](../neural_networks/raptor.md) | [`mc_nn_control`](../neural_networks/mc_neural_network_control.md) |
|
||||
| ---------------------------------------------------------------- | ------------------------------------------- | ------------------------------------------------------------------ |
|
||||
| Pre-trained policy that adapts to any quadrotor without training | ✓ RAPTOR | ✘ |
|
||||
| Train policy in PyTorch/TF | ✘ | ✓ TF Lite |
|
||||
| Train policy in RLtools | ✓ | ✘ |
|
||||
| Use manual control (remote) with NN policy | ✘ GPS/MoCap | ✓ Manual attitude commands |
|
||||
| Load policy checkpoints from SD card | ✓ Upload via MAVLink FTP | ✘ Compiled into firmware |
|
||||
| Offboard setpoints | ✓ MAVLink | ✘ |
|
||||
| Internal Trajectory Generator | ✓ (Position, Lissajous) | ✘ |
|
||||
119
docs/en/neural_networks/mc_neural_network_control.md
Normal file
119
docs/en/neural_networks/mc_neural_network_control.md
Normal file
@@ -0,0 +1,119 @@
|
||||
# MC Neural Networks Control
|
||||
|
||||
<Badge type="tip" text="PX4 v1.17" /> <Badge type="warning" text="Experimental" />
|
||||
|
||||
::: warning
|
||||
This is an experimental module.
|
||||
Use at your own risk.
|
||||
:::
|
||||
|
||||
The Multicopter Neural Network (NN) module ([mc_nn_control](../modules/modules_controller.md#mc-nn-control)) is an example module that allows you to experiment with using a pre-trained neural network on PX4.
|
||||
It might be used, for example, to experiment with controllers for non-traditional drone morphologies, computer vision tasks, and so on.
|
||||
|
||||
The module integrates a pre-trained neural network based on the [TensorFlow Lite Micro (TFLM)](./tflm.md) module.
|
||||
The module is trained for the [X500 V2](../frames_multicopter/holybro_x500v2_pixhawk6c.md) multicopter frame.
|
||||
While the controller is fairly robust, and might work on other platforms, we recommend [Training your own Network](#training-your-own-network) if you use a different vehicle.
|
||||
Note that after training the network you will need to update and rebuild PX4.
|
||||
|
||||
TLFM is a mature inference library intended for use on embedded devices.
|
||||
It has support for several architectures, so there is a high likelihood that you can build it for the board you want to use.
|
||||
If not, there are other possible NN frameworks, such as [Eigen](https://eigen.tuxfamily.org/index.php?title=Main_Page) and [Executorch](https://pytorch.org/executorch-overview).
|
||||
|
||||
This document explains how you can include the module in your PX4 build, and provides a broad overview of how it works.
|
||||
The other documents in the section provide more information about the integration, allowing you to replace the NN with a version trained on different data, or even to replace the TLFM library altogether.
|
||||
|
||||
If you are looking for more resources to learn about the module, a website has been created with links to a youtube video and a workshop paper. A full master's thesis will be added later. [A Neural Network Mode for PX4 on Embedded Flight Controllers](https://ntnu-arl.github.io/px4-nns/).
|
||||
|
||||
## Neural Network PX4 Firmware
|
||||
|
||||
::: warning
|
||||
This module requires Ubuntu 24.04 or newer (it is not supported in Ubuntu 22.04).
|
||||
:::
|
||||
|
||||
The module has been tested on a number of configurations, which can be build locally using the commands:
|
||||
|
||||
```sh
|
||||
make px4_sitl_neural
|
||||
```
|
||||
|
||||
```sh
|
||||
make px4_fmu-v6c_neural
|
||||
```
|
||||
|
||||
```sh
|
||||
make mro_pixracerpro_neural
|
||||
```
|
||||
|
||||
You can add the module to other board configurations by modifying their `default.px4board file` configuration to include these lines:
|
||||
|
||||
```sh
|
||||
CONFIG_LIB_TFLM=y
|
||||
CONFIG_MODULES_MC_NN_CONTROL=y
|
||||
```
|
||||
|
||||
:::tip
|
||||
The `mc_nn_control` module takes up roughly 50KB, and many of the `default.px4board file` are already close to filling all the flash on their boards. To make room for the neural control module you can remove the include statements for other modules, such as FW, rover, VTOL and UUV.
|
||||
:::
|
||||
|
||||
## Example Module Overview
|
||||
|
||||
The example module replaces the entire controller structure as well as the control allocator, as shown in the diagram below:
|
||||
|
||||
|
||||
|
||||
In the [controller diagram](../flight_stack/controller_diagrams.md) you can see the [uORB message](../middleware/uorb.md) flow.
|
||||
We hook into this flow by subscribing to messages at particular points, using our neural network to calculate outputs, and then publishing them into the next point in the flow.
|
||||
We also need to stop the module publishing the topic to be replaced, which is covered in [Neural Network Module: System Integration](nn_module_utilities.md)
|
||||
|
||||
### Input
|
||||
|
||||
The input can be changed to whatever you want.
|
||||
Set up the input you want to use during training and then provide the same input in PX4.
|
||||
In the Neural Control module the input is an array of 15 numbers, and consists of these values in this order:
|
||||
|
||||
- [3] Local position error. (goal position - current position)
|
||||
- [6] The first 2 rows of a 3 dimensional rotation matrix.
|
||||
- [3] Linear velocity
|
||||
- [3] Angular velocity
|
||||
|
||||
All the input values are collected from uORB topics and transformed into the correct representation in the `PopulateInputTensor()` function.
|
||||
PX4 uses the NED frame representation, while the Aerial Gym Simulator, in which the NN was trained, uses the ENU representation.
|
||||
Therefore two rotation matrices are created in the function and all the inputs are transformed from the NED representation to the ENU one.
|
||||
|
||||
|
||||
|
||||
ENU and NED are just rotation representations, the translational difference is only there so both can be seen in the same figure.
|
||||
|
||||
### Output
|
||||
|
||||
The output consists of 4 values, the motor forces, one for each motor.
|
||||
These are transformed in the `RescaleActions()` function.
|
||||
This is done because PX4 expects normalized motor commands while the Aerial Gym Simulator uses physical values.
|
||||
So the output from the network needs to be normalized before they can be sent to the motors in PX4.
|
||||
|
||||
The commands are published to the [ActuatorMotors](../msg_docs/ActuatorMotors.md) topic.
|
||||
The publishing is handled in `PublishOutput(float* command_actions)` function.
|
||||
|
||||
:::tip
|
||||
If the neural control mode is too aggressive or unresponsive the [MC_NN_THRST_COEF](../advanced_config/parameter_reference.md#MC_NN_THRST_COEF) parameter can be tuned.
|
||||
Decrease it for more thrust.
|
||||
:::
|
||||
|
||||
## Training your own Network
|
||||
|
||||
The network is currently trained for the [X500 V2](../frames_multicopter/holybro_x500v2_pixhawk6c.md).
|
||||
But the controller is somewhat robust, so it could work directly on other platforms, but performing system identification and training a new network is recommended.
|
||||
|
||||
Since the Aerial Gym Simulator is open-source you can download it and train your own networks as long as you have access to an NVIDIA GPU.
|
||||
If you want to train a control network optimized for your platform you can follow the instructions in the [Aerial Gym Documentation](https://ntnu-arl.github.io/aerial_gym_simulator/9_sim2real/).
|
||||
|
||||
You should do one system identification flight for this and get an approximate inertia matrix for your platform.
|
||||
On the `sys-id` flight you need ESC telemetry, you can read more about that in [DSHOT](../peripherals/dshot.md).
|
||||
|
||||
Then do the following steps:
|
||||
|
||||
- Do a hover flight
|
||||
- Read of the logs what RPM is required for the drone to hover.
|
||||
- Use the weight of each motor, length of the motor arms, total weight of the platform with battery to calculate an approximate inertia matrix for the platform.
|
||||
- Insert these values into the Aerial Gym configuration and train your network.
|
||||
- Convert the network as explained in [TFLM](tflm.md).
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
The neural control module ([mc_nn_control](../modules/modules_controller.md#mc-nn-control)) implements an end-to-end controller utilizing neural networks.
|
||||
|
||||
The parts of the module directly concerned with generating the code for the trained neural network and integrating it into the module are covered in [TensorFlow Lite Micro (TFLM)](../advanced/tflm.md).
|
||||
The parts of the module directly concerned with generating the code for the trained neural network and integrating it into the module are covered in [TensorFlow Lite Micro (TFLM)](./tflm.md).
|
||||
This page covers the changes that were made to integrate the module into PX4, both within the module, and in larger system configuration.
|
||||
|
||||
::: tip
|
||||
@@ -75,7 +75,7 @@ Which timing library is included and used is based on wether PX4 is built with N
|
||||
|
||||
## Changing the setpoint
|
||||
|
||||
The module uses the [TrajectorySetpoint](../msg_docs/TrajectorySetpoint.md) message’s position fields to define its target.
|
||||
The module uses the [TrajectorySetpoint](../msg_docs/TrajectorySetpoint.md) message's position fields to define its target.
|
||||
To follow a trajectory, you can send updated setpoints.
|
||||
For an example of how to do this in a PX4 module, see the [mc_nn_testing](https://github.com/SindreMHegre/PX4-Autopilot-public/tree/main/src/modules/mc_nn_testing) module in this fork.
|
||||
Note that this is not included in upstream PX4.
|
||||
221
docs/en/neural_networks/raptor.md
Normal file
221
docs/en/neural_networks/raptor.md
Normal file
@@ -0,0 +1,221 @@
|
||||
# RAPTOR: A Neural Network Module for Adaptive Quadrotor Control
|
||||
|
||||
<Badge type="tip" text="main (planned for PX4 v1.18)" /> <Badge type="info" text="Multicopter" /> <Badge type="warning" text="Experimental" />
|
||||
|
||||
::: warning
|
||||
This is an experimental module.
|
||||
Use at your own risk.
|
||||
:::
|
||||
|
||||
RAPTOR is a tiny reinforcement-learning based neural network module for quadrotor control that can be used to control a wide variety of quadrotors without retuning.
|
||||
|
||||
This topic provides an overview of the fundamental concepts, and explains how you can use the module in simulation and real hardware.
|
||||
|
||||
## Overview
|
||||
|
||||
|
||||
|
||||
RAPTOR is an adaptive policy for end-to-end quadrotor control.
|
||||
It is motivated by the human ability to adapt learned behaviours to similar situations.
|
||||
For example, while humans may initially require many hours of driving experience to be able to smoothly control the car and blend into traffic, when faced with a new vehicle they do not need to re-learn how to drive — they only need to experience a few rough braking/acceleration/steering responses to adjust their previously learned behavior.
|
||||
|
||||
Reinforcement Learning (RL) is a machine learning technique that uses trial and error to learn decision making/control behaviors, which is similar to the way that humans learn to drive.
|
||||
RL is interesting for controlling robots (and particularly UAVs) because it overcomes some fundamental limitations of classic, modular control architectures (information loss at module boundaries, requirement for expert tuning, etc).
|
||||
RL has been very successful in [high-performance quadrotor flight](https://doi.org/10.1038/s41586-023-06419-4), but previous designs have not been particularly adaptable to new frames and vehicle types.
|
||||
|
||||
RAPTOR fills this gap and demonstrates a single, tiny neural-network control policy that can control a wide variety of quadrotors (tested on real quadrotors from 32 g to 2.4 kg).
|
||||
|
||||
For more details please refer to this video:
|
||||
|
||||
<lite-youtube videoid="hVzdWRFTX3k" title="RAPTOR: A Foundation Policy for Quadrotor Control"/>
|
||||
|
||||
The method we developed for training the RAPTOR policy is called Meta-Imitation Learning:
|
||||
|
||||
|
||||
|
||||
You can torture test the RAPTOR policy in your browser at [https://raptor.rl.tools](https://raptor.rl.tools) or in the embedded app here:
|
||||
|
||||
<iframe src="https://rl-tools.github.io/raptor.rl.tools?raptor=false" width="100%" height="1000" style="border: none;"></iframe>
|
||||
|
||||
For more information please refer to the paper at [https://arxiv.org/abs/2509.11481](https://arxiv.org/abs/2509.11481).
|
||||
|
||||
## Structure
|
||||
|
||||
The RAPTOR control policy is an end-to-end policy that takes position, orientation, linear velocity and angular velocity as inputs and outputs motor commands (`actuator_motors`).
|
||||
To integrate it into PX4 we use the external mode registration facilities in PX4 (which also works well for internal modes as demonstrated in `mc_nn_control`).
|
||||
Because of this architecture the `mc_raptor` module is completely decoupled from all other PX4 logic.
|
||||
|
||||
By default, the RAPTOR module expects setpoints via `trajectory_setpoint` messages.
|
||||
If no `trajectory_setpoint` messages are received or if no `trajectory_setpoint` is received within 200 ms, the current position and orientation (with zero velocity) is used as the setpoint.
|
||||
Since feeding setpoints reliably via telemetry is still a challenge, we also implement a simple option to generate internal reference trajectories (controlled through the `MC_RAPTOR_INTREF` parameter) for demonstration and benchmarking purposes.
|
||||
|
||||
## Features
|
||||
|
||||
- Tiny neural network (just 2084 parameters) => minimal CPU usage
|
||||
- Easily maintainable
|
||||
- Simple CMake setup
|
||||
- Self-contained (no interference with other modules)
|
||||
- Single, simple and well-maintained dependency (RLtools)
|
||||
- Loading neural network parameters from SD card
|
||||
- Minimal flash usage (for possible inclusion into default build configurations)
|
||||
- Easy development: Train new neural network and just upload it via MAVLink FTP without requiring to re-flash the firmware
|
||||
- Tested on 10+ different real platforms (including flexible frames, brushed motors)
|
||||
- Actively developed and maintained
|
||||
|
||||
## Usage
|
||||
|
||||
### SITL
|
||||
|
||||
Build PX4 SITL with Raptor, disable QGC requirement, and adjust the `IMU_GYRO_RATEMAX` to match the simulation IMU rate
|
||||
|
||||
```sh
|
||||
make px4_sitl_raptor gz_x500
|
||||
param set NAV_DLL_ACT 0
|
||||
param set COM_DISARM_LAND -1 # When taking off in offboard the landing detector can cause mid-air disarms
|
||||
param set IMU_GYRO_RATEMAX 250 # Just for SITL. Tested with IMU_GYRO_RATEMAX=400 on real FCUs
|
||||
param set MC_RAPTOR_ENABLE 1 # Enable the mc_raptor module
|
||||
param save
|
||||
```
|
||||
|
||||
Upload the RAPTOR checkpoint to the "SD card": Separate terminal
|
||||
|
||||
```bash
|
||||
mavproxy.py --master udp:127.0.0.1:14540
|
||||
ftp mkdir /raptor # for the real FMU use: /fs/microsd/raptor
|
||||
ftp put src/modules/mc_raptor/blob/policy.tar /raptor/policy.tar
|
||||
```
|
||||
|
||||
Restart (<kbd>Ctrl+C</kbd>)
|
||||
|
||||
```sh
|
||||
make px4_sitl_raptor gz_x500
|
||||
commander takeoff
|
||||
commander status
|
||||
```
|
||||
|
||||
Note the external mode ID of `RAPTOR` in the status report
|
||||
|
||||
```sh
|
||||
commander mode ext{RAPTOR_MODE_ID}
|
||||
```
|
||||
|
||||
#### Internal Reference Trajectory Generation
|
||||
|
||||
In our experience, feeding the `trajectory_setpoint` via MAVLink (even via WiFi telemetry) is unreliable.
|
||||
But we do not want to constrain this module to only platforms that have a companion board.
|
||||
|
||||
For this reason we have integrated a simple internal reference trajectory generator for testing and benchmarking purposes.
|
||||
It supports position (constant position and yaw setpoint) as well as configurable [Lissajous trajectories](https://en.wikipedia.org/wiki/Lissajous_curve).
|
||||
|
||||
The Lissajous generator can, for example, generate smooth figure-eight trajectories that contain interesting accelerations for benchmarking and testing purposes.
|
||||
Please refer to the embedded configurator later in this section to explore the Lissajous parameters and view the resulting trajectories.
|
||||
|
||||
To use the internal reference generator, select the mode: `0`: Off/activation position tracking, `1`: Lissajous
|
||||
|
||||
```sh
|
||||
param set MC_RAPTOR_INTREF 1
|
||||
```
|
||||
|
||||
Restart (ctrl+c)
|
||||
|
||||
```sh
|
||||
commander takeoff
|
||||
commander mode ext{RAPTOR_MODE_ID}
|
||||
mc_raptor intref lissajous 0.5 1 0 2 1 1 10 3
|
||||
```
|
||||
|
||||
The trajectory is relative to the position and yaw of the vehicle at the point where the RAPTOR mode is activated (or the position and yaw where the parameters are changed if it is already activated).
|
||||
|
||||
You can adjust the parameters of the trajectory with the following tool.
|
||||
Make sure to copy the generated CLI string at the end:
|
||||
|
||||
<iframe src="https://rl-tools.github.io/mc-raptor-trajectory-tool" width="100%" height="1700" style="border: none;"></iframe>
|
||||
|
||||
### Real-World
|
||||
|
||||
#### Setup
|
||||
|
||||
The `mc_raptor` module has been mostly tested with the Holybro X500 V2 but it should also work out-of-the-box with other platforms (see the [Other Platforms](#other-platforms) section).
|
||||
|
||||
```sh
|
||||
make px4_fmu-v6c_raptor upload
|
||||
```
|
||||
|
||||
We recommend initially testing the RAPTOR mode using a dead man's switch.
|
||||
For this we configure the mode selection to be connected to a push button or a switch with a spring that automatically switches back.
|
||||
In the default position we configure e.g. `Stabilized Mode` and in the pressed configuration we select `External Mode 1` (since the name of the external mode is only transmitted at runtime).
|
||||
This allows to take off manually and then just trigger the RAPTOR mode for a split-second to see how it behaves.
|
||||
In our experiments it has been exceptionally stable (zero crashes) but we still think progressively activating it for longer is the safest way to build confidence.
|
||||
|
||||
::: warning
|
||||
Make sure that your platform uses the standard PX4 quadrotor motor layout:
|
||||
|
||||
1: front-right, 2: back-left, 3: front-left, 4: back-right
|
||||
:::
|
||||
|
||||
##### Other Platforms
|
||||
|
||||
To enable the `mc_raptor` module in other platforms, just add `CONFIG_MODULES_MC_RAPTOR=y` and `CONFIG_LIB_RL_TOOLS=y`
|
||||
|
||||
```diff
|
||||
+++ b/boards/px4/fmu-v6c/raptor.px4board
|
||||
@@ -35,2 +35,3 @@
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
+CONFIG_LIB_RL_TOOLS=y
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
@@ -64,2 +65,3 @@
|
||||
CONFIG_MODULES_MC_POS_CONTROL=y
|
||||
+CONFIG_MODULES_MC_RAPTOR=y
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=y
|
||||
```
|
||||
|
||||
#### Results
|
||||
|
||||
Even though there were moderate winds (~ 5 m/s) during the test, we found good figure-eight tracking performance at velocities up to 12 m/s:
|
||||
|
||||
|
||||
|
||||
We also tested the linear velocity in a straight line and found that the RAPTOR policy can reliably fly at > 17 m/s (the wind direction was orthogonal to the line):
|
||||
|
||||
|
||||
|
||||
### Troubleshooting
|
||||
|
||||
#### Logging
|
||||
|
||||
Use this logging configuration to log all relevant topics at maximum rate:
|
||||
|
||||
```sh
|
||||
cat > logger_topics.txt << EOF
|
||||
raptor_status 0
|
||||
raptor_input 0
|
||||
trajectory_setpoint 0
|
||||
vehicle_local_position 0
|
||||
vehicle_angular_velocity 0
|
||||
vehicle_attitude 0
|
||||
vehicle_status 0
|
||||
actuator_motors 0
|
||||
EOF
|
||||
```
|
||||
|
||||
Use mavproxy FTP to upload it:
|
||||
|
||||
```sh
|
||||
mavproxy.py
|
||||
```
|
||||
|
||||
##### Real
|
||||
|
||||
```sh
|
||||
ftp mkdir /fs/microsd/etc
|
||||
ftp mkdir /fs/microsd/etc/logging
|
||||
ftp put logger_topics.txt /fs/microsd/etc/logging/logger_topics.txt
|
||||
```
|
||||
|
||||
##### SITL
|
||||
|
||||
```sh
|
||||
ftp mkdir etc
|
||||
ftp mkdir logging
|
||||
ftp put logger_topics.txt etc/logging/logger_topics.txt
|
||||
```
|
||||
@@ -1,6 +1,6 @@
|
||||
# TensorFlow Lite Micro (TFLM)
|
||||
|
||||
The PX4 [Multicopter Neural Network](../advanced/neural_networks.md) module ([mc_nn_control](../modules/modules_controller.md#mc-nn-control)) integrates a neural network that uses the [TensorFlow Lite Micro (TFLM)](https://github.com/tensorflow/tflite-micro) inference library.
|
||||
The PX4 [MC Neural Networks Control](../neural_networks/mc_neural_network_control.md) module ([mc_nn_control](../modules/modules_controller.md#mc-nn-control)) integrates a neural network that uses the [TensorFlow Lite Micro (TFLM)](https://github.com/tensorflow/tflite-micro) inference library.
|
||||
|
||||
This is a mature inference library intended for use on embedded devices, and is hence a suitable choice for PX4.
|
||||
|
||||
@@ -68,7 +68,7 @@ The `_input_tensor` is also defined, it is fetched from `_control_interpreter->i
|
||||
|
||||
The `_input_tensor` is filled in the `PopulateInputTensor()` function.
|
||||
`_input_tensor` works by accessing the `->data.f` member array and fill in the required inputs for your network.
|
||||
The inputs used in the control network is covered in [Neural Networks](../advanced/neural_networks.md).
|
||||
The inputs used in the control network is covered in [MC Neural Networks Control](../neural_networks/mc_neural_network_control.md).
|
||||
|
||||
### Outputs
|
||||
|
||||
@@ -257,6 +257,8 @@ set(msg_files
|
||||
versioned/LongitudinalControlConfiguration.msg
|
||||
versioned/ManualControlSetpoint.msg
|
||||
versioned/ModeCompleted.msg
|
||||
versioned/RaptorInput.msg
|
||||
versioned/RaptorStatus.msg
|
||||
versioned/RegisterExtComponentReply.msg
|
||||
versioned/RegisterExtComponentRequest.msg
|
||||
versioned/TrajectorySetpoint.msg
|
||||
|
||||
18
msg/versioned/RaptorInput.msg
Normal file
18
msg/versioned/RaptorInput.msg
Normal file
@@ -0,0 +1,18 @@
|
||||
# Raptor Input
|
||||
|
||||
# The exact inputs to the Raptor foundation policy.
|
||||
# Having access to the exact inputs helps with debugging and post-hoc analysis.
|
||||
|
||||
uint32 MESSAGE_VERSION = 0
|
||||
|
||||
uint64 timestamp # [us] Time since system start
|
||||
uint64 timestamp_sample # [us] Sampling timestamp of the data this control response is based on
|
||||
bool active # Signals if the policy is active (aka publishing actuator_motors)
|
||||
float32[3] position # [m] [@frame FLU] Position of the vehicle_local_position frame
|
||||
float32[4] orientation # [-] Orientation in the vehicle_attitude frame but using the FLU convention as a unit quaternion (w, x, y, z)
|
||||
float32[3] linear_velocity # [m/s] [@frame FLU] Linear velocity in the vehicle_local_position frame
|
||||
float32[3] angular_velocity # [rad/s] [@frame FLU] Angular velocity in the body frame
|
||||
uint8 ACTION_DIM = 4 # Policy output dimensionality (for quadrotors)
|
||||
float32[4] previous_action # [@range -1, 1] Previous action. Motor commands normalized to [-1, 1]
|
||||
|
||||
# TOPICS raptor_input
|
||||
46
msg/versioned/RaptorStatus.msg
Normal file
46
msg/versioned/RaptorStatus.msg
Normal file
@@ -0,0 +1,46 @@
|
||||
# Raptor Status
|
||||
|
||||
# Diagnostic messages for the Raptor foundation policy.
|
||||
# This diagnostic data is useful for debugging (e.g. identifying missing input information).
|
||||
|
||||
uint32 MESSAGE_VERSION = 0
|
||||
|
||||
uint64 timestamp # [us] Time since system start
|
||||
uint64 timestamp_sample # [us] Sampling timestamp of the data this control response is based on
|
||||
bool subscription_update_angular_velocity # [bool] Flag signalling if the vehicle_angular_velocity was updated
|
||||
bool subscription_update_local_position # [bool] Flag signalling if the vehicle_local_position was updated
|
||||
bool subscription_update_attitude # [bool] Flag signalling if the vehicle_attitude was updated
|
||||
bool subscription_update_trajectory_setpoint # [bool] Flag signalling if the trajectory_setpoint was updated
|
||||
bool subscription_update_vehicle_status # [bool] Flag signalling if the vehicle_status was updated
|
||||
|
||||
uint8 exit_reason # [enum] Exit reason identifier. Representing conditions that lead to the Raptor policy not being executed
|
||||
uint8 EXIT_REASON_NONE = 0 # No exit reason => Raptor control step was executed (actuator_motors should have been published)
|
||||
uint8 EXIT_REASON_NO_ANGULAR_VELOCITY_UPDATE = 1 # We synchronize the control onto the input observation with the highest update frequency, which is vehicle_angular_velocity. If there was no update, we do not need to execute the policy again
|
||||
uint8 EXIT_REASON_NOT_ALL_OBSERVATIONS_SET = 2 # We can not execute the policy if not all observations are available
|
||||
uint8 EXIT_REASON_ANGULAR_VELOCITY_STALE = 3 # If OBSERVATION_TIMEOUT_ANGULAR_VELOCITY is exceeded, we treat the vehicle_angular_velocity as stale and can not run the policy
|
||||
uint8 EXIT_REASON_LOCAL_POSITION_STALE = 4 # If OBSERVATION_TIMEOUT_LOCAL_POSITION is exceeded, we treat the vehicle_local_position as stale and can not run the policy
|
||||
uint8 EXIT_REASON_ATTITUDE_STALE = 5 # If OBSERVATION_TIMEOUT_ATTITUDE is exceeded, we treat the vehicle_attitude as stale and can not run the policy
|
||||
uint8 EXIT_REASON_EXECUTOR_STATUS_SOURCE_NOT_CONTROL = 6 # The executor that runs the policy can run in oversampling mode, where it decides if the policy should be ran based on the timestamp and not based on fixed synchronization onto the vehicle_angular_velocity. In this case the executor can decide to skip running the policy if the interval is too small, in which case this flag is set.
|
||||
|
||||
uint32 timestamp_last_vehicle_angular_velocity # [us] Timestamp of the last received vehicle_angular_velocity message
|
||||
uint32 timestamp_last_vehicle_local_position # [us] Timestamp of the last received vehicle_local_position message
|
||||
uint32 timestamp_last_vehicle_attitude # [us] Timestamp of the last received vehicle_attitude message
|
||||
uint32 timestamp_last_trajectory_setpoint # [us] Timestamp of the last received trajectory_setpoint message
|
||||
|
||||
bool vehicle_angular_velocity_stale # [bool] True if vehicle_angular_velocity data is considered stale (exceeded timeout)
|
||||
bool vehicle_local_position_stale # [bool] True if vehicle_local_position data is considered stale (exceeded timeout)
|
||||
bool vehicle_attitude_stale # [bool] True if vehicle_attitude data is considered stale (exceeded timeout)
|
||||
bool trajectory_setpoint_stale # [bool] True if trajectory_setpoint data is considered stale (exceeded timeout)
|
||||
|
||||
bool active # [bool] True if the Raptor policy is currently active (publishing actuator_motors)
|
||||
uint8 substep # [-] The policy is trained at a fixed frequency (e.g. 100 Hz) but we might want to use it for control at higher frequencies (e.g. 400 Hz), which leads to a number of intermediate steps before the actual policy state is advanced (in this case 4 = 400 Hz / 100 Hz). This field provides the current substep (e.g. 0-3).
|
||||
float32 control_interval # [s] Time interval between control updates
|
||||
|
||||
float32 trajectory_setpoint_dt_mean # [us] The average trajectory setpoint arrival time interval (since Raptor mode activation within NUM_TRAJECTORY_SETPOINT_DTS received trajectory_setpoint messages)
|
||||
float32 trajectory_setpoint_dt_max # [us] The max trajectory setpoint arrival time interval (since Raptor mode activation and within NUM_TRAJECTORY_SETPOINT_DTS received trajectory_setpoint messages)
|
||||
float32 trajectory_setpoint_dt_max_since_activation # [us] The max trajectory setpoint arrival time interval (since Raptor mode activation)
|
||||
|
||||
float32[3] internal_reference_position # [m] [@frame FLU] Internal reference position
|
||||
float32[3] internal_reference_linear_velocity # [m/s] [@frame FLU] Internal reference linear velocity
|
||||
|
||||
# TOPICS raptor_status
|
||||
@@ -68,6 +68,7 @@ add_subdirectory(pure_pursuit EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(rate_control EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(rc EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(ringbuffer EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(rl_tools EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(rover_control EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(rtl EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL)
|
||||
|
||||
15
src/lib/rl_tools/CMakeLists.txt
Normal file
15
src/lib/rl_tools/CMakeLists.txt
Normal file
@@ -0,0 +1,15 @@
|
||||
if(CONFIG_LIB_RL_TOOLS)
|
||||
px4_add_git_submodule(TARGET git_rl_tools PATH "rl_tools")
|
||||
add_library(rl_tools INTERFACE)
|
||||
target_include_directories(rl_tools INTERFACE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/rl_tools/include
|
||||
)
|
||||
|
||||
target_compile_features(rl_tools INTERFACE cxx_std_17)
|
||||
|
||||
target_compile_options(rl_tools INTERFACE
|
||||
-Wno-unused-parameter
|
||||
-Wno-unused-variable
|
||||
-Wno-unused-local-typedefs
|
||||
)
|
||||
endif()
|
||||
7
src/lib/rl_tools/Kconfig
Normal file
7
src/lib/rl_tools/Kconfig
Normal file
@@ -0,0 +1,7 @@
|
||||
config LIB_RL_TOOLS
|
||||
bool "RLtools"
|
||||
default n
|
||||
---help---
|
||||
RLtools is a header-only library for reinforcement learning and neural networks.
|
||||
It enables running trained RL policies on embedded devices.
|
||||
This library is used for running RL-based controllers on the PX4 autopilot.
|
||||
1
src/lib/rl_tools/rl_tools
Submodule
1
src/lib/rl_tools/rl_tools
Submodule
Submodule src/lib/rl_tools/rl_tools added at 15940da2a8
3
src/modules/mc_raptor/CHECKLIST.md
Normal file
3
src/modules/mc_raptor/CHECKLIST.md
Normal file
@@ -0,0 +1,3 @@
|
||||
# Checklist
|
||||
1. Check the `REMAP_CRAZYFLIE` flag (this remaps the Crazyflie outputs to the PX4 SIH inputs)
|
||||
2. Check the `CONTROL_MULTIPLE` setting (this controls how much faster the control loop is than the simulation/step frequency during training. This is needed to aggregate e.g. 4 steps of action history into one step of the policy input)
|
||||
21
src/modules/mc_raptor/CMakeLists.txt
Normal file
21
src/modules/mc_raptor/CMakeLists.txt
Normal file
@@ -0,0 +1,21 @@
|
||||
include(px4_add_library)
|
||||
|
||||
px4_add_git_submodule(TARGET git_raptor_blob PATH "blob")
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__mc_raptor
|
||||
MAIN mc_raptor
|
||||
STACK_MAIN 4000
|
||||
SRCS
|
||||
mc_raptor.cpp
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
rl_tools
|
||||
git_raptor_blob
|
||||
EXTERNAL
|
||||
)
|
||||
|
||||
target_compile_features(modules__mc_raptor PRIVATE cxx_std_17)
|
||||
target_compile_options(modules__mc_raptor PRIVATE -Wno-unused-result)
|
||||
12
src/modules/mc_raptor/Kconfig
Normal file
12
src/modules/mc_raptor/Kconfig
Normal file
@@ -0,0 +1,12 @@
|
||||
menuconfig MODULES_MC_RAPTOR
|
||||
bool "mc_raptor"
|
||||
default n
|
||||
---help---
|
||||
Enable support for mc_raptor
|
||||
|
||||
menuconfig USER_MC_RAPTOR
|
||||
bool "mc_raptor running as userspace module"
|
||||
default n
|
||||
depends on BOARD_PROTECTED && MODULES_MC_RAPTOR
|
||||
---help---
|
||||
Put mc_raptor in userspace memory
|
||||
116
src/modules/mc_raptor/README.md
Normal file
116
src/modules/mc_raptor/README.md
Normal file
@@ -0,0 +1,116 @@
|
||||
# RAPTOR
|
||||
|
||||
|
||||
## SITL
|
||||
#### Standalone Usage (Without External Trajectory Setpoint)
|
||||
Build PX4 SITL with Raptor, disable QGC requirement, and adjust the `IMU_GYRO_RATEMAX` to match the simulation IMU rate
|
||||
```bash
|
||||
make px4_sitl_raptor gz_x500
|
||||
param set NAV_DLL_ACT 0
|
||||
param set COM_DISARM_LAND -1 # When taking off in offboard the landing detector can cause mid-air disarms
|
||||
param set IMU_GYRO_RATEMAX 250 # Just for SITL. Tested with IMU_GYRO_RATEMAX=400 on real FCUs
|
||||
param set MC_RAPTOR_ENABLE 1
|
||||
param set MC_RAPTOR_OFFB 0
|
||||
param save
|
||||
```
|
||||
Upload the RAPTOR checkpoint to the "SD card": Separate terminal
|
||||
```bash
|
||||
mavproxy.py --master udp:127.0.0.1:14540
|
||||
ftp mkdir /raptor # for the real FMU use: /fs/microsd/raptor
|
||||
ftp put src/modules/mc_raptor/blob/policy.tar /raptor/policy.tar
|
||||
```
|
||||
restart (ctrl+c)
|
||||
```bash
|
||||
make px4_sitl_raptor gz_x500
|
||||
commander takeoff
|
||||
commander status
|
||||
```
|
||||
|
||||
Note the external mode ID of `RAPTOR` in the status report
|
||||
|
||||
```bash
|
||||
commander mode ext{RAPTOR_MODE_ID}
|
||||
```
|
||||
|
||||
|
||||
#### Usage with External Trajectory Setpoint
|
||||
|
||||
|
||||
Send Lissajous setpoints via Mavlink:
|
||||
```bash
|
||||
pip install px4
|
||||
px4 udp:localhost:14540 track lissajous --A 2.0 --B 1.0 --duration 10 --ramp-duration 5 --takeoff 10.0 --iterations 2
|
||||
```
|
||||
|
||||
|
||||
## SIH
|
||||
```bash
|
||||
make px4_fmu-v6c_raptor upload
|
||||
```
|
||||
In QGroundControl:
|
||||
- Airframes => SIH Quadrotor X
|
||||
- Settings => Comm Links => Disable Pixhawk (disable automatic USB serial connection)
|
||||
```bash
|
||||
mavproxy.py --master /dev/serial/by-id/usb-Auterion_PX4_FMU_v6C.x_0-if00 --out udp:localhost:14550 --out udp:localhost:13337 --out udp:localhost:13338
|
||||
```
|
||||
New terminal (optional):
|
||||
```bash
|
||||
./Tools/mavlink_shell.py udp:localhost:13338
|
||||
```
|
||||
|
||||
```bash
|
||||
param set SIH_IXX 0.005
|
||||
param set SIH_IYY 0.005
|
||||
param set SIH_IZZ 0.010
|
||||
param set IMU_GYRO_RATEMAX 400
|
||||
param save
|
||||
reboot
|
||||
```
|
||||
|
||||
New terminal:
|
||||
```bash
|
||||
./Tools/simulation/jmavsim/jmavsim_run.sh -u -p 13337 -o
|
||||
```
|
||||
|
||||
|
||||
## Real World
|
||||
Using a DroneBridge WiFi telemetry @ 1000000 baud (also set `SER_TEL1_BAUD=1000000`) and maximum packet size = 16. It seems like larger maximum packet sizes can lead to delays in forwarding the `SET_POSITION_TARGET_LOCAL_NED` messages to `trajectory_setpoint`.
|
||||
```bash
|
||||
./Tools/mavlink_shell.py tcp:192.168.2.1:5760
|
||||
```
|
||||
```bash
|
||||
px4 tcp:192.168.2.1:5760 track lissajous --A 0.5 --B 0.5 --duration 10 --ramp-duration 5 --takeoff 1.0 --iterations 2
|
||||
```
|
||||
|
||||
|
||||
## Troubleshooting
|
||||
|
||||
|
||||
```bash
|
||||
cat > logger_topics.txt << EOF
|
||||
raptor_status 0
|
||||
raptor_input 0
|
||||
trajectory_setpoint 0
|
||||
vehicle_local_position 0
|
||||
vehicle_angular_velocity 0
|
||||
vehicle_attitude 0
|
||||
vehicle_status 0
|
||||
actuator_motors 0
|
||||
EOF
|
||||
```
|
||||
```bash
|
||||
mavproxy.py
|
||||
```
|
||||
```bash
|
||||
ftp mkdir /fs/microsd/etc
|
||||
ftp mkdir /fs/microsd/etc/logging
|
||||
ftp put logger_topics.txt /fs/microsd/etc/logging/logger_topics.txt
|
||||
```
|
||||
|
||||
For SITL:
|
||||
|
||||
```bash
|
||||
ftp mkdir etc
|
||||
ftp mkdir logging
|
||||
ftp put logger_topics.txt etc/logging/logger_topics.txt
|
||||
```
|
||||
1
src/modules/mc_raptor/blob
Submodule
1
src/modules/mc_raptor/blob
Submodule
Submodule src/modules/mc_raptor/blob added at 4f31fc9736
1077
src/modules/mc_raptor/mc_raptor.cpp
Normal file
1077
src/modules/mc_raptor/mc_raptor.cpp
Normal file
File diff suppressed because it is too large
Load Diff
283
src/modules/mc_raptor/mc_raptor.hpp
Normal file
283
src/modules/mc_raptor/mc_raptor.hpp
Normal file
@@ -0,0 +1,283 @@
|
||||
#pragma once
|
||||
|
||||
#include "trajectories/lissajous.hpp"
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_odometry.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/actuator_motors.h>
|
||||
#include <uORB/topics/trajectory_setpoint.h>
|
||||
#include <uORB/topics/register_ext_component_request.h>
|
||||
#include <uORB/topics/register_ext_component_reply.h>
|
||||
#include <uORB/topics/unregister_ext_component.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/raptor_status.h>
|
||||
#include <uORB/topics/raptor_input.h>
|
||||
#include <uORB/topics/tune_control.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/arming_check_request.h>
|
||||
#include <uORB/topics/arming_check_reply.h>
|
||||
#undef OK
|
||||
|
||||
#ifdef __PX4_POSIX
|
||||
#include <rl_tools/operations/cpu.h>
|
||||
#else
|
||||
#include <rl_tools/operations/arm.h>
|
||||
#endif
|
||||
|
||||
#include <rl_tools/nn/layers/standardize/operations_generic.h>
|
||||
#include <rl_tools/nn/layers/dense/operations_arm/opt.h>
|
||||
#include <rl_tools/nn/layers/sample_and_squash/operations_generic.h>
|
||||
#include <rl_tools/nn/layers/gru/operations_generic.h>
|
||||
#include <rl_tools/nn_models/mlp/operations_generic.h>
|
||||
#include <rl_tools/nn_models/sequential/operations_generic.h>
|
||||
|
||||
#include <rl_tools/inference/executor/executor.h>
|
||||
#include <rl_tools/inference/applications/l2f/l2f.h>
|
||||
|
||||
#include "blob/policy.h"
|
||||
|
||||
#include <rl_tools/persist/backends/tar/operations_posix.h>
|
||||
#include <rl_tools/nn/optimizers/adam/instance/persist.h>
|
||||
#include <rl_tools/nn/layers/gru/persist.h>
|
||||
#include <rl_tools/nn/layers/dense/persist.h>
|
||||
#include <rl_tools/nn_models/sequential/persist.h>
|
||||
|
||||
namespace rlt = rl_tools;
|
||||
|
||||
#define MC_RAPTOR_POLICY_NAMESPACE rlt::checkpoint::actor
|
||||
#define MC_RAPTOR_EXAMPLE_NAMESPACE rlt::checkpoint::example
|
||||
#define MC_RAPTOR_META_NAMESPACE rlt::checkpoint::meta
|
||||
// #define MC_RAPTOR_EMBED_POLICY // you can use this to directly embed the policy into the firmware instead of loading it from the sd card. To fit into the flash you might need to disable some unnecessary features in the .px4board config.
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class Raptor : public ModuleBase<Raptor>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
Raptor();
|
||||
~Raptor() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
bool init();
|
||||
|
||||
int print_status() override;
|
||||
|
||||
private:
|
||||
#ifdef __PX4_POSIX
|
||||
using DEVICE = rlt::devices::DefaultCPU;
|
||||
#else
|
||||
using DEV_SPEC = rlt::devices::DefaultARMSpecification;
|
||||
using DEVICE = rlt::devices::arm::OPT<DEV_SPEC>;
|
||||
#endif
|
||||
using TI = typename DEVICE::index_t;
|
||||
using RNG = DEVICE::SPEC::RANDOM::ENGINE<>;
|
||||
using T = float;
|
||||
static constexpr uint64_t EXT_COMPONENT_REQUEST_ID = 1337;
|
||||
DEVICE device;
|
||||
RNG rng;
|
||||
hrt_abstime init_time;
|
||||
// node constants
|
||||
static constexpr TI OBSERVATION_TIMEOUT_ANGULAR_VELOCITY = 10 * 1000;
|
||||
static constexpr TI OBSERVATION_TIMEOUT_LOCAL_POSITION = 100 * 1000;
|
||||
static constexpr TI OBSERVATION_TIMEOUT_ATTITUDE = 50 * 1000;
|
||||
static constexpr TI TRAJECTORY_SETPOINT_TIMEOUT = 200 * 1000;
|
||||
static constexpr T RESET_PREVIOUS_ACTION_VALUE = 0; // -1 to 1
|
||||
static constexpr bool ENABLE_CONTROL_FREQUENCY_INFO = false;
|
||||
|
||||
T max_position_error = 0.5;
|
||||
T max_velocity_error = 1.0;
|
||||
|
||||
void Run() override;
|
||||
|
||||
decltype(register_ext_component_reply_s::mode_id) ext_component_mode_id;
|
||||
decltype(register_ext_component_reply_s::arming_check_id) ext_component_arming_check_id;
|
||||
|
||||
enum class FlightModeState : TI {
|
||||
UNREGISTERED = 0,
|
||||
REGISTERED = 1,
|
||||
CONFIGURED = 2
|
||||
};
|
||||
FlightModeState flightmode_state = FlightModeState::UNREGISTERED;
|
||||
bool can_arm = false;
|
||||
void updateArmingCheckReply();
|
||||
|
||||
// node state
|
||||
vehicle_local_position_s _vehicle_local_position{};
|
||||
vehicle_angular_velocity_s _vehicle_angular_velocity{};
|
||||
vehicle_attitude_s _vehicle_attitude{};
|
||||
vehicle_status_s _vehicle_status{};
|
||||
trajectory_setpoint_s _trajectory_setpoint{};
|
||||
hrt_abstime timestamp_last_local_position, timestamp_last_angular_velocity, timestamp_last_attitude, timestamp_last_trajectory_setpoint,
|
||||
timestamp_last_manual_control_input, timestamp_last_vehicle_status;
|
||||
bool timestamp_last_local_position_set = false, timestamp_last_angular_velocity_set = false, timestamp_last_attitude_set = false,
|
||||
timestamp_last_trajectory_setpoint_set = false, timestamp_last_manual_control_input_set = false, timestamp_last_vehicle_status_set = false;
|
||||
bool timeout_message_sent = false;
|
||||
bool previous_trajectory_setpoint_stale = false;
|
||||
bool previous_active = false;
|
||||
|
||||
T position[3];
|
||||
T linear_velocity[3];
|
||||
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _register_ext_component_reply_sub{ORB_ID(register_ext_component_reply)};
|
||||
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
|
||||
uORB::Subscription _arming_check_request_sub{ORB_ID(arming_check_request)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
|
||||
uORB::Publication<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
|
||||
uORB::Publication<raptor_status_s> _raptor_status_pub{ORB_ID(raptor_status)};
|
||||
uORB::Publication<raptor_input_s> _raptor_input_pub{ORB_ID(raptor_input)};
|
||||
uORB::Publication<tune_control_s> _tune_control_pub{ORB_ID(tune_control)};
|
||||
uORB::Publication<register_ext_component_request_s> _register_ext_component_request_pub{ORB_ID(register_ext_component_request)};
|
||||
uORB::Publication<unregister_ext_component_s> _unregister_ext_component_pub{ORB_ID(unregister_ext_component)};
|
||||
uORB::Publication<vehicle_control_mode_s> _config_control_setpoints_pub{ORB_ID(config_control_setpoints)};
|
||||
uORB::Publication<arming_check_reply_s> _arming_check_reply_pub{ORB_ID(arming_check_reply)};
|
||||
// Performance (perf) counters
|
||||
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
|
||||
perf_counter_t _loop_interval_policy_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval_policy")};
|
||||
|
||||
struct EXECUTOR_CONFIG {
|
||||
static constexpr TI ACTION_HISTORY_LENGTH = 1;
|
||||
static constexpr TI CONTROL_INTERVAL_INTERMEDIATE_NS = 2.5 * 1000 * 1000; // Inference is at 500hz
|
||||
static constexpr TI CONTROL_INTERVAL_NATIVE_NS = 10 * 1000 * 1000; // Training is 100hz
|
||||
static constexpr TI TIMING_STATS_NUM_STEPS = 100;
|
||||
static constexpr bool FORCE_SYNC_INTERMEDIATE = true;
|
||||
static constexpr bool FORCE_SYNC_NATIVE_RUNTIME = true;
|
||||
static constexpr TI FORCE_SYNC_NATIVE = 8;
|
||||
static constexpr bool DYNAMIC_ALLOCATION = false;
|
||||
|
||||
using ACTOR_TYPE_ORIGINAL = MC_RAPTOR_POLICY_NAMESPACE ::TYPE;
|
||||
using POLICY_TEST = MC_RAPTOR_POLICY_NAMESPACE ::TYPE::template CHANGE_BATCH_SIZE<TI, 1>::template CHANGE_SEQUENCE_LENGTH<TI, 1>;
|
||||
using POLICY_BATCH_SIZE = ACTOR_TYPE_ORIGINAL::template CHANGE_BATCH_SIZE<TI, 1>;
|
||||
#ifdef MC_RAPTOR_EMBED_POLICY
|
||||
using POLICY = POLICY_BATCH_SIZE;
|
||||
#else
|
||||
using POLICY = POLICY_BATCH_SIZE::template CHANGE_CAPABILITY<rlt::nn::capability::Forward<false, false>>;
|
||||
#endif
|
||||
using TYPE_POLICY = typename POLICY::TYPE_POLICY;
|
||||
|
||||
#if defined(__PX4_POSIX)
|
||||
// Relax warning levels for Gazebo sitl. Because Gazebo SITL runs at 250Hz IMU rate, it is not a clean multiple of the training frequency (100hz), hence if the thresholds are set too strict, warnings will be triggered all the time. Generally, Raptor is quite robuts agains control frequency deviations.
|
||||
struct WARNING_LEVELS: rlt::inference::executor::WarningLevelsDefault<TYPE_POLICY> {
|
||||
using T = typename TYPE_POLICY::DEFAULT;
|
||||
static constexpr T INTERMEDIATE_TIMING_JITTER_HIGH_THRESHOLD = 2.0;
|
||||
static constexpr T INTERMEDIATE_TIMING_JITTER_LOW_THRESHOLD = 0.5;
|
||||
static constexpr T INTERMEDIATE_TIMING_BIAS_HIGH_THRESHOLD = 2.0;
|
||||
static constexpr T INTERMEDIATE_TIMING_BIAS_LOW_THRESHOLD = 0.5;
|
||||
static constexpr T NATIVE_TIMING_JITTER_HIGH_THRESHOLD = 2.0;
|
||||
static constexpr T NATIVE_TIMING_JITTER_LOW_THRESHOLD = 0.5;
|
||||
static constexpr T NATIVE_TIMING_BIAS_HIGH_THRESHOLD = 2.0;
|
||||
static constexpr T NATIVE_TIMING_BIAS_LOW_THRESHOLD = 0.5;
|
||||
};
|
||||
#else
|
||||
struct WARNING_LEVELS: rlt::inference::executor::WarningLevelsDefault<TYPE_POLICY> {
|
||||
using T = typename TYPE_POLICY::DEFAULT;
|
||||
static constexpr T NATIVE_TIMING_JITTER_HIGH_THRESHOLD = 1.5;
|
||||
static constexpr T NATIVE_TIMING_JITTER_LOW_THRESHOLD = 0.5;
|
||||
};
|
||||
#endif
|
||||
using TIMESTAMP = hrt_abstime;
|
||||
static constexpr TI OUTPUT_DIM = 4;
|
||||
static constexpr TI TEST_SEQUENCE_LENGTH_ACTUAL = 5;
|
||||
static constexpr TI TEST_BATCH_SIZE_ACTUAL = 2;
|
||||
|
||||
using EXECUTOR_SPEC =
|
||||
rl_tools::inference::applications::l2f::Specification<TYPE_POLICY, TI, TIMESTAMP, ACTION_HISTORY_LENGTH, OUTPUT_DIM, POLICY, CONTROL_INTERVAL_INTERMEDIATE_NS, CONTROL_INTERVAL_NATIVE_NS, FORCE_SYNC_INTERMEDIATE, FORCE_SYNC_NATIVE, FORCE_SYNC_NATIVE_RUNTIME, WARNING_LEVELS, DYNAMIC_ALLOCATION>;
|
||||
using EXECUTOR_STATUS = rlt::inference::executor::Status<EXECUTOR_SPEC::EXECUTOR_SPEC>;
|
||||
};
|
||||
using EXECUTOR_SPEC = EXECUTOR_CONFIG::EXECUTOR_SPEC;
|
||||
rl_tools::inference::applications::L2F<EXECUTOR_SPEC> executor;
|
||||
#ifdef MC_RAPTOR_EMBED_POLICY
|
||||
const decltype(MC_RAPTOR_POLICY_NAMESPACE ::module) &policy = MC_RAPTOR_POLICY_NAMESPACE::module;
|
||||
#else
|
||||
EXECUTOR_CONFIG::POLICY policy;
|
||||
#endif
|
||||
static constexpr TI CHECKPOINT_NAME_LENGTH = 100;
|
||||
char checkpoint_name[CHECKPOINT_NAME_LENGTH] = "n/a";
|
||||
|
||||
#ifdef MC_RAPTOR_EMBED_POLICY
|
||||
bool test_policy();
|
||||
#else
|
||||
bool test_policy(FILE *f, TI input_offset, TI output_offset);
|
||||
#endif
|
||||
|
||||
void reset();
|
||||
void observe(rl_tools::inference::applications::l2f::Observation<EXECUTOR_SPEC> &observation);
|
||||
|
||||
static constexpr bool REMAP_FROM_CRAZYFLIE =
|
||||
true; // Policy (Crazyflie assignment) => Quadrotor (PX4 Quadrotor X assignment) PX4 SIH assumes the Quadrotor X configuration, which assumes different rotor positions than the crazyflie mapping (from crazyflie outputs to PX4): 1=>1, 2=>4, 3=>2, 4=>3
|
||||
// controller state
|
||||
|
||||
// messaging state
|
||||
static constexpr TI POLICY_INTERVAL_WARNING_THRESHOLD = 100; // us
|
||||
static constexpr TI POLICY_FREQUENCY_CHECK_INTERVAL = 1000 * 1000; // 1s
|
||||
static constexpr TI POLICY_FREQUENCY_INFO_INTERVAL = 10; // 10 x POLICY_FREQUENCY_CHECK_INTERVAL = 10x
|
||||
static constexpr TI POLICY_CONTROL_FREQUENCY_TRAINING = 100;
|
||||
|
||||
TI num_statii;
|
||||
TI num_healthy_executor_statii_intermediate, num_non_healthy_executor_statii_intermediate, num_healthy_executor_statii_native,
|
||||
num_non_healthy_executor_statii_native;
|
||||
EXECUTOR_CONFIG::EXECUTOR_STATUS last_intermediate_status, last_native_status;
|
||||
bool last_intermediate_status_set, last_native_status_set;
|
||||
|
||||
TI policy_frequency_check_counter;
|
||||
hrt_abstime timestamp_last_policy_frequency_check;
|
||||
bool timestamp_last_policy_frequency_check_set = false;
|
||||
|
||||
static constexpr TI NUM_TRAJECTORY_SETPOINT_DTS = 100;
|
||||
int32_t trajectory_setpoint_dts[NUM_TRAJECTORY_SETPOINT_DTS];
|
||||
TI trajectory_setpoint_dt_index = 0;
|
||||
TI trajectory_setpoint_dt_max_since_reset = 0;
|
||||
bool trajectory_setpoint_dts_full = false;
|
||||
|
||||
static constexpr TI TRAJECTORY_SETPOINT_INVALID_COUNT_WARNING_INTERVAL = 100;
|
||||
TI trajectory_setpoint_invalid_count = 0;
|
||||
|
||||
float previous_action[EXECUTOR_SPEC::OUTPUT_DIM];
|
||||
bool use_internal_reference = false;
|
||||
bool internal_reference_params_changed = false;
|
||||
T internal_reference_activation_position[3];
|
||||
T internal_reference_activation_orientation[4];
|
||||
hrt_abstime internal_reference_activation_time;
|
||||
enum class InternalReference : TI { // make sure this corresponds to the enum values for MC_RAPTOR_INTREF in module.yaml
|
||||
NONE = 0,
|
||||
LISSAJOUS = 1
|
||||
};
|
||||
InternalReference internal_reference = InternalReference::NONE;
|
||||
LissajousParameters lissajous_params{}; // Set via 'mc_raptor intref lissajous ...' command
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax,
|
||||
(ParamBool<px4::params::MC_RAPTOR_OFFB>) _param_mc_raptor_offboard,
|
||||
(ParamInt<px4::params::MC_RAPTOR_INTREF>) _param_mc_raptor_intref
|
||||
)
|
||||
|
||||
|
||||
};
|
||||
43
src/modules/mc_raptor/module.yaml
Normal file
43
src/modules/mc_raptor/module.yaml
Normal file
@@ -0,0 +1,43 @@
|
||||
module_name: mc_raptor
|
||||
parameters:
|
||||
- group: Multicopter Raptor
|
||||
definitions:
|
||||
MC_RAPTOR_ENABLE:
|
||||
description:
|
||||
short: Enable Raptor flight mode
|
||||
long: |
|
||||
When enabled, the Raptor flight mode will be available. Please set MC_RAPTOR_OFFB according to your use case.
|
||||
type: boolean
|
||||
default: false
|
||||
category: System
|
||||
MC_RAPTOR_VERBOS:
|
||||
description:
|
||||
short: Enable verbose output
|
||||
long: |
|
||||
When enabled, the Raptor flight mode will print verbose output to the console.
|
||||
type: boolean
|
||||
default: false
|
||||
category: System
|
||||
|
||||
MC_RAPTOR_OFFB:
|
||||
description:
|
||||
short: Enable Offboard mode replacement
|
||||
long: |
|
||||
When enabled, the Raptor mode will replace the Offboard mode.
|
||||
If disabled, the Raptor mode will be available as a separate external mode. In the latter case, Raptor will just hold the position, without requiring external setpoints. When Raptor replaces the Offboard mode, it requires external setpoints to be activated.
|
||||
type: boolean
|
||||
default: false
|
||||
category: System
|
||||
|
||||
MC_RAPTOR_INTREF:
|
||||
description:
|
||||
short: Use internal reference instead of trajectory_setpoint
|
||||
long: |
|
||||
When enabled, instead of using the trajectory_setpoint, the position and yaw of the vehicle at the point when the Raptor mode is activated will be used as reference.
|
||||
Use 'mc_raptor intref lissajous <A> <B> <C> <fa> <fb> <fc> <duration> <ramp>' to configure the trajectory.
|
||||
type: enum
|
||||
values:
|
||||
0: None
|
||||
1: Lissajous
|
||||
default: 0
|
||||
category: System
|
||||
38
src/modules/mc_raptor/trajectories/lissajous.hpp
Normal file
38
src/modules/mc_raptor/trajectories/lissajous.hpp
Normal file
@@ -0,0 +1,38 @@
|
||||
#pragma once
|
||||
|
||||
#include "trajectory.hpp"
|
||||
#include <math.h>
|
||||
|
||||
struct LissajousParameters {
|
||||
float A = 0.5f; // amplitude a
|
||||
float B = 1.0f; // amplitude b
|
||||
float C = 0.0f; // amplitude c
|
||||
float a = 2.0f; // frequency a
|
||||
float b = 1.0f; // frequency b
|
||||
float c = 1.0f; // frequency c
|
||||
float duration = 10.0f;
|
||||
float ramp_duration = 3.0f;
|
||||
};
|
||||
|
||||
inline Setpoint lissajous(float time, const LissajousParameters ¶ms)
|
||||
{
|
||||
float time_velocity = (params.ramp_duration > 0.0f)
|
||||
? fminf(time, params.ramp_duration) / params.ramp_duration
|
||||
: 1.0f;
|
||||
|
||||
float ramp_time = time_velocity * fminf(time, params.ramp_duration) / 2.0f;
|
||||
float progress = (ramp_time + fmaxf(0.0f, time - params.ramp_duration)) * 2.0f * static_cast<float>(M_PI) / params.duration;
|
||||
float d_progress = 2.0f * static_cast<float>(M_PI) * time_velocity / params.duration;
|
||||
|
||||
Setpoint setpoint{};
|
||||
setpoint.position[0] = params.A * sinf(params.a * progress);
|
||||
setpoint.position[1] = params.B * sinf(params.b * progress);
|
||||
setpoint.position[2] = params.C * sinf(params.c * progress);
|
||||
setpoint.yaw = 0.0f;
|
||||
setpoint.linear_velocity[0] = params.A * cosf(params.a * progress) * params.a * d_progress;
|
||||
setpoint.linear_velocity[1] = params.B * cosf(params.b * progress) * params.b * d_progress;
|
||||
setpoint.linear_velocity[2] = params.C * cosf(params.c * progress) * params.c * d_progress;
|
||||
setpoint.yaw_rate = 0.0f;
|
||||
|
||||
return setpoint;
|
||||
}
|
||||
6
src/modules/mc_raptor/trajectories/trajectory.hpp
Normal file
6
src/modules/mc_raptor/trajectories/trajectory.hpp
Normal file
@@ -0,0 +1,6 @@
|
||||
struct Setpoint {
|
||||
float position[3];
|
||||
float yaw;
|
||||
float linear_velocity[3];
|
||||
float yaw_rate;
|
||||
};
|
||||
Reference in New Issue
Block a user