mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-20 04:33:35 +08:00
feature: Integrating the RAPTOR foundation policy (#26082)
* moving raptor bump compiles and raptor mode appears hovering with RAPTOR seems to work Using Raptor to execute offboard commands works (using multirobot f03825a5795a77c5a095f799eeb8e0b646fe7176 to feed the trajectory_setpoint). Requires more testing simplified rotmat runtime inference frequency multiple arming request response reflects actual readiness adjusting to fit IMU gyro ratemax relaxing control timing warning thresholds for SITL Using mode registration to signal if offboard commands should be forwarded to trajectory_setpoint instead of just hardcoding vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD adopting new "request_offboard_setpoint" in raptor module replace offboard seems good mc_raptor: overwrite offboard parameter separate raptor config addendum Raptor off by default RAPTOR readme Loading raptor checkpoint from tar works. check if load was successful refactoring: cutting out the pure C interface to allow direct testing of the policy input/output behavior from the file, without fully loading it into memory first adapter not needed anymore ripping out test observation mode (not used in a long time) fixing warnings bump RLtools to fix the remaining warnings Loading RAPTOR checkpoint from sdcard seems to work on FMU-6C embedding Raptor policy into flash works again also printing checkpoint name when using the embedded policy cleaner handling of the checkpoint name back to reading from file ripping out visual odometry checks cleaner more debug but no success bump rlt bump pre next rebase we can publish the no angvel update because we latch onto it with the scheduled work item anyways this kind of runs on the 6c still bad SIH almost flying saving stale traj setpoint yaw new error. timestamp not the problem anymore bump rlt; SIH works with executor shaping up bumping blob (include tar checkpoint) cleaning up fixing formatting update readme * moving raptor bump compiles and raptor mode appears hovering with RAPTOR seems to work Using Raptor to execute offboard commands works (using multirobot f03825a5795a77c5a095f799eeb8e0b646fe7176 to feed the trajectory_setpoint). Requires more testing simplified rotmat runtime inference frequency multiple arming request response reflects actual readiness adjusting to fit IMU gyro ratemax relaxing control timing warning thresholds for SITL Using mode registration to signal if offboard commands should be forwarded to trajectory_setpoint instead of just hardcoding vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD adopting new "request_offboard_setpoint" in raptor module replace offboard seems good mc_raptor: overwrite offboard parameter separate raptor config addendum Raptor off by default RAPTOR readme Loading raptor checkpoint from tar works. check if load was successful refactoring: cutting out the pure C interface to allow direct testing of the policy input/output behavior from the file, without fully loading it into memory first adapter not needed anymore ripping out test observation mode (not used in a long time) fixing warnings bump RLtools to fix the remaining warnings Loading RAPTOR checkpoint from sdcard seems to work on FMU-6C embedding Raptor policy into flash works again also printing checkpoint name when using the embedded policy cleaner handling of the checkpoint name back to reading from file ripping out visual odometry checks cleaner more debug but no success bump rlt bump pre next rebase we can publish the no angvel update because we latch onto it with the scheduled work item anyways this kind of runs on the 6c still bad SIH almost flying saving stale traj setpoint yaw new error. timestamp not the problem anymore bump rlt; SIH works with executor shaping up bumping blob (include tar checkpoint) cleaning up fixing formatting update readme updating gitignore * fixing format and declaring submodules as cmake dependencies * adding uORB message documentation * fixing comment alignment * Adding option to restrict mc_raptor to not listen to the trajectory_setpoint (use the position and yaw at activation time as reference instead) * bump RLtools; relax timing thresholds and adding real world readme * smooth traj tracking performance * Measuring trajectory_setpoint timing (providing stats in raptor_status); reverting accidental .gitignore modification * More ideomatic way of setting the path to the policy checkpoint * Reset trajectory_setpoint on raptor mode activation * Adding internal trajectory generation (feeding trajectory_setpoint over Mavlink is too noisy). Quite agile trajectory tracking, good performance * stable flight * Update msg/versioned/RaptorInput.msg Co-authored-by: Hamish Willee <hamishwillee@gmail.com> * adopting message formatting conventions * sort raptor.px4board * Archiving RegisterExtComponentRequestV1.msg * Add message versioning for VehicleStatus v2 and RegisterExtComponentRequest v2 * fixing formatting * making internal reference configurable via command * RAPTOR docs wip * raptor internal reference documentation * Finishing RAPTOR docs first draft * adding logging instructions * Fixing missing command documentation test error * fixing format * adding motor layout warning * raptor minimal subedit - prettier, images etc * Improve intro * Fix up Neural_Networks version * Mentioning "Adaptive" in the RAPTOR documentation's title * Adding clarifications about the internal reference trajectory generator * Removing "foundation policy" wording * Fixing new-line check * Removing redundant (evident through directory hierarchy) raptor_ from filenames * Unifying Neural Network docs (mc_nn_control and mc_raptor) under the "Neural Network" topic * Fix to standard structure * Making the distinction between mc_nn_control and mc_raptor more clear and fixing the comparison table * Removing trajectory_setpoint forwarding flag from external mode registration request and from the vehicle status * Trivial layout and wording fixes * fixing docs error --------- Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
This commit is contained in:
Jonas Eschmann
co-authored by
Hamish Willee
Hamish Willee
parent
95b8328162
commit
db2c6b2abe
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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
|
||||
@@ -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
|
||||
```
|
||||
Submodule
+1
Submodule src/modules/mc_raptor/blob added at 4f31fc9736
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,283 @@
|
||||
#pragma once
|
||||
|
||||
#include "trajectories/lissajous.hpp"
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_odometry.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/actuator_motors.h>
|
||||
#include <uORB/topics/trajectory_setpoint.h>
|
||||
#include <uORB/topics/register_ext_component_request.h>
|
||||
#include <uORB/topics/register_ext_component_reply.h>
|
||||
#include <uORB/topics/unregister_ext_component.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/raptor_status.h>
|
||||
#include <uORB/topics/raptor_input.h>
|
||||
#include <uORB/topics/tune_control.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/arming_check_request.h>
|
||||
#include <uORB/topics/arming_check_reply.h>
|
||||
#undef OK
|
||||
|
||||
#ifdef __PX4_POSIX
|
||||
#include <rl_tools/operations/cpu.h>
|
||||
#else
|
||||
#include <rl_tools/operations/arm.h>
|
||||
#endif
|
||||
|
||||
#include <rl_tools/nn/layers/standardize/operations_generic.h>
|
||||
#include <rl_tools/nn/layers/dense/operations_arm/opt.h>
|
||||
#include <rl_tools/nn/layers/sample_and_squash/operations_generic.h>
|
||||
#include <rl_tools/nn/layers/gru/operations_generic.h>
|
||||
#include <rl_tools/nn_models/mlp/operations_generic.h>
|
||||
#include <rl_tools/nn_models/sequential/operations_generic.h>
|
||||
|
||||
#include <rl_tools/inference/executor/executor.h>
|
||||
#include <rl_tools/inference/applications/l2f/l2f.h>
|
||||
|
||||
#include "blob/policy.h"
|
||||
|
||||
#include <rl_tools/persist/backends/tar/operations_posix.h>
|
||||
#include <rl_tools/nn/optimizers/adam/instance/persist.h>
|
||||
#include <rl_tools/nn/layers/gru/persist.h>
|
||||
#include <rl_tools/nn/layers/dense/persist.h>
|
||||
#include <rl_tools/nn_models/sequential/persist.h>
|
||||
|
||||
namespace rlt = rl_tools;
|
||||
|
||||
#define MC_RAPTOR_POLICY_NAMESPACE rlt::checkpoint::actor
|
||||
#define MC_RAPTOR_EXAMPLE_NAMESPACE rlt::checkpoint::example
|
||||
#define MC_RAPTOR_META_NAMESPACE rlt::checkpoint::meta
|
||||
// #define MC_RAPTOR_EMBED_POLICY // you can use this to directly embed the policy into the firmware instead of loading it from the sd card. To fit into the flash you might need to disable some unnecessary features in the .px4board config.
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class Raptor : public ModuleBase<Raptor>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
Raptor();
|
||||
~Raptor() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
bool init();
|
||||
|
||||
int print_status() override;
|
||||
|
||||
private:
|
||||
#ifdef __PX4_POSIX
|
||||
using DEVICE = rlt::devices::DefaultCPU;
|
||||
#else
|
||||
using DEV_SPEC = rlt::devices::DefaultARMSpecification;
|
||||
using DEVICE = rlt::devices::arm::OPT<DEV_SPEC>;
|
||||
#endif
|
||||
using TI = typename DEVICE::index_t;
|
||||
using RNG = DEVICE::SPEC::RANDOM::ENGINE<>;
|
||||
using T = float;
|
||||
static constexpr uint64_t EXT_COMPONENT_REQUEST_ID = 1337;
|
||||
DEVICE device;
|
||||
RNG rng;
|
||||
hrt_abstime init_time;
|
||||
// node constants
|
||||
static constexpr TI OBSERVATION_TIMEOUT_ANGULAR_VELOCITY = 10 * 1000;
|
||||
static constexpr TI OBSERVATION_TIMEOUT_LOCAL_POSITION = 100 * 1000;
|
||||
static constexpr TI OBSERVATION_TIMEOUT_ATTITUDE = 50 * 1000;
|
||||
static constexpr TI TRAJECTORY_SETPOINT_TIMEOUT = 200 * 1000;
|
||||
static constexpr T RESET_PREVIOUS_ACTION_VALUE = 0; // -1 to 1
|
||||
static constexpr bool ENABLE_CONTROL_FREQUENCY_INFO = false;
|
||||
|
||||
T max_position_error = 0.5;
|
||||
T max_velocity_error = 1.0;
|
||||
|
||||
void Run() override;
|
||||
|
||||
decltype(register_ext_component_reply_s::mode_id) ext_component_mode_id;
|
||||
decltype(register_ext_component_reply_s::arming_check_id) ext_component_arming_check_id;
|
||||
|
||||
enum class FlightModeState : TI {
|
||||
UNREGISTERED = 0,
|
||||
REGISTERED = 1,
|
||||
CONFIGURED = 2
|
||||
};
|
||||
FlightModeState flightmode_state = FlightModeState::UNREGISTERED;
|
||||
bool can_arm = false;
|
||||
void updateArmingCheckReply();
|
||||
|
||||
// node state
|
||||
vehicle_local_position_s _vehicle_local_position{};
|
||||
vehicle_angular_velocity_s _vehicle_angular_velocity{};
|
||||
vehicle_attitude_s _vehicle_attitude{};
|
||||
vehicle_status_s _vehicle_status{};
|
||||
trajectory_setpoint_s _trajectory_setpoint{};
|
||||
hrt_abstime timestamp_last_local_position, timestamp_last_angular_velocity, timestamp_last_attitude, timestamp_last_trajectory_setpoint,
|
||||
timestamp_last_manual_control_input, timestamp_last_vehicle_status;
|
||||
bool timestamp_last_local_position_set = false, timestamp_last_angular_velocity_set = false, timestamp_last_attitude_set = false,
|
||||
timestamp_last_trajectory_setpoint_set = false, timestamp_last_manual_control_input_set = false, timestamp_last_vehicle_status_set = false;
|
||||
bool timeout_message_sent = false;
|
||||
bool previous_trajectory_setpoint_stale = false;
|
||||
bool previous_active = false;
|
||||
|
||||
T position[3];
|
||||
T linear_velocity[3];
|
||||
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _register_ext_component_reply_sub{ORB_ID(register_ext_component_reply)};
|
||||
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
|
||||
uORB::Subscription _arming_check_request_sub{ORB_ID(arming_check_request)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
|
||||
uORB::Publication<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
|
||||
uORB::Publication<raptor_status_s> _raptor_status_pub{ORB_ID(raptor_status)};
|
||||
uORB::Publication<raptor_input_s> _raptor_input_pub{ORB_ID(raptor_input)};
|
||||
uORB::Publication<tune_control_s> _tune_control_pub{ORB_ID(tune_control)};
|
||||
uORB::Publication<register_ext_component_request_s> _register_ext_component_request_pub{ORB_ID(register_ext_component_request)};
|
||||
uORB::Publication<unregister_ext_component_s> _unregister_ext_component_pub{ORB_ID(unregister_ext_component)};
|
||||
uORB::Publication<vehicle_control_mode_s> _config_control_setpoints_pub{ORB_ID(config_control_setpoints)};
|
||||
uORB::Publication<arming_check_reply_s> _arming_check_reply_pub{ORB_ID(arming_check_reply)};
|
||||
// Performance (perf) counters
|
||||
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
|
||||
perf_counter_t _loop_interval_policy_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval_policy")};
|
||||
|
||||
struct EXECUTOR_CONFIG {
|
||||
static constexpr TI ACTION_HISTORY_LENGTH = 1;
|
||||
static constexpr TI CONTROL_INTERVAL_INTERMEDIATE_NS = 2.5 * 1000 * 1000; // Inference is at 500hz
|
||||
static constexpr TI CONTROL_INTERVAL_NATIVE_NS = 10 * 1000 * 1000; // Training is 100hz
|
||||
static constexpr TI TIMING_STATS_NUM_STEPS = 100;
|
||||
static constexpr bool FORCE_SYNC_INTERMEDIATE = true;
|
||||
static constexpr bool FORCE_SYNC_NATIVE_RUNTIME = true;
|
||||
static constexpr TI FORCE_SYNC_NATIVE = 8;
|
||||
static constexpr bool DYNAMIC_ALLOCATION = false;
|
||||
|
||||
using ACTOR_TYPE_ORIGINAL = MC_RAPTOR_POLICY_NAMESPACE ::TYPE;
|
||||
using POLICY_TEST = MC_RAPTOR_POLICY_NAMESPACE ::TYPE::template CHANGE_BATCH_SIZE<TI, 1>::template CHANGE_SEQUENCE_LENGTH<TI, 1>;
|
||||
using POLICY_BATCH_SIZE = ACTOR_TYPE_ORIGINAL::template CHANGE_BATCH_SIZE<TI, 1>;
|
||||
#ifdef MC_RAPTOR_EMBED_POLICY
|
||||
using POLICY = POLICY_BATCH_SIZE;
|
||||
#else
|
||||
using POLICY = POLICY_BATCH_SIZE::template CHANGE_CAPABILITY<rlt::nn::capability::Forward<false, false>>;
|
||||
#endif
|
||||
using TYPE_POLICY = typename POLICY::TYPE_POLICY;
|
||||
|
||||
#if defined(__PX4_POSIX)
|
||||
// Relax warning levels for Gazebo sitl. Because Gazebo SITL runs at 250Hz IMU rate, it is not a clean multiple of the training frequency (100hz), hence if the thresholds are set too strict, warnings will be triggered all the time. Generally, Raptor is quite robuts agains control frequency deviations.
|
||||
struct WARNING_LEVELS: rlt::inference::executor::WarningLevelsDefault<TYPE_POLICY> {
|
||||
using T = typename TYPE_POLICY::DEFAULT;
|
||||
static constexpr T INTERMEDIATE_TIMING_JITTER_HIGH_THRESHOLD = 2.0;
|
||||
static constexpr T INTERMEDIATE_TIMING_JITTER_LOW_THRESHOLD = 0.5;
|
||||
static constexpr T INTERMEDIATE_TIMING_BIAS_HIGH_THRESHOLD = 2.0;
|
||||
static constexpr T INTERMEDIATE_TIMING_BIAS_LOW_THRESHOLD = 0.5;
|
||||
static constexpr T NATIVE_TIMING_JITTER_HIGH_THRESHOLD = 2.0;
|
||||
static constexpr T NATIVE_TIMING_JITTER_LOW_THRESHOLD = 0.5;
|
||||
static constexpr T NATIVE_TIMING_BIAS_HIGH_THRESHOLD = 2.0;
|
||||
static constexpr T NATIVE_TIMING_BIAS_LOW_THRESHOLD = 0.5;
|
||||
};
|
||||
#else
|
||||
struct WARNING_LEVELS: rlt::inference::executor::WarningLevelsDefault<TYPE_POLICY> {
|
||||
using T = typename TYPE_POLICY::DEFAULT;
|
||||
static constexpr T NATIVE_TIMING_JITTER_HIGH_THRESHOLD = 1.5;
|
||||
static constexpr T NATIVE_TIMING_JITTER_LOW_THRESHOLD = 0.5;
|
||||
};
|
||||
#endif
|
||||
using TIMESTAMP = hrt_abstime;
|
||||
static constexpr TI OUTPUT_DIM = 4;
|
||||
static constexpr TI TEST_SEQUENCE_LENGTH_ACTUAL = 5;
|
||||
static constexpr TI TEST_BATCH_SIZE_ACTUAL = 2;
|
||||
|
||||
using EXECUTOR_SPEC =
|
||||
rl_tools::inference::applications::l2f::Specification<TYPE_POLICY, TI, TIMESTAMP, ACTION_HISTORY_LENGTH, OUTPUT_DIM, POLICY, CONTROL_INTERVAL_INTERMEDIATE_NS, CONTROL_INTERVAL_NATIVE_NS, FORCE_SYNC_INTERMEDIATE, FORCE_SYNC_NATIVE, FORCE_SYNC_NATIVE_RUNTIME, WARNING_LEVELS, DYNAMIC_ALLOCATION>;
|
||||
using EXECUTOR_STATUS = rlt::inference::executor::Status<EXECUTOR_SPEC::EXECUTOR_SPEC>;
|
||||
};
|
||||
using EXECUTOR_SPEC = EXECUTOR_CONFIG::EXECUTOR_SPEC;
|
||||
rl_tools::inference::applications::L2F<EXECUTOR_SPEC> executor;
|
||||
#ifdef MC_RAPTOR_EMBED_POLICY
|
||||
const decltype(MC_RAPTOR_POLICY_NAMESPACE ::module) &policy = MC_RAPTOR_POLICY_NAMESPACE::module;
|
||||
#else
|
||||
EXECUTOR_CONFIG::POLICY policy;
|
||||
#endif
|
||||
static constexpr TI CHECKPOINT_NAME_LENGTH = 100;
|
||||
char checkpoint_name[CHECKPOINT_NAME_LENGTH] = "n/a";
|
||||
|
||||
#ifdef MC_RAPTOR_EMBED_POLICY
|
||||
bool test_policy();
|
||||
#else
|
||||
bool test_policy(FILE *f, TI input_offset, TI output_offset);
|
||||
#endif
|
||||
|
||||
void reset();
|
||||
void observe(rl_tools::inference::applications::l2f::Observation<EXECUTOR_SPEC> &observation);
|
||||
|
||||
static constexpr bool REMAP_FROM_CRAZYFLIE =
|
||||
true; // Policy (Crazyflie assignment) => Quadrotor (PX4 Quadrotor X assignment) PX4 SIH assumes the Quadrotor X configuration, which assumes different rotor positions than the crazyflie mapping (from crazyflie outputs to PX4): 1=>1, 2=>4, 3=>2, 4=>3
|
||||
// controller state
|
||||
|
||||
// messaging state
|
||||
static constexpr TI POLICY_INTERVAL_WARNING_THRESHOLD = 100; // us
|
||||
static constexpr TI POLICY_FREQUENCY_CHECK_INTERVAL = 1000 * 1000; // 1s
|
||||
static constexpr TI POLICY_FREQUENCY_INFO_INTERVAL = 10; // 10 x POLICY_FREQUENCY_CHECK_INTERVAL = 10x
|
||||
static constexpr TI POLICY_CONTROL_FREQUENCY_TRAINING = 100;
|
||||
|
||||
TI num_statii;
|
||||
TI num_healthy_executor_statii_intermediate, num_non_healthy_executor_statii_intermediate, num_healthy_executor_statii_native,
|
||||
num_non_healthy_executor_statii_native;
|
||||
EXECUTOR_CONFIG::EXECUTOR_STATUS last_intermediate_status, last_native_status;
|
||||
bool last_intermediate_status_set, last_native_status_set;
|
||||
|
||||
TI policy_frequency_check_counter;
|
||||
hrt_abstime timestamp_last_policy_frequency_check;
|
||||
bool timestamp_last_policy_frequency_check_set = false;
|
||||
|
||||
static constexpr TI NUM_TRAJECTORY_SETPOINT_DTS = 100;
|
||||
int32_t trajectory_setpoint_dts[NUM_TRAJECTORY_SETPOINT_DTS];
|
||||
TI trajectory_setpoint_dt_index = 0;
|
||||
TI trajectory_setpoint_dt_max_since_reset = 0;
|
||||
bool trajectory_setpoint_dts_full = false;
|
||||
|
||||
static constexpr TI TRAJECTORY_SETPOINT_INVALID_COUNT_WARNING_INTERVAL = 100;
|
||||
TI trajectory_setpoint_invalid_count = 0;
|
||||
|
||||
float previous_action[EXECUTOR_SPEC::OUTPUT_DIM];
|
||||
bool use_internal_reference = false;
|
||||
bool internal_reference_params_changed = false;
|
||||
T internal_reference_activation_position[3];
|
||||
T internal_reference_activation_orientation[4];
|
||||
hrt_abstime internal_reference_activation_time;
|
||||
enum class InternalReference : TI { // make sure this corresponds to the enum values for MC_RAPTOR_INTREF in module.yaml
|
||||
NONE = 0,
|
||||
LISSAJOUS = 1
|
||||
};
|
||||
InternalReference internal_reference = InternalReference::NONE;
|
||||
LissajousParameters lissajous_params{}; // Set via 'mc_raptor intref lissajous ...' command
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax,
|
||||
(ParamBool<px4::params::MC_RAPTOR_OFFB>) _param_mc_raptor_offboard,
|
||||
(ParamInt<px4::params::MC_RAPTOR_INTREF>) _param_mc_raptor_intref
|
||||
)
|
||||
|
||||
|
||||
};
|
||||
@@ -0,0 +1,43 @@
|
||||
module_name: mc_raptor
|
||||
parameters:
|
||||
- group: Multicopter Raptor
|
||||
definitions:
|
||||
MC_RAPTOR_ENABLE:
|
||||
description:
|
||||
short: Enable Raptor flight mode
|
||||
long: |
|
||||
When enabled, the Raptor flight mode will be available. Please set MC_RAPTOR_OFFB according to your use case.
|
||||
type: boolean
|
||||
default: false
|
||||
category: System
|
||||
MC_RAPTOR_VERBOS:
|
||||
description:
|
||||
short: Enable verbose output
|
||||
long: |
|
||||
When enabled, the Raptor flight mode will print verbose output to the console.
|
||||
type: boolean
|
||||
default: false
|
||||
category: System
|
||||
|
||||
MC_RAPTOR_OFFB:
|
||||
description:
|
||||
short: Enable Offboard mode replacement
|
||||
long: |
|
||||
When enabled, the Raptor mode will replace the Offboard mode.
|
||||
If disabled, the Raptor mode will be available as a separate external mode. In the latter case, Raptor will just hold the position, without requiring external setpoints. When Raptor replaces the Offboard mode, it requires external setpoints to be activated.
|
||||
type: boolean
|
||||
default: false
|
||||
category: System
|
||||
|
||||
MC_RAPTOR_INTREF:
|
||||
description:
|
||||
short: Use internal reference instead of trajectory_setpoint
|
||||
long: |
|
||||
When enabled, instead of using the trajectory_setpoint, the position and yaw of the vehicle at the point when the Raptor mode is activated will be used as reference.
|
||||
Use 'mc_raptor intref lissajous <A> <B> <C> <fa> <fb> <fc> <duration> <ramp>' to configure the trajectory.
|
||||
type: enum
|
||||
values:
|
||||
0: None
|
||||
1: Lissajous
|
||||
default: 0
|
||||
category: System
|
||||
@@ -0,0 +1,38 @@
|
||||
#pragma once
|
||||
|
||||
#include "trajectory.hpp"
|
||||
#include <math.h>
|
||||
|
||||
struct LissajousParameters {
|
||||
float A = 0.5f; // amplitude a
|
||||
float B = 1.0f; // amplitude b
|
||||
float C = 0.0f; // amplitude c
|
||||
float a = 2.0f; // frequency a
|
||||
float b = 1.0f; // frequency b
|
||||
float c = 1.0f; // frequency c
|
||||
float duration = 10.0f;
|
||||
float ramp_duration = 3.0f;
|
||||
};
|
||||
|
||||
inline Setpoint lissajous(float time, const LissajousParameters ¶ms)
|
||||
{
|
||||
float time_velocity = (params.ramp_duration > 0.0f)
|
||||
? fminf(time, params.ramp_duration) / params.ramp_duration
|
||||
: 1.0f;
|
||||
|
||||
float ramp_time = time_velocity * fminf(time, params.ramp_duration) / 2.0f;
|
||||
float progress = (ramp_time + fmaxf(0.0f, time - params.ramp_duration)) * 2.0f * static_cast<float>(M_PI) / params.duration;
|
||||
float d_progress = 2.0f * static_cast<float>(M_PI) * time_velocity / params.duration;
|
||||
|
||||
Setpoint setpoint{};
|
||||
setpoint.position[0] = params.A * sinf(params.a * progress);
|
||||
setpoint.position[1] = params.B * sinf(params.b * progress);
|
||||
setpoint.position[2] = params.C * sinf(params.c * progress);
|
||||
setpoint.yaw = 0.0f;
|
||||
setpoint.linear_velocity[0] = params.A * cosf(params.a * progress) * params.a * d_progress;
|
||||
setpoint.linear_velocity[1] = params.B * cosf(params.b * progress) * params.b * d_progress;
|
||||
setpoint.linear_velocity[2] = params.C * cosf(params.c * progress) * params.c * d_progress;
|
||||
setpoint.yaw_rate = 0.0f;
|
||||
|
||||
return setpoint;
|
||||
}
|
||||
@@ -0,0 +1,6 @@
|
||||
struct Setpoint {
|
||||
float position[3];
|
||||
float yaw;
|
||||
float linear_velocity[3];
|
||||
float yaw_rate;
|
||||
};
|
||||
Reference in New Issue
Block a user