hyperray 0.0.1__py3-none-any.whl

hyperdrone/_vendor/rl-tools/include/rl_tools/rl/environments/hyperdrone/pose.h

@@ -0,0 +1,253 @@
+#include "../../../version.h"
+#include "../../../rl_tools.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_HYPERDRONE_POSE_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_RL_ENVIRONMENTS_HYPERDRONE_POSE_H
+
+#include "../l2f/quaternion_helper.h"
+#include "../../../rendering/raytracing/types.h"
+
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools {
+    template <typename T>
+    RL_TOOLS_FUNCTION_PLACEMENT rendering::raytracing::Camera<T> make_camera_data(const T position[3], const T look_at[3], const T up[3], T fov, T aspect);
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools::rl::environments::hyperdrone {
+
+    // orientation is a (w, x, y, z) quaternion mapping the camera's FLU axes into the body frame
+    template <typename T>
+    struct Pose {
+        T position[3] = {0, 0, 0};
+        T orientation[4] = {1, 0, 0, 0};
+    };
+    template <typename T>
+    struct CameraMount {
+        Pose<T> pose;
+    };
+    template <typename T>
+    struct Camera {
+        CameraMount<T> mount;
+        T fov = 63.78166175396324; // horizontal, degrees
+    };
+
+    template <typename T>
+    struct CameraRandomization {
+        T offset_body_range[3] = {0, 0, 0};
+        T rotation_body_range[3] = {0, 0, 0};
+    };
+
+    template <typename T>
+    RL_TOOLS_FUNCTION_PLACEMENT void cross_vector(const T a[3], const T b[3], T out[3]){
+        out[0] = a[1] * b[2] - a[2] * b[1];
+        out[1] = a[2] * b[0] - a[0] * b[2];
+        out[2] = a[0] * b[1] - a[1] * b[0];
+    }
+
+    template <typename T>
+    RL_TOOLS_FUNCTION_PLACEMENT T dot_vector(const T a[3], const T b[3]){
+        return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
+    }
+
+    template <typename DEVICE, typename T>
+    RL_TOOLS_FUNCTION_PLACEMENT void normalize_or(DEVICE& device, T v[3], T fallback_0, T fallback_1, T fallback_2){
+        T norm_sq = dot_vector(v, v);
+        if(norm_sq > static_cast<T>(1e-12)){
+            T inv_norm = static_cast<T>(1) / math::sqrt(device.math, norm_sq);
+            v[0] *= inv_norm;
+            v[1] *= inv_norm;
+            v[2] *= inv_norm;
+        } else {
+            v[0] = fallback_0;
+            v[1] = fallback_1;
+            v[2] = fallback_2;
+        }
+    }
+
+    template <typename DEVICE, typename T>
+    RL_TOOLS_FUNCTION_PLACEMENT void rotate_around_axis(DEVICE& device, const T in[3], const T axis[3], T angle, T out[3]){
+        T c = math::cos(device.math, angle);
+        T s = math::sin(device.math, angle);
+        T axis_cross_in[3];
+        cross_vector(axis, in, axis_cross_in);
+        T axis_dot_in = dot_vector(axis, in);
+        T one_minus_c = static_cast<T>(1) - c;
+        out[0] = in[0] * c + axis_cross_in[0] * s + axis[0] * axis_dot_in * one_minus_c;
+        out[1] = in[1] * c + axis_cross_in[1] * s + axis[1] * axis_dot_in * one_minus_c;
+        out[2] = in[2] * c + axis_cross_in[2] * s + axis[2] * axis_dot_in * one_minus_c;
+    }
+
+    template <typename T>
+    RL_TOOLS_FUNCTION_PLACEMENT void rotate_scene_yaw(const T in[3], T out[3], T scene_yaw_cos, T scene_yaw_sin){
+        out[0] = scene_yaw_cos * in[0] - scene_yaw_sin * in[1];
+        out[1] = scene_yaw_sin * in[0] + scene_yaw_cos * in[1];
+        out[2] = in[2];
+    }
+
+    template <typename DEVICE, typename T>
+    RL_TOOLS_FUNCTION_PLACEMENT void mount_axes(DEVICE&, const CameraMount<T>& mount, T forward_body[3], T up_body[3]){
+        const T forward_camera[3] = {1, 0, 0};
+        const T up_camera[3] = {0, 0, 1};
+        rl::environments::l2f::rotate_vector_by_quaternion<DEVICE, T>(mount.pose.orientation, forward_camera, forward_body);
+        rl::environments::l2f::rotate_vector_by_quaternion<DEVICE, T>(mount.pose.orientation, up_camera, up_body);
+    }
+
+    template <typename DEVICE, typename T, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT void randomize_camera_mount(DEVICE& device, const CameraRandomization<T>& randomization, CameraMount<T>& mount, RNG& rng){
+        for(unsigned axis_i = 0; axis_i < 3; axis_i++){
+            T range = randomization.offset_body_range[axis_i];
+            if(range != static_cast<T>(0)){
+                mount.pose.position[axis_i] += range * random::uniform_real_distribution(device.random, static_cast<T>(-1), static_cast<T>(1), rng);
+            }
+        }
+        // rotations about the fixed body axes x, y, z (applied in that order)
+        for(unsigned axis_i = 0; axis_i < 3; axis_i++){
+            T range = randomization.rotation_body_range[axis_i];
+            if(range != static_cast<T>(0)){
+                T half_angle = range * random::uniform_real_distribution(device.random, static_cast<T>(-1), static_cast<T>(1), rng) / static_cast<T>(2);
+                T rotation[4] = {math::cos(device.math, half_angle), 0, 0, 0};
+                rotation[1 + axis_i] = math::sin(device.math, half_angle);
+                T orientation[4];
+                rl::environments::l2f::quaternion_multiplication<DEVICE, T>(rotation, mount.pose.orientation, orientation);
+                for(unsigned component_i = 0; component_i < 4; component_i++){
+                    mount.pose.orientation[component_i] = orientation[component_i];
+                }
+            }
+        }
+    }
+
+    // scene-under-drone: the dynamics state lives near the origin; scene_translation/scene_yaw
+    // place the drone's frame inside the scene at camera-production time
+    template <typename DEVICE, typename T>
+    RL_TOOLS_FUNCTION_PLACEMENT rendering::raytracing::Camera<T> make_camera(
+        DEVICE& device,
+        const Camera<T>& camera,
+        const T orientation[4],
+        const T state_position[3],
+        T aspect,
+        const T scene_translation[3],
+        T scene_yaw_cos,
+        T scene_yaw_sin
+    ){
+        T forward_body[3];
+        T up_body[3];
+        mount_axes(device, camera.mount, forward_body, up_body);
+        T cam_pos_local[3];
+        rl::environments::l2f::rotate_vector_by_quaternion<DEVICE, T>(orientation, camera.mount.pose.position, cam_pos_local);
+        T cam_forward_local[3];
+        rl::environments::l2f::rotate_vector_by_quaternion<DEVICE, T>(orientation, forward_body, cam_forward_local);
+        T cam_up_local[3];
+        rl::environments::l2f::rotate_vector_by_quaternion<DEVICE, T>(orientation, up_body, cam_up_local);
+
+        T state_position_world[3];
+        rotate_scene_yaw(state_position, state_position_world, scene_yaw_cos, scene_yaw_sin);
+        T cam_pos_world[3];
+        rotate_scene_yaw(cam_pos_local, cam_pos_world, scene_yaw_cos, scene_yaw_sin);
+        T cam_forward_world[3];
+        rotate_scene_yaw(cam_forward_local, cam_forward_world, scene_yaw_cos, scene_yaw_sin);
+        T cam_up_world[3];
+        rotate_scene_yaw(cam_up_local, cam_up_world, scene_yaw_cos, scene_yaw_sin);
+
+        T position[3] = {
+            state_position_world[0] + cam_pos_world[0] + scene_translation[0],
+            state_position_world[1] + cam_pos_world[1] + scene_translation[1],
+            state_position_world[2] + cam_pos_world[2] + scene_translation[2]
+        };
+        T look_at[3] = {
+            position[0] + cam_forward_world[0],
+            position[1] + cam_forward_world[1],
+            position[2] + cam_forward_world[2]
+        };
+        T up[3] = {cam_up_world[0], cam_up_world[1], cam_up_world[2]};
+        return make_camera_data(position, look_at, up, camera.fov, aspect);
+    }
+
+    template <typename DEVICE, typename T>
+    RL_TOOLS_FUNCTION_PLACEMENT rendering::raytracing::Camera<T> make_target_camera(
+        DEVICE& device,
+        const Camera<T>& camera,
+        T aspect,
+        const T scene_translation[3],
+        T scene_yaw_cos,
+        T scene_yaw_sin,
+        T target_frame_roll,
+        T target_frame_pitch
+    ){
+        T offset_world[3];
+        rotate_scene_yaw(camera.mount.pose.position, offset_world, scene_yaw_cos, scene_yaw_sin);
+        T forward_body[3];
+        T up_body[3];
+        mount_axes(device, camera.mount, forward_body, up_body);
+        T pitch_axis[3];
+        cross_vector(forward_body, up_body, pitch_axis);
+        normalize_or(device, pitch_axis, static_cast<T>(0), static_cast<T>(-1), static_cast<T>(0));
+        T pitched_forward_body[3];
+        T pitched_up_body[3];
+        rotate_around_axis(device, forward_body, pitch_axis, target_frame_pitch, pitched_forward_body);
+        rotate_around_axis(device, up_body, pitch_axis, target_frame_pitch, pitched_up_body);
+        T roll_axis[3] = {pitched_forward_body[0], pitched_forward_body[1], pitched_forward_body[2]};
+        normalize_or(device, roll_axis, static_cast<T>(1), static_cast<T>(0), static_cast<T>(0));
+        T rolled_up_body[3];
+        rotate_around_axis(device, pitched_up_body, roll_axis, target_frame_roll, rolled_up_body);
+        T forward_world[3];
+        rotate_scene_yaw(pitched_forward_body, forward_world, scene_yaw_cos, scene_yaw_sin);
+        T up_world[3];
+        rotate_scene_yaw(rolled_up_body, up_world, scene_yaw_cos, scene_yaw_sin);
+
+        T position[3] = {
+            scene_translation[0] + offset_world[0],
+            scene_translation[1] + offset_world[1],
+            scene_translation[2] + offset_world[2]
+        };
+        T look_at[3] = {
+            position[0] + forward_world[0],
+            position[1] + forward_world[1],
+            position[2] + forward_world[2]
+        };
+        T up[3] = {up_world[0], up_world[1], up_world[2]};
+        return make_camera_data(position, look_at, up, camera.fov, aspect);
+    }
+
+    // the body pose (scene-under-drone frame) that carries the mount to make_target_camera's view:
+    // the target perturbation turns the body rigidly about the camera
+    template <typename DEVICE, typename T>
+    RL_TOOLS_FUNCTION_PLACEMENT void make_target_body_pose(DEVICE& device, const Camera<T>& camera, T target_frame_roll, T target_frame_pitch, T orientation[4], T position[3]){
+        T forward_body[3];
+        T up_body[3];
+        mount_axes(device, camera.mount, forward_body, up_body);
+        T pitch_axis[3];
+        cross_vector(forward_body, up_body, pitch_axis);
+        normalize_or(device, pitch_axis, static_cast<T>(0), static_cast<T>(-1), static_cast<T>(0));
+        T roll_axis[3];
+        rotate_around_axis(device, forward_body, pitch_axis, target_frame_pitch, roll_axis);
+        normalize_or(device, roll_axis, static_cast<T>(1), static_cast<T>(0), static_cast<T>(0));
+        const T pitch_sin = math::sin(device.math, target_frame_pitch / 2);
+        const T roll_sin = math::sin(device.math, target_frame_roll / 2);
+        const T pitch_rotation[4] = {math::cos(device.math, target_frame_pitch / 2), pitch_sin * pitch_axis[0], pitch_sin * pitch_axis[1], pitch_sin * pitch_axis[2]};
+        const T roll_rotation[4] = {math::cos(device.math, target_frame_roll / 2), roll_sin * roll_axis[0], roll_sin * roll_axis[1], roll_sin * roll_axis[2]};
+        rl::environments::l2f::quaternion_multiplication<DEVICE, T>(roll_rotation, pitch_rotation, orientation);
+        T mount_turned[3];
+        rl::environments::l2f::rotate_vector_by_quaternion<DEVICE, T>(orientation, camera.mount.pose.position, mount_turned);
+        for(unsigned axis_i = 0; axis_i < 3; axis_i++){
+            position[axis_i] = camera.mount.pose.position[axis_i] - mount_turned[axis_i];
+        }
+    }
+
+    template <typename T>
+    RL_TOOLS_FUNCTION_PLACEMENT rendering::raytracing::Camera<T> interpolate_camera(const rendering::raytracing::Camera<T>& from, const rendering::raytracing::Camera<T>& to, T alpha){
+        rendering::raytracing::Camera<T> out;
+        for(unsigned i = 0; i < 3; i++){
+            out.pos[i] = from.pos[i] + (to.pos[i] - from.pos[i]) * alpha;
+            out.dir_00[i] = from.dir_00[i] + (to.dir_00[i] - from.dir_00[i]) * alpha;
+            out.dir_du[i] = from.dir_du[i] + (to.dir_du[i] - from.dir_du[i]) * alpha;
+            out.dir_dv[i] = from.dir_dv[i] + (to.dir_dv[i] - from.dir_dv[i]) * alpha;
+        }
+        return out;
+    }
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+
+#endif

hyperdrone/_vendor/rl-tools/include/rl_tools/rl/environments/hyperdrone/presets.h

@@ -0,0 +1,154 @@
+#include "../../../version.h"
+#include "../../../rl_tools.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_HYPERDRONE_PRESETS_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_RL_ENVIRONMENTS_HYPERDRONE_PRESETS_H
+
+#include "world.h"
+#include "../l2f/parameters/registry.h"
+#include "../l2f/parameters/init/default.h"
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools::rl::environments::hyperdrone::presets {
+
+    namespace x500_fpv {
+        template <typename T_T, typename T_TI>
+        struct DynamicsStaticParameters {
+            using T = T_T;
+            using TI = T_TI;
+            using REWARD_FUNCTION = l2f::parameters::reward_functions::Squared<T>;
+            static constexpr TI EPISODE_STEP_LIMIT = 500;
+            using PARAMETERS_SPEC = l2f::ParametersBaseSpecification<T, TI, 4, EPISODE_STEP_LIMIT, REWARD_FUNCTION>;
+            using PARAMETERS_TYPE = l2f::ParametersDisturbances<l2f::ParametersSpecification<T, TI, l2f::ParametersBase<PARAMETERS_SPEC>>>;
+            static constexpr auto ACTION_INTERFACE = l2f::parameters::ActionInterface::DIRECT_MOTOR;
+            static constexpr TI N_SUBSTEPS = 1;
+            static constexpr TI ACTION_HISTORY_LENGTH = 1;
+            static constexpr TI CLOSED_FORM = false;
+            using STATE_BASE = l2f::StateBase<l2f::StateSpecification<T, TI>>;
+            using STATE_PLAIN = l2f::StateRotorsHistory<l2f::StateRotorsHistorySpecification<T, TI, ACTION_HISTORY_LENGTH, CLOSED_FORM, l2f::StateRandomForce<l2f::StateSpecification<T, TI, l2f::StateLastAction<l2f::StateSpecification<T, TI, STATE_BASE>>>>>>;
+            using STATE_TYPE = l2f::StateRenderRotorPhase<l2f::StateSpecification<T, TI, STATE_PLAIN>>;
+            using OBSERVATION_TYPE = l2f::observation::Position<l2f::observation::PositionSpecification<T, TI,
+                    l2f::observation::OrientationRotationMatrix<l2f::observation::OrientationRotationMatrixSpecification<T, TI,
+                    l2f::observation::LinearVelocity<l2f::observation::LinearVelocitySpecification<T, TI,
+                    l2f::observation::AngularVelocity<l2f::observation::AngularVelocitySpecification<T, TI>>>>>>>>;
+            using OBSERVATION_TYPE_PRIVILEGED = l2f::observation::Position<l2f::observation::PositionSpecificationPrivileged<T, TI,
+                    l2f::observation::OrientationRotationMatrix<l2f::observation::OrientationRotationMatrixSpecificationPrivileged<T, TI,
+                    l2f::observation::LinearVelocity<l2f::observation::LinearVelocitySpecificationPrivileged<T, TI,
+                    l2f::observation::AngularVelocity<l2f::observation::AngularVelocitySpecificationPrivileged<T, TI>>>>>>>>;
+            static constexpr bool PRIVILEGED_OBSERVATION_NOISE = false;
+            using PARAMETERS = PARAMETERS_TYPE;
+            static constexpr auto dynamics = l2f::parameters::dynamics::registry<l2f::parameters::dynamics::REGISTRY::x500, PARAMETERS_SPEC>;
+            static constexpr typename PARAMETERS_TYPE::Integration integration = {(T)0.01};
+            static constexpr typename PARAMETERS_TYPE::MDP::Initialization init = l2f::parameters::init::init_90_deg<PARAMETERS_SPEC>;
+            static constexpr typename PARAMETERS_TYPE::MDP mdp = {init, REWARD_FUNCTION{}, {}, {}, {}};
+            static constexpr typename PARAMETERS_TYPE::Disturbances disturbances = {{0, 0}, {0, 0}};
+            static constexpr PARAMETERS_TYPE PARAMETER_VALUES = {{dynamics, integration, mdp}, disturbances};
+            static constexpr T STATE_LIMIT_POSITION_X = 100000;
+            static constexpr T STATE_LIMIT_POSITION_Y = 100000;
+            static constexpr T STATE_LIMIT_POSITION_Z = 100000;
+            static constexpr T STATE_LIMIT_VELOCITY_X = 100000;
+            static constexpr T STATE_LIMIT_VELOCITY_Y = 100000;
+            static constexpr T STATE_LIMIT_VELOCITY_Z = 100000;
+            static constexpr T STATE_LIMIT_ANGULAR_VELOCITY_X = 100000;
+            static constexpr T STATE_LIMIT_ANGULAR_VELOCITY_Y = 100000;
+            static constexpr T STATE_LIMIT_ANGULAR_VELOCITY_Z = 100000;
+        };
+    }
+
+    // reference FPV configuration on the x500 platform: spinning-prop render state
+    // (StateRenderRotorPhase) and the drone entity attached to its own cameras — register an
+    // x500 body/prop_* assembly in SharedContext::drones before init. Derive-and-shadow for
+    // further customization, like every hyperdrone Specification
+    template <typename T_T, typename T_TI>
+    struct X500FPV: Specification<T_T, T_TI, x500_fpv::DynamicsStaticParameters<T_T, T_TI>> {
+        using T = T_T;
+        using TI = T_TI;
+        static constexpr TI MAX_ENTITY_SLOTS_PER_INSTANCE = 8;
+        static constexpr bool SELF_VISIBLE = true;
+    };
+
+    namespace x500_fpv_imu {
+        // x500 FPV variant stepping at IMU rate: the integration dt is the IMU sampling period
+        // and StateIMU produces one noised accelerometer/gyroscope sample per step
+        template <typename T_T, typename T_TI>
+        struct DynamicsStaticParameters {
+            using T = T_T;
+            using TI = T_TI;
+            using REWARD_FUNCTION = l2f::parameters::reward_functions::Squared<T>;
+            static constexpr TI IMU_RATE = 200; // Hz
+            static constexpr TI EPISODE_LENGTH_SECONDS = 10;
+            static constexpr TI EPISODE_STEP_LIMIT = EPISODE_LENGTH_SECONDS * IMU_RATE;
+            using PARAMETERS_SPEC = l2f::ParametersBaseSpecification<T, TI, 4, EPISODE_STEP_LIMIT, REWARD_FUNCTION>;
+            using PARAMETERS_TYPE = l2f::ParametersDisturbances<l2f::ParametersSpecification<T, TI, l2f::ParametersIMU<l2f::ParametersSpecification<T, TI, l2f::ParametersBase<PARAMETERS_SPEC>>>>>;
+            static constexpr auto ACTION_INTERFACE = l2f::parameters::ActionInterface::DIRECT_MOTOR;
+            static constexpr TI N_SUBSTEPS = 1;
+            static constexpr TI ACTION_HISTORY_LENGTH = 1;
+            static constexpr TI CLOSED_FORM = false;
+            using STATE_BASE = l2f::StateBase<l2f::StateSpecification<T, TI>>;
+            using STATE_IMU = l2f::StateIMU<T, TI, STATE_BASE>;
+            using STATE_PLAIN = l2f::StateRotorsHistory<l2f::StateRotorsHistorySpecification<T, TI, ACTION_HISTORY_LENGTH, CLOSED_FORM, l2f::StateRandomForce<l2f::StateSpecification<T, TI, l2f::StateLastAction<l2f::StateSpecification<T, TI, STATE_IMU>>>>>>;
+            using STATE_TYPE = l2f::StateRenderRotorPhase<l2f::StateSpecification<T, TI, STATE_PLAIN>>;
+            using OBSERVATION_TYPE = l2f::observation::Position<l2f::observation::PositionSpecification<T, TI,
+                    l2f::observation::OrientationRotationMatrix<l2f::observation::OrientationRotationMatrixSpecification<T, TI,
+                    l2f::observation::LinearVelocity<l2f::observation::LinearVelocitySpecification<T, TI,
+                    l2f::observation::AngularVelocity<l2f::observation::AngularVelocitySpecification<T, TI>>>>>>>>;
+            using OBSERVATION_TYPE_PRIVILEGED = l2f::observation::Position<l2f::observation::PositionSpecificationPrivileged<T, TI,
+                    l2f::observation::OrientationRotationMatrix<l2f::observation::OrientationRotationMatrixSpecificationPrivileged<T, TI,
+                    l2f::observation::LinearVelocity<l2f::observation::LinearVelocitySpecificationPrivileged<T, TI,
+                    l2f::observation::AngularVelocity<l2f::observation::AngularVelocitySpecificationPrivileged<T, TI>>>>>>>>;
+            static constexpr bool PRIVILEGED_OBSERVATION_NOISE = false;
+            using PARAMETERS = PARAMETERS_TYPE;
+            static constexpr auto dynamics = l2f::parameters::dynamics::registry<l2f::parameters::dynamics::REGISTRY::x500, PARAMETERS_SPEC>;
+            static constexpr typename PARAMETERS_TYPE::Integration integration = {(T)1 / (T)IMU_RATE};
+            // rest start: level attitude, zero velocities at t=0 — the benchmark protocol
+            // evaluates the pose relative to the initial pose and lets the estimator assume
+            // zero tilt and zero velocity instead of receiving any ground truth
+            static constexpr typename PARAMETERS_TYPE::MDP::Initialization init = {
+                    1,   // guidance (always guided: origin, level, at rest)
+                    0,   // max_position
+                    0,   // max_angle
+                    0,   // max_linear_velocity
+                    0,   // max_angular_velocity
+                    true,// relative_rpm
+                    0,   // min_rpm
+                    0,   // max_rpm
+            };
+            // MEMS-class per-sample white noise at IMU_RATE (noise density * sqrt(rate))
+            static constexpr T GYRO_NOISE_STD = 0.07;           // rad/s
+            static constexpr T ACCELEROMETER_NOISE_STD = 0.28;  // m/s^2
+            static constexpr typename PARAMETERS_TYPE::MDP mdp = {init, REWARD_FUNCTION{}, {}, {}, {}};
+            static constexpr typename PARAMETERS_TYPE::IMU imu = {
+                    {{ // accelerometer error
+                            {0, ACCELEROMETER_NOISE_STD},     // noise: mean, std [m/s^2]
+                            {(T)0.2, (T)0, (T)0.001}          // bias: turn-on [m/s^2], tau <= 0: random walk, increment density [m/s^2/sqrt(s)]
+                    }},
+                    {{ // gyro error
+                            {0, GYRO_NOISE_STD},              // noise: mean, std [rad/s]
+                            {(T)0.02, (T)0, (T)0.0006}        // bias: turn-on [rad/s], tau <= 0: random walk, increment density [rad/s/sqrt(s)]
+                    }}
+            };
+            static constexpr typename PARAMETERS_TYPE::Disturbances disturbances = {{0, 0}, {0, 0}};
+            static constexpr PARAMETERS_TYPE PARAMETER_VALUES = {{{dynamics, integration, mdp}, imu}, disturbances};
+            static constexpr T STATE_LIMIT_POSITION_X = 100000;
+            static constexpr T STATE_LIMIT_POSITION_Y = 100000;
+            static constexpr T STATE_LIMIT_POSITION_Z = 100000;
+            static constexpr T STATE_LIMIT_VELOCITY_X = 100000;
+            static constexpr T STATE_LIMIT_VELOCITY_Y = 100000;
+            static constexpr T STATE_LIMIT_VELOCITY_Z = 100000;
+            static constexpr T STATE_LIMIT_ANGULAR_VELOCITY_X = 100000;
+            static constexpr T STATE_LIMIT_ANGULAR_VELOCITY_Y = 100000;
+            static constexpr T STATE_LIMIT_ANGULAR_VELOCITY_Z = 100000;
+        };
+    }
+
+    template <typename T_T, typename T_TI>
+    struct X500FPVIMU: Specification<T_T, T_TI, x500_fpv_imu::DynamicsStaticParameters<T_T, T_TI>> {
+        using T = T_T;
+        using TI = T_TI;
+        static constexpr TI MAX_ENTITY_SLOTS_PER_INSTANCE = 8;
+        static constexpr bool SELF_VISIBLE = true;
+    };
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+
+#endif

hyperdrone/_vendor/rl-tools/include/rl_tools/rl/environments/hyperdrone/raptor/operations_cpu.h

@@ -0,0 +1,26 @@
+#include "../../../../version.h"
+#include "../../../../rl_tools.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_HYPERDRONE_RAPTOR_OPERATIONS_CPU_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_RL_ENVIRONMENTS_HYPERDRONE_RAPTOR_OPERATIONS_CPU_H
+
+#include "operations_generic.h"
+#include "../../../../persist/backends/tar/operations_cpu.h"
+#include "../../../../nn/layers/dense/persist.h"
+#include "../../../../nn/layers/gru/persist.h"
+#include "../../../../nn_models/sequential/persist.h"
+
+#include <string>
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools {
+    template <typename DEVICE, typename SPEC>
+    bool load(DEVICE& device, rl::environments::hyperdrone::Raptor<SPEC>& policy, const std::string& path) {
+        persist::backends::tar::File<typename SPEC::TI> checkpoint(path, persist::backends::tar::Mode::READ);
+        auto actor = get_group(device, checkpoint, "actor");
+        return load(device, policy.model, actor);
+    }
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+
+#endif

hyperdrone/_vendor/rl-tools/include/rl_tools/rl/environments/hyperdrone/raptor/operations_generic.h

@@ -0,0 +1,91 @@
+#include "../../../../version.h"
+#include "../../../../rl_tools.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_HYPERDRONE_RAPTOR_OPERATIONS_GENERIC_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_RL_ENVIRONMENTS_HYPERDRONE_RAPTOR_OPERATIONS_GENERIC_H
+
+#include "raptor.h"
+#include "../../l2f/quaternion_helper.h"
+#include "../../../../nn/layers/dense/operations_generic.h"
+#include "../../../../nn/layers/gru/operations_generic.h"
+#include "../../../../nn_models/sequential/operations_generic.h"
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools {
+    template <typename DEVICE, typename SPEC>
+    void malloc(DEVICE& device, rl::environments::hyperdrone::Raptor<SPEC>& policy) {
+        malloc(device, policy.model);
+        malloc(device, policy.state);
+        malloc(device, policy.buffer);
+        malloc(device, policy.observations);
+        malloc(device, policy.actions);
+    }
+
+    template <typename DEVICE, typename SPEC>
+    void free(DEVICE& device, rl::environments::hyperdrone::Raptor<SPEC>& policy) {
+        free(device, policy.model);
+        free(device, policy.state);
+        free(device, policy.buffer);
+        free(device, policy.observations);
+        free(device, policy.actions);
+    }
+
+    template <typename DEVICE, typename SPEC, typename RNG>
+    void reset(DEVICE& device, rl::environments::hyperdrone::Raptor<SPEC>& policy, RNG& rng) {
+        reset(device, policy.model, policy.state, rng);
+    }
+
+    template <typename DEVICE, typename SPEC, typename RNG>
+    void evaluate_step(DEVICE& device, rl::environments::hyperdrone::Raptor<SPEC>& policy, RNG& rng) {
+        Mode<nn::layers::gru::NoAutoResetMode<mode::Default<>>> mode;
+        evaluate_step(device, policy.model, policy.observations, policy.state, policy.actions, policy.buffer, rng, mode);
+    }
+
+    template <typename DEVICE, typename SPEC, typename STATE>
+    RL_TOOLS_FUNCTION_PLACEMENT void update_yaw_target(DEVICE& device, const rl::environments::hyperdrone::Raptor<SPEC>&, const STATE& state, rl::environments::hyperdrone::raptor::Target<typename SPEC::T>& target, typename SPEC::T yaw_delta, typename SPEC::T dt, typename SPEC::T yaw_rate_limit = SPEC::YAW_RATE_LIMIT, typename SPEC::T yaw_error_limit = SPEC::YAW_ERROR_LIMIT) {
+        using T = typename SPEC::T;
+        const auto& q = state.orientation;
+        const T yaw = math::atan2(device.math, (T)2 * (q[0] * q[3] + q[1] * q[2]), (T)1 - (T)2 * (q[2] * q[2] + q[3] * q[3]));
+        const T max_delta = yaw_rate_limit * dt;
+        const T requested_error = target.yaw + math::clamp(device.math, yaw_delta, -max_delta, max_delta) - yaw;
+        const T error = math::atan2(device.math, math::sin(device.math, requested_error), math::cos(device.math, requested_error));
+        const T limited_yaw = yaw + math::clamp(device.math, error, -yaw_error_limit, yaw_error_limit);
+        target.yaw = math::atan2(device.math, math::sin(device.math, limited_yaw), math::cos(device.math, limited_yaw));
+    }
+
+    template <typename DEVICE, typename SPEC, typename STATE>
+    RL_TOOLS_FUNCTION_PLACEMENT void observe(DEVICE& device, rl::environments::hyperdrone::Raptor<SPEC>& policy, const STATE& state, const rl::environments::hyperdrone::raptor::Target<typename SPEC::T>& target, const typename SPEC::T previous_action[4], typename SPEC::TI instance_i) {
+        using T = typename SPEC::T;
+        using TI = typename SPEC::TI;
+        using namespace rl::environments::l2f;
+        T inverse_yaw[4] = {math::cos(device.math, target.yaw / (T)2), 0, 0, -math::sin(device.math, target.yaw / (T)2)};
+        T position_error[3];
+        T velocity_error[3];
+        for (TI axis_i = 0; axis_i < 3; axis_i++) {
+            position_error[axis_i] = state.position[axis_i] - target.position[axis_i];
+            velocity_error[axis_i] = state.linear_velocity[axis_i] - target.linear_velocity[axis_i];
+        }
+        T rotated_position[3];
+        T rotated_velocity[3];
+        T orientation[4];
+        T rotation[3][3];
+        rotate_vector_by_quaternion<DEVICE, T>(inverse_yaw, position_error, rotated_position);
+        rotate_vector_by_quaternion<DEVICE, T>(inverse_yaw, velocity_error, rotated_velocity);
+        quaternion_multiplication<DEVICE, T>(inverse_yaw, state.orientation, orientation);
+        quaternion_to_rotation_matrix<DEVICE, T>(orientation, rotation);
+        for (TI axis_i = 0; axis_i < 3; axis_i++) {
+            set(device, policy.observations, math::clamp(device.math, rotated_position[axis_i], -SPEC::POSITION_CLIP, SPEC::POSITION_CLIP), instance_i, axis_i);
+            set(device, policy.observations, math::clamp(device.math, rotated_velocity[axis_i], -SPEC::VELOCITY_CLIP, SPEC::VELOCITY_CLIP), instance_i, 12 + axis_i);
+            set(device, policy.observations, state.angular_velocity[axis_i], instance_i, 15 + axis_i);
+            for (TI column_i = 0; column_i < 3; column_i++) {
+                set(device, policy.observations, rotation[axis_i][column_i], instance_i, 3 + 3 * axis_i + column_i);
+            }
+        }
+        for (TI action_i = 0; action_i < 4; action_i++) {
+            set(device, policy.observations, previous_action[action_i], instance_i, 18 + action_i);
+        }
+    }
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+
+#endif

hyperdrone/_vendor/rl-tools/include/rl_tools/rl/environments/hyperdrone/raptor/raptor.h

@@ -0,0 +1,71 @@
+#include "../../../../version.h"
+#include "../../../../rl_tools.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_HYPERDRONE_RAPTOR_RAPTOR_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_RL_ENVIRONMENTS_HYPERDRONE_RAPTOR_RAPTOR_H
+
+#include "../../l2f/multirotor.h"
+#include "../../../../numeric_types/policy.h"
+#include "../../../../nn/layers/dense/layer.h"
+#include "../../../../nn/layers/gru/layer.h"
+#include "../../../../nn_models/sequential/model.h"
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools::rl::environments::hyperdrone {
+    namespace raptor {
+        inline constexpr const char* CHECKPOINT = "conta:b6bd082c60fcb27f42a2b5f2f635587d2b60d170";
+
+        template <typename T_TYPE_POLICY, typename T_TI, T_TI T_INSTANCES>
+        struct Specification {
+            using TYPE_POLICY = T_TYPE_POLICY;
+            using T = typename TYPE_POLICY::DEFAULT;
+            using TI = T_TI;
+            static constexpr TI INSTANCES = T_INSTANCES;
+            static constexpr TI INPUT_DIM = 22;
+            static constexpr TI HIDDEN_DIM = 16;
+            static constexpr TI OUTPUT_DIM = 4;
+            static constexpr T POSITION_CLIP = 1;
+            static constexpr T VELOCITY_CLIP = 2;
+            static constexpr T YAW_ERROR_LIMIT = 0.25;
+            static constexpr T YAW_RATE_LIMIT = 1;
+            using INPUT_LAYER = nn::layers::dense::BindConfiguration<nn::layers::dense::Configuration<TYPE_POLICY, TI, HIDDEN_DIM, nn::activation_functions::ActivationFunction::RELU, nn::layers::dense::DefaultInitializer<TYPE_POLICY, TI>, nn::parameters::groups::Input>>;
+            using RECURRENT_LAYER = nn::layers::gru::BindConfiguration<nn::layers::gru::Configuration<TYPE_POLICY, TI, HIDDEN_DIM, nn::parameters::groups::Normal, false>>;
+            using OUTPUT_LAYER = nn::layers::dense::BindConfiguration<nn::layers::dense::Configuration<TYPE_POLICY, TI, OUTPUT_DIM, nn::activation_functions::ActivationFunction::IDENTITY, nn::layers::dense::DefaultInitializer<TYPE_POLICY, TI>, nn::parameters::groups::Output>>;
+            using MODULE_CHAIN = nn_models::sequential::Module<INPUT_LAYER, RECURRENT_LAYER, OUTPUT_LAYER>;
+        };
+
+        // Targets use the dynamics frame; yaw defines the policy's reference heading.
+        template <typename T>
+        struct Target {
+            T position[3] = {};
+            T linear_velocity[3] = {};
+            T yaw = 0;
+        };
+    }
+
+    template <typename T_SPEC>
+    struct Raptor {
+        using SPEC = T_SPEC;
+        using T = typename SPEC::T;
+        using TI = typename SPEC::TI;
+        static constexpr TI INSTANCES = SPEC::INSTANCES;
+        using OBSERVATION_TYPE = l2f::observation::Position<l2f::observation::PositionSpecificationPrivileged<T, TI,
+                l2f::observation::OrientationRotationMatrix<l2f::observation::OrientationRotationMatrixSpecificationPrivileged<T, TI,
+                l2f::observation::LinearVelocity<l2f::observation::LinearVelocitySpecificationPrivileged<T, TI,
+                l2f::observation::AngularVelocity<l2f::observation::AngularVelocitySpecificationPrivileged<T, TI,
+                l2f::observation::ActionHistory<l2f::observation::ActionHistorySpecification<T, TI, 1>>>>>>>>>>;
+        static constexpr TI OBSERVATION_DIM = OBSERVATION_TYPE::DIM;
+        static constexpr TI ACTION_DIM = SPEC::OUTPUT_DIM;
+        static_assert(OBSERVATION_DIM == SPEC::INPUT_DIM);
+        static_assert(INSTANCES > 0);
+        using MODEL = nn_models::sequential::Build<nn::capability::Forward<true, false>, typename SPEC::MODULE_CHAIN, tensor::Shape<TI, 1, INSTANCES, OBSERVATION_DIM>>;
+        MODEL model;
+        typename MODEL::template State<true> state;
+        typename MODEL::template Buffer<true> buffer;
+        Tensor<tensor::Specification<T, TI, tensor::Shape<TI, INSTANCES, OBSERVATION_DIM>>> observations;
+        Tensor<tensor::Specification<T, TI, tensor::Shape<TI, INSTANCES, ACTION_DIM>>> actions;
+    };
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+
+#endif