diff --git a/.gitmodules b/.gitmodules index 4061b65c11..86cb85c961 100644 --- a/.gitmodules +++ b/.gitmodules @@ -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 diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index 4b2c95eb34..461ce2d95b 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index adff4e963f..24a3f81ed7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -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 diff --git a/boards/px4/fmu-v6c/raptor.px4board b/boards/px4/fmu-v6c/raptor.px4board new file mode 100644 index 0000000000..be07cafcbe --- /dev/null +++ b/boards/px4/fmu-v6c/raptor.px4board @@ -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 diff --git a/boards/px4/sitl/raptor.px4board b/boards/px4/sitl/raptor.px4board new file mode 100644 index 0000000000..b00735efc2 --- /dev/null +++ b/boards/px4/sitl/raptor.px4board @@ -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 diff --git a/docs/assets/advanced/neural_networks/raptor/method.jpg b/docs/assets/advanced/neural_networks/raptor/method.jpg new file mode 100644 index 0000000000..935da7683d Binary files /dev/null and b/docs/assets/advanced/neural_networks/raptor/method.jpg differ diff --git a/docs/assets/advanced/neural_networks/raptor/results_figure_eight.svg b/docs/assets/advanced/neural_networks/raptor/results_figure_eight.svg new file mode 100644 index 0000000000..81d3a51286 --- /dev/null +++ b/docs/assets/advanced/neural_networks/raptor/results_figure_eight.svg @@ -0,0 +1 @@ + diff --git a/docs/assets/advanced/neural_networks/raptor/results_line.svg b/docs/assets/advanced/neural_networks/raptor/results_line.svg new file mode 100644 index 0000000000..0d7d142013 --- /dev/null +++ b/docs/assets/advanced/neural_networks/raptor/results_line.svg @@ -0,0 +1 @@ + diff --git a/docs/assets/advanced/neural_networks/raptor/visual_abstract.jpg b/docs/assets/advanced/neural_networks/raptor/visual_abstract.jpg new file mode 100644 index 0000000000..40ba52877a Binary files /dev/null and b/docs/assets/advanced/neural_networks/raptor/visual_abstract.jpg differ diff --git a/docs/en/SUMMARY.md b/docs/en/SUMMARY.md index 7b9cf2b34c..5cd8646f75 100644 --- a/docs/en/SUMMARY.md +++ b/docs/en/SUMMARY.md @@ -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) diff --git a/docs/en/advanced/neural_networks.md b/docs/en/advanced/neural_networks.md index 454d60a16a..039259fd5f 100644 --- a/docs/en/advanced/neural_networks.md +++ b/docs/en/advanced/neural_networks.md @@ -1,119 +1 @@ -# Neural Networks - - - -::: 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: - -![neural_control](../../assets/advanced/neural_control.png) - -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-NED](../../assets/advanced/ENU-NED.png) - -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). + diff --git a/docs/en/neural_networks/index.md b/docs/en/neural_networks/index.md new file mode 100644 index 0000000000..4379f079f9 --- /dev/null +++ b/docs/en/neural_networks/index.md @@ -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) — 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) — 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) | ✘ | diff --git a/docs/en/neural_networks/mc_neural_network_control.md b/docs/en/neural_networks/mc_neural_network_control.md new file mode 100644 index 0000000000..6f83a67cb3 --- /dev/null +++ b/docs/en/neural_networks/mc_neural_network_control.md @@ -0,0 +1,119 @@ +# MC Neural Networks Control + + + +::: 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: + +![neural_control](../../assets/advanced/neural_control.png) + +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-NED](../../assets/advanced/ENU-NED.png) + +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). diff --git a/docs/en/advanced/nn_module_utilities.md b/docs/en/neural_networks/nn_module_utilities.md similarity index 97% rename from docs/en/advanced/nn_module_utilities.md rename to docs/en/neural_networks/nn_module_utilities.md index d00f8aff63..b1df217ded 100644 --- a/docs/en/advanced/nn_module_utilities.md +++ b/docs/en/neural_networks/nn_module_utilities.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. diff --git a/docs/en/neural_networks/raptor.md b/docs/en/neural_networks/raptor.md new file mode 100644 index 0000000000..3867908fd8 --- /dev/null +++ b/docs/en/neural_networks/raptor.md @@ -0,0 +1,221 @@ +# RAPTOR: A Neural Network Module for Adaptive Quadrotor Control + + + +::: 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 + +![Visual Abstract](../../assets/advanced/neural_networks/raptor/visual_abstract.jpg) + +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: + + + +The method we developed for training the RAPTOR policy is called Meta-Imitation Learning: + +![Diagram showing the Method Overview](../../assets/advanced/neural_networks/raptor/method.jpg) + +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: + + + +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 (Ctrl+C) + +```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: + + + +### 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: + +![Lissajous](../../assets/advanced/neural_networks/raptor/results_figure_eight.svg) + +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): + +![Linear Oscillation](../../assets/advanced/neural_networks/raptor/results_line.svg) + +### 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 +``` diff --git a/docs/en/advanced/tflm.md b/docs/en/neural_networks/tflm.md similarity index 90% rename from docs/en/advanced/tflm.md rename to docs/en/neural_networks/tflm.md index 2c30ac5216..57b2847867 100644 --- a/docs/en/advanced/tflm.md +++ b/docs/en/neural_networks/tflm.md @@ -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 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 29b538401a..5d1fb157b6 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -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 diff --git a/msg/versioned/RaptorInput.msg b/msg/versioned/RaptorInput.msg new file mode 100644 index 0000000000..4193397137 --- /dev/null +++ b/msg/versioned/RaptorInput.msg @@ -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 diff --git a/msg/versioned/RaptorStatus.msg b/msg/versioned/RaptorStatus.msg new file mode 100644 index 0000000000..25801a0020 --- /dev/null +++ b/msg/versioned/RaptorStatus.msg @@ -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 diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index f732ddbef1..33d58ab640 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -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) diff --git a/src/lib/rl_tools/CMakeLists.txt b/src/lib/rl_tools/CMakeLists.txt new file mode 100644 index 0000000000..83402f2c45 --- /dev/null +++ b/src/lib/rl_tools/CMakeLists.txt @@ -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() diff --git a/src/lib/rl_tools/Kconfig b/src/lib/rl_tools/Kconfig new file mode 100644 index 0000000000..8014b02bcf --- /dev/null +++ b/src/lib/rl_tools/Kconfig @@ -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. diff --git a/src/lib/rl_tools/rl_tools b/src/lib/rl_tools/rl_tools new file mode 160000 index 0000000000..15940da2a8 --- /dev/null +++ b/src/lib/rl_tools/rl_tools @@ -0,0 +1 @@ +Subproject commit 15940da2a8334d7532c4a5650ee4e09526206414 diff --git a/src/modules/mc_raptor/CHECKLIST.md b/src/modules/mc_raptor/CHECKLIST.md new file mode 100644 index 0000000000..ce2ca6be47 --- /dev/null +++ b/src/modules/mc_raptor/CHECKLIST.md @@ -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) diff --git a/src/modules/mc_raptor/CMakeLists.txt b/src/modules/mc_raptor/CMakeLists.txt new file mode 100644 index 0000000000..5cde791827 --- /dev/null +++ b/src/modules/mc_raptor/CMakeLists.txt @@ -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) diff --git a/src/modules/mc_raptor/Kconfig b/src/modules/mc_raptor/Kconfig new file mode 100644 index 0000000000..a99c05db70 --- /dev/null +++ b/src/modules/mc_raptor/Kconfig @@ -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 diff --git a/src/modules/mc_raptor/README.md b/src/modules/mc_raptor/README.md new file mode 100644 index 0000000000..753187b43e --- /dev/null +++ b/src/modules/mc_raptor/README.md @@ -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 +``` diff --git a/src/modules/mc_raptor/blob b/src/modules/mc_raptor/blob new file mode 160000 index 0000000000..4f31fc9736 --- /dev/null +++ b/src/modules/mc_raptor/blob @@ -0,0 +1 @@ +Subproject commit 4f31fc97366487e460e44a43054756a085ab6cb7 diff --git a/src/modules/mc_raptor/mc_raptor.cpp b/src/modules/mc_raptor/mc_raptor.cpp new file mode 100644 index 0000000000..4d3f0d486c --- /dev/null +++ b/src/modules/mc_raptor/mc_raptor.cpp @@ -0,0 +1,1077 @@ +#include "mc_raptor.hpp" +#undef OK + +#include +#include + +#include + +Raptor::Raptor(): ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + // node state + timestamp_last_angular_velocity_set = false; + timestamp_last_local_position_set = false; + timestamp_last_attitude_set = false; + timestamp_last_trajectory_setpoint_set = false; + timestamp_last_vehicle_status_set = false; + previous_trajectory_setpoint_stale = false; + previous_active = false; + timeout_message_sent = false; + timestamp_last_policy_frequency_check_set = false; + last_intermediate_status_set = false; + last_native_status_set = false; + policy_frequency_check_counter = 0; + flightmode_state = FlightModeState::UNREGISTERED; + can_arm = false; + trajectory_setpoint_dt_index = 0; + trajectory_setpoint_dts_full = false; + trajectory_setpoint_invalid_count = 0; + trajectory_setpoint_dt_max_since_reset = 0; + internal_reference_activation_position[0] = 0.0f; + internal_reference_activation_position[1] = 0.0f; + internal_reference_activation_position[2] = 0.0f; + internal_reference_params_changed = false; + + _actuator_motors_pub.advertise(); + _tune_control_pub.advertise(); +} +void Raptor::reset() +{ + + trajectory_setpoint_dt_index = 0; + trajectory_setpoint_dts_full = false; + trajectory_setpoint_invalid_count = 0; + trajectory_setpoint_dt_max_since_reset = 0; + timestamp_last_trajectory_setpoint_set = false; + + + for (TI action_i = 0; action_i < EXECUTOR_SPEC::OUTPUT_DIM; action_i++) { + this->previous_action[action_i] = RESET_PREVIOUS_ACTION_VALUE; + } + + rlt::reset(device, executor, policy, rng); +} + +Raptor::~Raptor() +{ + perf_free(_loop_perf); + perf_free(_loop_interval_perf); +} + +#ifdef MC_RAPTOR_EMBED_POLICY +bool Raptor::test_policy() +{ +#else +bool Raptor::test_policy(FILE *f, TI input_offset, TI output_offset) +{ +#endif + using namespace rl_tools::inference::applications::l2f; +#ifndef RL_TOOLS_DISABLE_TEST + // This tests the policy using a known input output pair that has been saved into the policy checkpoint to verify that it has been loaded correctly + using POLICY = EXECUTOR_CONFIG::POLICY_TEST; + POLICY::template Buffer buffers_test; + POLICY::State policy_state_test; + rl_tools::Tensor, false>> + test_output; + rl_tools::Mode> mode; + using EXAMPLE_INPUT_SPEC = MC_RAPTOR_EXAMPLE_NAMESPACE::input::SPEC; + using EXAMPLE_OUTPUT_SPEC = MC_RAPTOR_EXAMPLE_NAMESPACE::output::SPEC; + float acc = 0; + uint64_t num_values = 0; + rl_tools::inference::applications::l2f::Action action; + + for (TI batch_i = 0; batch_i < EXECUTOR_CONFIG::TEST_BATCH_SIZE_ACTUAL; batch_i++) { + rl_tools::reset(device, policy, policy_state_test, rng); + + for (TI step_i = 0; step_i < EXECUTOR_CONFIG::TEST_SEQUENCE_LENGTH_ACTUAL; step_i++) { +#ifdef MC_RAPTOR_EMBED_POLICY + const auto step_input = rl_tools::view(device, MC_RAPTOR_EXAMPLE_NAMESPACE::input::container, step_i); + const auto batch_input = rl_tools::view_range(device, step_input, batch_i, rlt::tensor::ViewSpec<0, 1> {}); + const auto step_output_target = rl_tools::view(device, MC_RAPTOR_EXAMPLE_NAMESPACE::output::container, step_i); + const auto batch_output_target = rl_tools::view_range(device, step_output_target, batch_i, rlt::tensor::ViewSpec<0, 1> {}); +#else + rl_tools::Tensor, false>> + batch_input; + rl_tools::Tensor, false>> + batch_output_target; + fseek(f, input_offset + (step_i * EXAMPLE_INPUT_SPEC::STRIDE::FIRST + batch_i * EXAMPLE_INPUT_SPEC::STRIDE::template GET<1>)*sizeof( + EXAMPLE_INPUT_SPEC::T), SEEK_SET); + fread(batch_input._data, sizeof(EXAMPLE_INPUT_SPEC::T), EXAMPLE_INPUT_SPEC::SHAPE::LAST, f); + fseek(f, output_offset + (step_i * EXAMPLE_OUTPUT_SPEC::STRIDE::FIRST + batch_i * EXAMPLE_OUTPUT_SPEC::STRIDE::template GET<1>)*sizeof( + EXAMPLE_OUTPUT_SPEC::T), SEEK_SET); + fread(batch_output_target._data, sizeof(EXAMPLE_OUTPUT_SPEC::T), EXAMPLE_OUTPUT_SPEC::SHAPE::LAST, f); +#endif + rl_tools::utils::assert_exit(device, !rl_tools::is_nan(device, batch_input), "input is nan"); + rl_tools::evaluate_step(device, policy, batch_input, policy_state_test, test_output, buffers_test, rng, mode); + rl_tools::utils::assert_exit(device, !rl_tools::is_nan(device, test_output), "output is nan"); + + for (TI action_i = 0; action_i < EXECUTOR_CONFIG::OUTPUT_DIM; action_i++) { + acc += rl_tools::math::abs(device.math, rl_tools::get(device, test_output, 0, action_i) - rl_tools::get(device, batch_output_target, 0, + action_i)); + num_values += 1; + rl_tools::utils::assert_exit(device, !rl_tools::math::is_nan(device.math, acc), "output is nan"); + + if (batch_i == 0 && step_i == EXECUTOR_CONFIG::TEST_SEQUENCE_LENGTH_ACTUAL - 1) { + action.action[action_i] = rl_tools::get(device, test_output, 0, action_i); + } + } + } + } + + float abs_diff = acc / num_values; + PX4_INFO("Checkpoint test diff: %f", (double)abs_diff); + + for (TI output_i = 0; output_i < EXECUTOR_CONFIG::OUTPUT_DIM; output_i++) { + PX4_INFO("output[%d]: %f", (int)output_i, (double)action.action[output_i]); + } + + constexpr float EPSILON = 1e-5; + + bool healthy = abs_diff < EPSILON; + + if (!healthy) { + PX4_ERR("Checkpoint test failed with diff %.10f", (double)abs_diff); + return false; + + } else { + PX4_INFO("Checkpoint test passed with diff %.10f", (double)abs_diff); + return true; + } + +#else + return 0; +#endif +} + +bool Raptor::init() +{ + this->init_time = hrt_absolute_time(); + + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("vehicle_angular_velocity_sub callback registration failed"); + return false; + } + +#ifndef MC_RAPTOR_EMBED_POLICY + const char *path = PX4_STORAGEDIR "/raptor/policy.tar"; + + struct stat st; + bool file_exists = (stat(path, &st) == 0); + + if (file_exists) { + PX4_INFO("Policy checkpoint %s exists", path); + FILE *f = fopen(path, "rb"); + + if (!f) { + PX4_ERR("Failed to open %s: %s", path, strerror(errno)); + return false; + } + + if (fseek(f, 0, SEEK_END) != 0) { + PX4_ERR("fseek failed: %s", strerror(errno)); + fclose(f); + return false; + } + + long size = ftell(f); + + if (size < 0) { + PX4_ERR("ftell failed: %s", strerror(errno)); + fclose(f); + return false; + + } else { + rewind(f); + bool successfully_loaded = false; + using SPEC = rlt::persist::backends::tar::ReaderGroupSpecification>; + rlt::persist::backends::tar::ReaderGroup reader_group; + reader_group.data.f = f; + reader_group.data.size = size; + auto actor_group = rlt::get_group(device, reader_group, "actor"); + successfully_loaded = rlt::load(device, policy, actor_group); + constexpr TI METADATA_BUFFER_SIZE = 256; + char metadata_buffer[METADATA_BUFFER_SIZE]; + TI read_size = 0; + rlt::persist::backends::tar::get(device, reader_group.data, "actor/meta", metadata_buffer, METADATA_BUFFER_SIZE, read_size); + TI checkpoint_name_position = 0; + TI checkpoint_name_len = 0; + + if (rlt::persist::backends::tar::seek_in_metadata(device, metadata_buffer, METADATA_BUFFER_SIZE, "checkpoint_name", + checkpoint_name_position, checkpoint_name_len)) { + strncpy(checkpoint_name, metadata_buffer + checkpoint_name_position, CHECKPOINT_NAME_LENGTH); + checkpoint_name[checkpoint_name_len < CHECKPOINT_NAME_LENGTH ? checkpoint_name_len : CHECKPOINT_NAME_LENGTH - 1] = '\0'; + + } else { + PX4_ERR("Failed to get checkpoint name from metadata"); + return false; + } + + if (successfully_loaded) { + PX4_INFO("Policy loaded from file %s", path); + TI input_offset = 0; + TI input_size = 0; + rlt::persist::backends::tar::seek(device, reader_group.data, "example/input/data", input_offset, input_size); + PX4_INFO("Input offset: %d", (int)input_offset); + TI output_offset = 0; + TI output_size = 0; + rlt::persist::backends::tar::seek(device, reader_group.data, "example/output/data", output_offset, output_size); + PX4_INFO("Output offset: %d", (int)output_offset); + + if (!test_policy(f, input_offset, output_offset)) { + PX4_ERR("Checkpoint test failed"); + return false; + } + + } else { + PX4_ERR("Failed to load policy from file %s", path); + return false; + } + + fclose(f); + } + + } else { + PX4_INFO("File %s does not exist", path); + return false; + } + +#else + + strncpy(checkpoint_name, MC_RAPTOR_META_NAMESPACE::name, CHECKPOINT_NAME_LENGTH); + + if (!test_policy()) { + PX4_ERR("Checkpoint test failed"); + return false; + } + +#endif + PX4_INFO("Checkpoint name: %s", checkpoint_name); + + + register_ext_component_request_s register_ext_component_request{}; + register_ext_component_request.timestamp = hrt_absolute_time(); + strncpy(register_ext_component_request.name, "RAPTOR", sizeof(register_ext_component_request.name) - 1); + register_ext_component_request.request_id = Raptor::EXT_COMPONENT_REQUEST_ID; + register_ext_component_request.px4_ros2_api_version = 1; + register_ext_component_request.register_arming_check = true; + register_ext_component_request.register_mode = true; + register_ext_component_request.enable_replace_internal_mode = _param_mc_raptor_offboard.get(); + register_ext_component_request.replace_internal_mode = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; + _register_ext_component_request_pub.publish(register_ext_component_request); + + int32_t imu_gyro_ratemax = _param_imu_gyro_ratemax.get(); + + if (imu_gyro_ratemax % POLICY_CONTROL_FREQUENCY_TRAINING != 0) { + PX4_WARN("IMU_GYRO_RATEMAX=%d Hz is not a multiple of the training frequency (%d Hz)", (int)imu_gyro_ratemax, + (int)POLICY_CONTROL_FREQUENCY_TRAINING); + } + + int32_t force_sync_native = imu_gyro_ratemax / POLICY_CONTROL_FREQUENCY_TRAINING; + executor.executor.force_sync_native = force_sync_native; + executor.executor.force_sync_native_initialized = true; + PX4_INFO("IMU_GYRO_RATEMAX=%d Hz", (int)imu_gyro_ratemax); + PX4_INFO("POLICY_CONTROL_FREQUENCY_TRAINING=%d Hz", (int)POLICY_CONTROL_FREQUENCY_TRAINING); + PX4_INFO("Setting force_sync_native = %d Hz / %d Hz = %d", (int)imu_gyro_ratemax, (int)POLICY_CONTROL_FREQUENCY_TRAINING, + (int)force_sync_native); + + this->use_internal_reference = _param_mc_raptor_intref.get(); + + this->reset(); + + return true; +} +template +T clip(T x, T max, T min) +{ + if (x > max) { + return max; + } + + if (x < min) { + return min; + } + + return x; +} +template +void quaternion_multiplication(T q1[4], T q2[4], T q_res[4]) +{ + q_res[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]; + q_res[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]; + q_res[2] = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]; + q_res[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]; +} +template +void quaternion_conjugate(T q[4], T q_res[4]) +{ + q_res[0] = +q[0]; + q_res[1] = -q[1]; + q_res[2] = -q[2]; + q_res[3] = -q[3]; +} +template +void quaternion_to_rotation_matrix(T q[4], T R[9]) +{ + // row-major + T qw = q[0]; + T qx = q[1]; + T qy = q[2]; + T qz = q[3]; + + R[0] = 1 - 2 * qy * qy - 2 * qz * qz; + R[1] = 2 * qx * qy - 2 * qw * qz; + R[2] = 2 * qx * qz + 2 * qw * qy; + R[3] = 2 * qx * qy + 2 * qw * qz; + R[4] = 1 - 2 * qx * qx - 2 * qz * qz; + R[5] = 2 * qy * qz - 2 * qw * qx; + R[6] = 2 * qx * qz - 2 * qw * qy; + R[7] = 2 * qy * qz + 2 * qw * qx; + R[8] = 1 - 2 * qx * qx - 2 * qy * qy; +} + +template +void rotate_vector(T R[9], T v[3], T v_rotated[3]) +{ + v_rotated[0] = R[0] * v[0] + R[1] * v[1] + R[2] * v[2]; + v_rotated[1] = R[3] * v[0] + R[4] * v[1] + R[5] * v[2]; + v_rotated[2] = R[6] * v[0] + R[7] * v[1] + R[8] * v[2]; +} + +void Raptor::observe(rl_tools::inference::applications::l2f::Observation &observation) +{ + // converting from FRD to FLU + T Rt_inv[9]; + + { + // Orientation + // FRD to FLU + T q_target[4]; + q_target[0] = cosf(0.5f * _trajectory_setpoint.yaw); // minus because the setpoint yaw is in NED + q_target[1] = 0; + q_target[2] = 0; + q_target[3] = sinf(0.5f * _trajectory_setpoint.yaw); + + T qt[4], qtc[4], qr[4]; + qt[0] = +q_target[0]; // conjugate to build the difference between setpoint and current + qt[1] = +q_target[1]; + qt[2] = -q_target[2]; + qt[3] = -q_target[3]; + quaternion_conjugate(qt, qtc); + quaternion_to_rotation_matrix(qtc, Rt_inv); + + qr[0] = +_vehicle_attitude.q[0]; + qr[1] = +_vehicle_attitude.q[1]; + qr[2] = -_vehicle_attitude.q[2]; + qr[3] = -_vehicle_attitude.q[3]; + // qr = qt * qd + // qd = qt' * qr + T qd[4]; + quaternion_multiplication(qtc, qr, qd); + + observation.orientation[0] = qd[0]; + observation.orientation[1] = qd[1]; + observation.orientation[2] = qd[2]; + observation.orientation[3] = qd[3]; + } + + { + // Position + T p[3], pt[3]; // FLU + p[0] = +(position[0] - _trajectory_setpoint.position[0]); + p[1] = -(position[1] - _trajectory_setpoint.position[1]); + p[2] = -(position[2] - _trajectory_setpoint.position[2]); + rotate_vector(Rt_inv, p, pt); // The position and velocity error are in the target frame + observation.position[0] = clip(pt[0], max_position_error, -max_position_error); + observation.position[1] = clip(pt[1], max_position_error, -max_position_error); + observation.position[2] = clip(pt[2], max_position_error, -max_position_error); + } + { + // Linear Velocity + T v[3], vt[3]; + v[0] = +(linear_velocity[0] - _trajectory_setpoint.velocity[0]); + v[1] = -(linear_velocity[1] - _trajectory_setpoint.velocity[1]); + v[2] = -(linear_velocity[2] - _trajectory_setpoint.velocity[2]); + rotate_vector(Rt_inv, v, vt); + observation.linear_velocity[0] = clip(vt[0], max_velocity_error, -max_velocity_error); + observation.linear_velocity[1] = clip(vt[1], max_velocity_error, -max_velocity_error); + observation.linear_velocity[2] = clip(vt[2], max_velocity_error, -max_velocity_error); + } + { + // Angular Velocity + observation.angular_velocity[0] = +_vehicle_angular_velocity.xyz[0]; + observation.angular_velocity[1] = -_vehicle_angular_velocity.xyz[1]; + observation.angular_velocity[2] = -_vehicle_angular_velocity.xyz[2]; + } + + for (TI action_i = 0; action_i < EXECUTOR_CONFIG::OUTPUT_DIM; action_i++) { + observation.previous_action[action_i] = this->previous_action[action_i]; + } +} + + +void Raptor::updateArmingCheckReply() +{ + if (flightmode_state == FlightModeState::CONFIGURED) { + if (_arming_check_request_sub.updated()) { + arming_check_request_s arming_check_request; + _arming_check_request_sub.copy(&arming_check_request); + arming_check_reply_s arming_check_reply; + arming_check_reply.timestamp = hrt_absolute_time(); + arming_check_reply.request_id = arming_check_request.request_id; + arming_check_reply.registration_id = ext_component_arming_check_id; + arming_check_reply.health_component_index = arming_check_reply.HEALTH_COMPONENT_INDEX_NONE; + arming_check_reply.num_events = 0; + arming_check_reply.can_arm_and_run = can_arm; + arming_check_reply.mode_req_angular_velocity = true; + arming_check_reply.mode_req_local_position = true; + arming_check_reply.mode_req_attitude = true; + arming_check_reply.mode_req_local_alt = true; + arming_check_reply.mode_req_home_position = false; + arming_check_reply.mode_req_mission = false; + arming_check_reply.mode_req_global_position = false; + arming_check_reply.mode_req_prevent_arming = false; + arming_check_reply.mode_req_manual_control = false; + _arming_check_reply_pub.publish(arming_check_reply); + } + } +} + + +void Raptor::Run() +{ + if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); + + if (flightmode_state >= FlightModeState::REGISTERED) { + unregister_ext_component_s unregister_ext_component{}; + unregister_ext_component.timestamp = hrt_absolute_time(); + strncpy(unregister_ext_component.name, "RAPTOR", sizeof(unregister_ext_component.name) - 1); + unregister_ext_component.arming_check_id = ext_component_arming_check_id; + unregister_ext_component.mode_id = ext_component_mode_id; + unregister_ext_component.mode_executor_id = -1; + _unregister_ext_component_pub.publish(unregister_ext_component); + } + + ScheduleClear(); + exit_and_cleanup(); + return; + } + + register_ext_component_reply_s register_ext_component_reply; + + if (_register_ext_component_reply_sub.update(®ister_ext_component_reply)) { + if (register_ext_component_reply.request_id == Raptor::EXT_COMPONENT_REQUEST_ID && register_ext_component_reply.success) { + ext_component_arming_check_id = register_ext_component_reply.arming_check_id; + ext_component_mode_id = register_ext_component_reply.mode_id; + flightmode_state = FlightModeState::REGISTERED; + PX4_INFO("Raptor mode registration successful, arming_check_id: %d, mode_id: %d", ext_component_arming_check_id, ext_component_mode_id); + } + } + + if (flightmode_state == FlightModeState::REGISTERED) { + vehicle_control_mode_s config_control_setpoints{}; + config_control_setpoints.timestamp = hrt_absolute_time(); + config_control_setpoints.source_id = ext_component_mode_id; + config_control_setpoints.flag_multicopter_position_control_enabled = false; + config_control_setpoints.flag_control_manual_enabled = false; + config_control_setpoints.flag_control_offboard_enabled = false; + config_control_setpoints.flag_control_position_enabled = false; + config_control_setpoints.flag_control_climb_rate_enabled = false; + config_control_setpoints.flag_control_allocation_enabled = false; + config_control_setpoints.flag_control_termination_enabled = true; + _config_control_setpoints_pub.publish(config_control_setpoints); + flightmode_state = FlightModeState::CONFIGURED; + PX4_INFO("Raptor mode configuration sent"); + } + + + perf_count(_loop_interval_perf); + + perf_begin(_loop_perf); + hrt_abstime current_time = hrt_absolute_time(); + + raptor_status_s status{}; + status.timestamp = current_time; + status.timestamp_sample = current_time; + status.exit_reason = raptor_status_s::EXIT_REASON_NONE; + status.substep = 0; + status.active = false; + status.control_interval = NAN; + status.trajectory_setpoint_dt_mean = NAN; + status.trajectory_setpoint_dt_max = NAN; + status.trajectory_setpoint_dt_max_since_activation = NAN; + + if (trajectory_setpoint_dts_full || trajectory_setpoint_dt_index > 0) { + float trajectory_setpoint_dt_mean = 0; + float trajectory_setpoint_dt_max = 0; + + for (TI i = 0; i < (trajectory_setpoint_dts_full ? NUM_TRAJECTORY_SETPOINT_DTS : trajectory_setpoint_dt_index); i++) { + TI index = trajectory_setpoint_dts_full ? i : trajectory_setpoint_dt_index - 1 - i; + trajectory_setpoint_dt_mean += trajectory_setpoint_dts[index]; + + if (trajectory_setpoint_dts[index] > trajectory_setpoint_dt_max) { + trajectory_setpoint_dt_max = trajectory_setpoint_dts[index]; + } + } + + if (trajectory_setpoint_dt_max > trajectory_setpoint_dt_max_since_reset) { + trajectory_setpoint_dt_max_since_reset = trajectory_setpoint_dt_max; + } + + trajectory_setpoint_dt_mean /= NUM_TRAJECTORY_SETPOINT_DTS; + status.trajectory_setpoint_dt_mean = trajectory_setpoint_dt_mean; + status.trajectory_setpoint_dt_max = trajectory_setpoint_dt_max; + status.trajectory_setpoint_dt_max_since_activation = trajectory_setpoint_dt_max_since_reset; + } + + status.subscription_update_vehicle_status = _vehicle_status_sub.update(&_vehicle_status); + + if (status.subscription_update_vehicle_status) { + timestamp_last_vehicle_status = current_time; + timestamp_last_vehicle_status_set = true; + } + + bool next_active = timestamp_last_vehicle_status_set && _vehicle_status.nav_state == ext_component_mode_id; + + if (!previous_active && next_active) { + this->reset(); + PX4_INFO("Resetting Inference Executor (Recurrent State)"); + + } else { + if (previous_active && !next_active) { + PX4_INFO("inactive"); + } + } + + bool angular_velocity_update = false; + status.subscription_update_angular_velocity = _vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity); + + if (status.subscription_update_angular_velocity) { + timestamp_last_angular_velocity = current_time; + timestamp_last_angular_velocity_set = true; + angular_velocity_update = true; + } + + status.timestamp_last_vehicle_angular_velocity = current_time; + status.timestamp_sample = _vehicle_angular_velocity.timestamp_sample; + + status.subscription_update_local_position = _vehicle_local_position_sub.update(&_vehicle_local_position); + + if (status.subscription_update_local_position) { + timestamp_last_local_position = current_time; + timestamp_last_local_position_set = true; + } + + status.timestamp_last_vehicle_local_position = current_time; + + status.subscription_update_attitude = _vehicle_attitude_sub.update(&_vehicle_attitude); + + if (status.subscription_update_attitude) { + timestamp_last_attitude = current_time; + timestamp_last_attitude_set = true; + } + + status.timestamp_last_vehicle_attitude = timestamp_last_attitude; + + trajectory_setpoint_s temp_trajectory_setpoint; + bool use_external_reference = !use_internal_reference; + status.subscription_update_trajectory_setpoint = use_external_reference && _trajectory_setpoint_sub.update(&temp_trajectory_setpoint); + + if (status.subscription_update_trajectory_setpoint) { + if ( + PX4_ISFINITE(temp_trajectory_setpoint.position[0]) && + PX4_ISFINITE(temp_trajectory_setpoint.position[1]) && + PX4_ISFINITE(temp_trajectory_setpoint.position[2]) && + PX4_ISFINITE(temp_trajectory_setpoint.yaw) && + PX4_ISFINITE(temp_trajectory_setpoint.velocity[0]) && + PX4_ISFINITE(temp_trajectory_setpoint.velocity[1]) && + PX4_ISFINITE(temp_trajectory_setpoint.velocity[2]) && + PX4_ISFINITE(temp_trajectory_setpoint.yawspeed) + ) { + if (timestamp_last_trajectory_setpoint_set) { + trajectory_setpoint_dts[trajectory_setpoint_dt_index] = current_time - timestamp_last_trajectory_setpoint; + trajectory_setpoint_dt_index++; + + if (trajectory_setpoint_dt_index == NUM_TRAJECTORY_SETPOINT_DTS) { + if (next_active && !trajectory_setpoint_dts_full) { + PX4_INFO("trajectory_setpoint_dts_full"); + } + + trajectory_setpoint_dts_full = true; + trajectory_setpoint_dt_index = 0; + } + } + + timestamp_last_trajectory_setpoint_set = true; + status.timestamp_last_trajectory_setpoint = current_time; + timestamp_last_trajectory_setpoint = current_time; + _trajectory_setpoint = temp_trajectory_setpoint; + + } else { + trajectory_setpoint_invalid_count++; + + if (next_active && trajectory_setpoint_invalid_count % TRAJECTORY_SETPOINT_INVALID_COUNT_WARNING_INTERVAL == 0) { + PX4_WARN("trajectory_setpoint invalid, count: %d", (int)trajectory_setpoint_invalid_count); + } + } + } + + constexpr bool PUBLISH_NON_COMPLETE_STATUS = true; + + if (!angular_velocity_update) { + status.exit_reason = raptor_status_s::EXIT_REASON_NO_ANGULAR_VELOCITY_UPDATE; + + if constexpr(PUBLISH_NON_COMPLETE_STATUS) { + _raptor_status_pub.publish(status); + } + + updateArmingCheckReply(); + return; + } + + if (!timestamp_last_angular_velocity_set || !timestamp_last_local_position_set || !timestamp_last_attitude_set) { + status.exit_reason = raptor_status_s::EXIT_REASON_NOT_ALL_OBSERVATIONS_SET; + status.vehicle_angular_velocity_stale = !timestamp_last_angular_velocity_set; + status.vehicle_local_position_stale = !timestamp_last_local_position_set; + status.vehicle_attitude_stale = !timestamp_last_attitude_set; + + if constexpr(PUBLISH_NON_COMPLETE_STATUS) { + _raptor_status_pub.publish(status); + } + + can_arm = false; + updateArmingCheckReply(); + return; + } + + if ((current_time - timestamp_last_angular_velocity) > OBSERVATION_TIMEOUT_ANGULAR_VELOCITY) { + status.exit_reason = raptor_status_s::EXIT_REASON_ANGULAR_VELOCITY_STALE; + + if constexpr(PUBLISH_NON_COMPLETE_STATUS) { + _raptor_status_pub.publish(status); + } + + if (!timeout_message_sent) { + PX4_ERR("angular velocity timeout"); + timeout_message_sent = true; + } + + can_arm = false; + updateArmingCheckReply(); + return; + } + + if ((current_time - timestamp_last_local_position) > OBSERVATION_TIMEOUT_LOCAL_POSITION) { + status.exit_reason = raptor_status_s::EXIT_REASON_LOCAL_POSITION_STALE; + + if constexpr(PUBLISH_NON_COMPLETE_STATUS) { + _raptor_status_pub.publish(status); + } + + if (!timeout_message_sent) { + PX4_ERR("local position timeout"); + timeout_message_sent = true; + } + + can_arm = false; + updateArmingCheckReply(); + return; + + } else { + position[0] = _vehicle_local_position.x; + position[1] = _vehicle_local_position.y; + position[2] = _vehicle_local_position.z; + linear_velocity[0] = _vehicle_local_position.vx; + linear_velocity[1] = _vehicle_local_position.vy; + linear_velocity[2] = _vehicle_local_position.vz; + } + + // position and linear_velocity are guaranteed to be set after this point + auto previous_internal_reference = internal_reference; + internal_reference = static_cast(_param_mc_raptor_intref.get()); + bool internal_reference_changed = previous_internal_reference != internal_reference; + + if (internal_reference_changed) { + PX4_INFO("internal reference changed from %d to %d", (int)previous_internal_reference, (int)internal_reference); + } + + if (use_internal_reference && internal_reference != InternalReference::NONE) { + if (next_active && (!previous_active || internal_reference_changed || internal_reference_params_changed)) { + internal_reference_activation_position[0] = position[0]; + internal_reference_activation_position[1] = - position[1]; + internal_reference_activation_position[2] = - position[2]; + internal_reference_activation_orientation[0] = _vehicle_attitude.q[0]; + internal_reference_activation_orientation[1] = _vehicle_attitude.q[1]; + internal_reference_activation_orientation[2] = -_vehicle_attitude.q[2]; + internal_reference_activation_orientation[3] = -_vehicle_attitude.q[3]; + internal_reference_activation_time = current_time; + PX4_INFO("internal reference activated at: %f %f %f", (double)internal_reference_activation_position[0], + (double)internal_reference_activation_position[1], (double)internal_reference_activation_position[2]); + internal_reference_params_changed = false; + } + + Setpoint setpoint{}; + + if (internal_reference == InternalReference::LISSAJOUS) { + setpoint = lissajous(static_cast(current_time - internal_reference_activation_time) / 1000000, lissajous_params); + + } else { + PX4_ERR("internal reference type not supported"); + } + + auto &q = internal_reference_activation_orientation; + matrix::Quatf q_activation_frame(q[0], q[1], q[2], q[3]); + matrix::Vector3f position_activation_frame = q_activation_frame.rotateVector(matrix::Vector3f(setpoint.position[0], setpoint.position[1], + setpoint.position[2])); + matrix::Vector3f linear_velocity_activation_frame = q_activation_frame.rotateVector(matrix::Vector3f(setpoint.linear_velocity[0], + setpoint.linear_velocity[1], setpoint.linear_velocity[2])); + + _trajectory_setpoint.position[0] = +(internal_reference_activation_position[0] + position_activation_frame(0)); + _trajectory_setpoint.position[1] = -(internal_reference_activation_position[1] + position_activation_frame(1)); + _trajectory_setpoint.position[2] = -(internal_reference_activation_position[2] + position_activation_frame(2)); + _trajectory_setpoint.yaw = - atan2f(2.0f * (q[1] * q[2] + q[0] * q[3]), 1.0f - 2.0f * (q[2] * q[2] + q[3] * q[3])) - setpoint.yaw; + _trajectory_setpoint.velocity[0] = +linear_velocity_activation_frame(0); + _trajectory_setpoint.velocity[1] = -linear_velocity_activation_frame(1); + _trajectory_setpoint.velocity[2] = -linear_velocity_activation_frame(2); + _trajectory_setpoint.yawspeed = -setpoint.yaw_rate; + timestamp_last_trajectory_setpoint_set = true; + status.timestamp_last_trajectory_setpoint = current_time; + timestamp_last_trajectory_setpoint = current_time; + + status.internal_reference_position[0] = _trajectory_setpoint.position[0]; + status.internal_reference_position[1] = _trajectory_setpoint.position[1]; + status.internal_reference_position[2] = _trajectory_setpoint.position[2]; + status.internal_reference_linear_velocity[0] = _trajectory_setpoint.velocity[0]; + status.internal_reference_linear_velocity[1] = _trajectory_setpoint.velocity[1]; + status.internal_reference_linear_velocity[2] = _trajectory_setpoint.velocity[2]; + } + + if ((current_time - timestamp_last_attitude) > OBSERVATION_TIMEOUT_ATTITUDE) { + status.exit_reason = raptor_status_s::EXIT_REASON_ATTITUDE_STALE; + + if constexpr(PUBLISH_NON_COMPLETE_STATUS) { + _raptor_status_pub.publish(status); + } + + if (!timeout_message_sent) { + PX4_ERR("attitude timeout"); + timeout_message_sent = true; + } + + can_arm = false; + updateArmingCheckReply(); + return; + } + + timeout_message_sent = false; + + // is ready to control at this point + can_arm = true; + updateArmingCheckReply(); + + if (!timestamp_last_trajectory_setpoint_set || use_internal_reference + || (current_time - timestamp_last_trajectory_setpoint) > TRAJECTORY_SETPOINT_TIMEOUT) { + status.trajectory_setpoint_stale = true; + + if (!previous_trajectory_setpoint_stale || (!previous_active && next_active)) { + _trajectory_setpoint.position[0] = position[0]; + _trajectory_setpoint.position[1] = position[1]; + _trajectory_setpoint.position[2] = position[2]; + auto &q = _vehicle_attitude.q; + _trajectory_setpoint.yaw = atan2f(2.0f * (q[1] * q[2] + q[0] * q[3]), 1.0f - 2.0f * (q[2] * q[2] + q[3] * q[3])); + _trajectory_setpoint.velocity[0] = 0; + _trajectory_setpoint.velocity[1] = 0; + _trajectory_setpoint.velocity[2] = 0; + _trajectory_setpoint.yawspeed = 0; + + if (!previous_trajectory_setpoint_stale) { + PX4_WARN("trajectory_setpoint turned stale at: %f %f %f, yaw: %f %llu / %llu us", (double)position[0], (double)position[1], + (double)position[2], + (double)_trajectory_setpoint.yaw, (unsigned long long)(current_time - timestamp_last_trajectory_setpoint), + (unsigned long long)(TRAJECTORY_SETPOINT_TIMEOUT)); + + } else { + PX4_WARN("trajectory_setpoint reset due to activation at: %f %f %f, yaw: %f", (double)position[0], (double)position[1], (double)position[2], + (double)_trajectory_setpoint.yaw); + } + } + + previous_trajectory_setpoint_stale = true; + + } else { + if (previous_trajectory_setpoint_stale) { + PX4_WARN("trajectory_setpoint turned non-stale at: %f %f %f", (double)position[0], (double)position[1], (double)position[2]); + previous_trajectory_setpoint_stale = false; + } + + status.trajectory_setpoint_stale = false; + } + + + + + + rl_tools::inference::applications::l2f::Observation observation; + rl_tools::inference::applications::l2f::Action action; + observe(observation); + hrt_abstime nanoseconds = current_time * 1000; + auto executor_status = rl_tools::control(device, executor, nanoseconds, policy, observation, action, rng); + + if (!executor_status.OK) { + if (executor_status.TIMESTAMP_INVALID) { + PX4_ERR("RLtools executor error: Timestamp invalid"); + } + + if (executor_status.LAST_CONTROL_TIMESTAMP_GREATER_THAN_LAST_OBSERVATION_TIMESTAMP) { + PX4_ERR("RLtools executor error: Last control timestamp %llu greater than last observation timestamp %llu", + (unsigned long long)executor.executor.last_control_timestamp, (unsigned long long)executor.executor.last_observation_timestamp); + } + } + + if (executor_status.source != decltype(executor_status.source)::CONTROL) { + // status.exit_reason = raptor_status_s::EXIT_REASON_EXECUTOR_STATUS_SOURCE_NOT_CONTROL; + // if constexpr(PUBLISH_NON_COMPLETE_STATUS){ + // _raptor_status_pub.publish(status); + // } + // Exit early if it is not time to control + return; + } + + + status.active = next_active; + + + // no return after this point! + + raptor_input_s input_msg; + input_msg.active = status.active; + static_assert(raptor_input_s::ACTION_DIM == EXECUTOR_CONFIG::OUTPUT_DIM); + input_msg.timestamp = current_time; + input_msg.timestamp_sample = _vehicle_angular_velocity.timestamp_sample; + + for (TI dim_i = 0; dim_i < 3; dim_i++) { + input_msg.position[dim_i] = observation.position[dim_i]; + input_msg.orientation[dim_i] = observation.orientation[dim_i]; + input_msg.linear_velocity[dim_i] = observation.linear_velocity[dim_i]; + input_msg.angular_velocity[dim_i] = observation.angular_velocity[dim_i]; + } + + input_msg.orientation[3] = observation.orientation[3]; + + for (TI dim_i = 0; dim_i < EXECUTOR_CONFIG::OUTPUT_DIM; dim_i++) { + input_msg.previous_action[dim_i] = observation.previous_action[dim_i]; + } + + _raptor_input_pub.publish(input_msg); + _raptor_status_pub.publish(status); + + actuator_motors_s actuator_motors{}; + actuator_motors.timestamp = hrt_absolute_time(); + actuator_motors.timestamp_sample = _vehicle_angular_velocity.timestamp_sample; + + for (TI action_i = 0; action_i < actuator_motors_s::NUM_CONTROLS; action_i++) { + if (action_i < EXECUTOR_CONFIG::OUTPUT_DIM) { + T value = action.action[action_i]; + this->previous_action[action_i] = value; + value = (value + 1) / 2; + constexpr T training_min = 0; + constexpr T training_max = 1.0; + T scaled_value = (training_max - training_min) * value + training_min; + actuator_motors.control[action_i] = scaled_value; + + } else { + actuator_motors.control[action_i] = NAN; + } + } + + if constexpr(Raptor::REMAP_FROM_CRAZYFLIE) { + actuator_motors_s temp = actuator_motors; + temp.control[0] = actuator_motors.control[0]; + temp.control[1] = actuator_motors.control[2]; + temp.control[2] = actuator_motors.control[3]; + temp.control[3] = actuator_motors.control[1]; + actuator_motors = temp; + } + + if (status.active) { + _actuator_motors_pub.publish(actuator_motors); + } + + perf_end(_loop_perf); + previous_active = next_active; + + if (executor_status.source == decltype(executor_status.source)::CONTROL) { + if (executor_status.step_type == decltype(executor_status.step_type)::INTERMEDIATE) { + this->last_intermediate_status = executor_status; + this->last_intermediate_status_set = true; + + } else if (executor_status.step_type == decltype(executor_status.step_type)::NATIVE) { + this->last_native_status = executor_status; + this->last_native_status_set = true; + } + } + + if (!this->timestamp_last_policy_frequency_check_set + || (current_time - timestamp_last_policy_frequency_check) > POLICY_FREQUENCY_CHECK_INTERVAL) { + if (this->timestamp_last_policy_frequency_check_set) { + if (last_intermediate_status_set) { + if (!this->last_intermediate_status.timing_bias.OK || !this->last_intermediate_status.timing_jitter.OK) { + PX4_WARN("Raptor: INTERMEDIATE: BIAS %fx JITTER %fx", (double)this->last_intermediate_status.timing_bias.MAGNITUDE, + (double)this->last_intermediate_status.timing_jitter.MAGNITUDE); + + } else { + if (ENABLE_CONTROL_FREQUENCY_INFO && this->policy_frequency_check_counter % POLICY_FREQUENCY_INFO_INTERVAL == 0) { + PX4_INFO("Raptor: INTERMEDIATE: BIAS %fx JITTER %fx", (double)this->last_intermediate_status.timing_bias.MAGNITUDE, + (double)this->last_intermediate_status.timing_jitter.MAGNITUDE); + } + } + } + + if (last_native_status_set) { + if (!this->last_native_status.timing_bias.OK || !this->last_native_status.timing_jitter.OK) { + PX4_WARN("Raptor: NATIVE: BIAS %fx JITTER %fx", (double)this->last_native_status.timing_bias.MAGNITUDE, + (double)this->last_native_status.timing_jitter.MAGNITUDE); + + } else { + if (ENABLE_CONTROL_FREQUENCY_INFO && this->policy_frequency_check_counter % POLICY_FREQUENCY_INFO_INTERVAL == 0) { + PX4_INFO("Raptor: NATIVE: BIAS %fx JITTER %fx", (double)this->last_native_status.timing_bias.MAGNITUDE, + (double)this->last_native_status.timing_jitter.MAGNITUDE); + } + } + } + } + + this->num_healthy_executor_statii_intermediate = 0; + this->num_non_healthy_executor_statii_intermediate = 0; + this->num_healthy_executor_statii_native = 0; + this->num_non_healthy_executor_statii_native = 0; + this->num_statii = 0; + this->timestamp_last_policy_frequency_check = current_time; + this->timestamp_last_policy_frequency_check_set = true; + this->policy_frequency_check_counter++; + } + + this->num_statii++; + this->num_healthy_executor_statii_intermediate += executor_status.OK && executor_status.source == decltype(executor_status.source)::CONTROL + && executor_status.step_type == decltype(executor_status.step_type)::INTERMEDIATE; + this->num_non_healthy_executor_statii_intermediate += (!executor_status.OK) + && executor_status.source == decltype(executor_status.source)::CONTROL + && executor_status.step_type == decltype(executor_status.step_type)::INTERMEDIATE; + this->num_healthy_executor_statii_native += executor_status.OK && executor_status.source == decltype(executor_status.source)::CONTROL + && executor_status.step_type == decltype(executor_status.step_type)::NATIVE; + this->num_non_healthy_executor_statii_native += (!executor_status.OK) + && executor_status.source == decltype(executor_status.source)::CONTROL + && executor_status.step_type == decltype(executor_status.step_type)::NATIVE; +} + +int Raptor::task_spawn(int argc, char *argv[]) +{ + Raptor *instance = new Raptor(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int Raptor::print_status() +{ + perf_print_counter(_loop_perf); + perf_print_counter(_loop_interval_perf); + perf_print_counter(_loop_interval_policy_perf); + PX4_INFO_RAW("Checkpoint: %s\n", checkpoint_name); + return 0; +} + +int Raptor::custom_command(int argc, char *argv[]) +{ + if (argc >= 2 && strcmp(argv[0], "intref") == 0) { + if (strcmp(argv[1], "lissajous") == 0) { + // Usage: mc_raptor intref lissajous + if (argc != 10) { + PX4_ERR("Usage: mc_raptor intref lissajous "); + return PX4_ERROR; + } + + Raptor *instance = get_instance(); + + if (instance == nullptr) { + PX4_ERR("mc_raptor is not running"); + return PX4_ERROR; + } + + instance->lissajous_params.A = strtof(argv[2], nullptr); + instance->lissajous_params.B = strtof(argv[3], nullptr); + instance->lissajous_params.C = strtof(argv[4], nullptr); + instance->lissajous_params.a = strtof(argv[5], nullptr); + instance->lissajous_params.b = strtof(argv[6], nullptr); + instance->lissajous_params.c = strtof(argv[7], nullptr); + instance->lissajous_params.duration = strtof(argv[8], nullptr); + instance->lissajous_params.ramp_duration = strtof(argv[9], nullptr); + instance->internal_reference_params_changed = true; + + PX4_INFO("Lissajous params set: A=%.2f B=%.2f C=%.2f fa=%.2f fb=%.2f fc=%.2f duration=%.2f ramp=%.2f \n", + (double)instance->lissajous_params.A, + (double)instance->lissajous_params.B, + (double)instance->lissajous_params.C, + (double)instance->lissajous_params.a, + (double)instance->lissajous_params.b, + (double)instance->lissajous_params.c, + (double)instance->lissajous_params.duration, + (double)instance->lissajous_params.ramp_duration); + + + return PX4_OK; + } + } + + return print_usage("unknown command"); +} + +int Raptor::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +RAPTOR Policy Flight Mode + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("mc_raptor", "template"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_COMMAND_DESCR("intref", "Modify internal reference"); + PRINT_MODULE_USAGE_ARG("lissajous", "Set Lissajous trajectory parameters", false); + PRINT_MODULE_USAGE_ARG("", "Amplitude X [m]", false); + PRINT_MODULE_USAGE_ARG("", "Amplitude Y [m]", false); + PRINT_MODULE_USAGE_ARG("", "Amplitude Z [m]", false); + PRINT_MODULE_USAGE_ARG("", "Frequency a", false); + PRINT_MODULE_USAGE_ARG("", "Frequency b", false); + PRINT_MODULE_USAGE_ARG("", "Frequency c", false); + PRINT_MODULE_USAGE_ARG("", "Total duration [s]", false); + PRINT_MODULE_USAGE_ARG("", "Ramp duration [s]", false); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int mc_raptor_main(int argc, char *argv[]) +{ + return Raptor::main(argc, argv); +} diff --git a/src/modules/mc_raptor/mc_raptor.hpp b/src/modules/mc_raptor/mc_raptor.hpp new file mode 100644 index 0000000000..0ffb8e6d01 --- /dev/null +++ b/src/modules/mc_raptor/mc_raptor.hpp @@ -0,0 +1,283 @@ +#pragma once + +#include "trajectories/lissajous.hpp" + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#undef OK + +#ifdef __PX4_POSIX +#include +#else +#include +#endif + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "blob/policy.h" + +#include +#include +#include +#include +#include + +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, 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; +#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_pub{ORB_ID(actuator_motors)}; + uORB::Publication _raptor_status_pub{ORB_ID(raptor_status)}; + uORB::Publication _raptor_input_pub{ORB_ID(raptor_input)}; + uORB::Publication _tune_control_pub{ORB_ID(tune_control)}; + uORB::Publication _register_ext_component_request_pub{ORB_ID(register_ext_component_request)}; + uORB::Publication _unregister_ext_component_pub{ORB_ID(unregister_ext_component)}; + uORB::Publication _config_control_setpoints_pub{ORB_ID(config_control_setpoints)}; + uORB::Publication _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::template CHANGE_SEQUENCE_LENGTH; + using POLICY_BATCH_SIZE = ACTOR_TYPE_ORIGINAL::template CHANGE_BATCH_SIZE; +#ifdef MC_RAPTOR_EMBED_POLICY + using POLICY = POLICY_BATCH_SIZE; +#else + using POLICY = POLICY_BATCH_SIZE::template CHANGE_CAPABILITY>; +#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 { + 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 { + 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; + using EXECUTOR_STATUS = rlt::inference::executor::Status; + }; + using EXECUTOR_SPEC = EXECUTOR_CONFIG::EXECUTOR_SPEC; + rl_tools::inference::applications::L2F 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 &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) _param_imu_gyro_ratemax, + (ParamBool) _param_mc_raptor_offboard, + (ParamInt) _param_mc_raptor_intref + ) + + +}; diff --git a/src/modules/mc_raptor/module.yaml b/src/modules/mc_raptor/module.yaml new file mode 100644 index 0000000000..ff036b05ea --- /dev/null +++ b/src/modules/mc_raptor/module.yaml @@ -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 ' to configure the trajectory. + type: enum + values: + 0: None + 1: Lissajous + default: 0 + category: System diff --git a/src/modules/mc_raptor/trajectories/lissajous.hpp b/src/modules/mc_raptor/trajectories/lissajous.hpp new file mode 100644 index 0000000000..340ead01bf --- /dev/null +++ b/src/modules/mc_raptor/trajectories/lissajous.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include "trajectory.hpp" +#include + +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(M_PI) / params.duration; + float d_progress = 2.0f * static_cast(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; +} diff --git a/src/modules/mc_raptor/trajectories/trajectory.hpp b/src/modules/mc_raptor/trajectories/trajectory.hpp new file mode 100644 index 0000000000..fcdf405701 --- /dev/null +++ b/src/modules/mc_raptor/trajectories/trajectory.hpp @@ -0,0 +1,6 @@ +struct Setpoint { + float position[3]; + float yaw; + float linear_velocity[3]; + float yaw_rate; +};