hyperverse 0.0.1__py3-none-any.whl

hyperdrone/_vendor/rl-tools/include/rl_tools/rl/environments/l2f/parameters/dynamics/x500_sim.h

@@ -0,0 +1,114 @@
+#include "../../../../../version.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_L2F_PARAMETERS_DYNAMICS_X500_SIM_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_RL_ENVIRONMENTS_L2F_PARAMETERS_DYNAMICS_X500_SIM_H
+#include "../../multirotor.h"
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools::rl::environments::l2f::parameters::dynamics{
+    namespace x500{
+        template<typename T, typename TI>
+        constexpr Dynamics<T, TI, 4> sim = {
+            // Rotor positions
+            {
+                {
+                    +0.176776695296636,
+                    -0.176776695296636,
+                    0
+
+                },
+                {
+                    -0.176776695296636,
+                    +0.176776695296636,
+                    0
+
+                },
+                {
+                    +0.176776695296636,
+                    +0.176776695296636,
+                    0
+                },
+                {
+                    -0.176776695296636,
+                    -0.176776695296636,
+                    0
+                },
+            },
+            // Rotor thrust directions
+            {
+                {0, 0, 1},
+                {0, 0, 1},
+                {0, 0, 1},
+                {0, 0, 1},
+            },
+            // Rotor torque directions
+            {
+                {0, 0, -1},
+                {0, 0, -1},
+                {0, 0, +1},
+                {0, 0, +1},
+            },
+            // thrust constants
+            {
+                {0, 0, 8.74},
+                {0, 0, 8.74},
+                {0, 0, 8.74},
+                {0, 0, 8.74}
+            },
+            // torque constant
+//            0.025126582278481014,
+            {0.11697849233439939, 0.11697849233439939, 0.11697849233439939, 0.11697849233439939},
+            // T, RPM time constant
+            {0.03, 0.03, 0.03, 0.03},
+            {0.03, 0.03, 0.03, 0.03},
+            // mass vehicle
+            2.000,
+            // gravity
+            {0, 0, -9.81},
+            // J
+            {
+                {
+                    0.0216666666666666,
+                    0.0000000000000000000000000000000000000000,
+                    0.0000000000000000000000000000000000000000
+                },
+                {
+                    0.0000000000000000000000000000000000000000,
+                    0.0216666666666666,
+                    0.0000000000000000000000000000000000000000
+                },
+                {
+                    0.0000000000000000000000000000000000000000,
+                    0.0000000000000000000000000000000000000000,
+                    0.04
+                }
+            },
+            // J_inv
+            {
+                {
+                    46.153846153846295,
+                    0.0000000000000000000000000000000000000000,
+                    0.0000000000000000000000000000000000000000
+                },
+                {
+                    0.0000000000000000000000000000000000000000,
+                    46.153846153846295,
+                    0.0000000000000000000000000000000000000000
+                },
+                {
+                    0.0000000000000000000000000000000000000000,
+                    0.0000000000000000000000000000000000000000,
+                    25
+                }
+            },
+            // hovering throttle (julia): sqrt((mass * 9.81/4 - thrust_curve[1])/thrust_curve[3]),
+            0.749141384950337, //julia sqrt((mass * 9.81/4 - thrust_curve[1])/thrust_curve[3])
+            // action limit
+            {0.0, 1.0},
+        };
+
+    }
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+
+#endif

hyperdrone/_vendor/rl-tools/include/rl_tools/rl/environments/l2f/parameters/init/default.h

@@ -0,0 +1,56 @@
+#include "../../../../../version.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_L2F_PARAMETERS_INIT_DEFAULT_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_RL_ENVIRONMENTS_L2F_PARAMETERS_INIT_DEFAULT_H
+
+#include "../../multirotor.h"
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools::rl::environments::l2f::parameters::init{
+    template<typename SPEC>
+    constexpr typename ParametersBase<SPEC>::MDP::Initialization init_20_deg = {
+            0.1, // guidance
+            0.5, // position
+            1.5707963267948966/90.0 * 20.0,   // orientation
+            1,   // linear velocity
+            1,  // angular velocity
+            true,// relative rpm
+            -1,  // min rpm
+            +1,  // max rpm
+    };
+    template<typename SPEC>
+    constexpr typename ParametersBase<SPEC>::MDP::Initialization init_90_deg = {
+            0.1, // guidance
+            0.5, // position
+            1.5707963267948966,   // orientation
+            1,   // linear velocity
+            1,  // angular velocity
+            true,// relative rpm
+            -1,  // min rpm
+            0,  // max rpm
+    };
+    template<typename SPEC>
+    constexpr typename ParametersBase<SPEC>::MDP::Initialization init_180_deg = {
+            0.1, // guidance
+            0.5, // position
+            2*1.5707963267948966,   // orientation
+            1,   // linear velocity
+            1,  // angular velocity
+            true,// relative rpm
+            -1,  // min rpm
+            +1,  // max rpm
+    };
+    template<typename SPEC>
+    constexpr typename ParametersBase<SPEC>::MDP::Initialization init_0_deg = {
+            0.1, // guidance
+            0.5, // position
+            0,   // orientation
+            1,   // linear velocity
+            1,  // angular velocity
+            true,// relative rpm
+            -1,  // min rpm
+            +1,  // max rpm
+    };
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+#endif

hyperdrone/_vendor/rl-tools/include/rl_tools/rl/environments/l2f/parameters/registry.h

@@ -0,0 +1,96 @@
+#include "../../../../version.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_L2F_PARAMETERS_REGISTRY_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_RL_ENVIRONMENTS_L2F_PARAMETERS_REGISTRY_H
+
+#include "dynamics/crazyflie.h"
+#include "dynamics/crazyflie_openmv.h"
+#include "dynamics/mrs.h"
+#include "dynamics/arpl.h"
+#include "dynamics/x500.h"
+#include "dynamics/x500_real.h"
+#include "dynamics/x500_sim.h"
+#include "dynamics/fs.h"
+#include "dynamics/flightmare.h"
+#include "dynamics/soft.h"
+#include "dynamics/soft_rigid.h"
+
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools::rl::environments::l2f::parameters{
+    namespace dynamics{
+        enum class REGISTRY{
+            crazyflie,
+            crazyflie_openmv,
+            mrs,
+            x500,
+            x500_real,
+            x500_sim,
+            arpl,
+            fs_base,
+            flightmare,
+            soft,
+            soft_rigid,
+        };
+        template <REGISTRY MODEL, typename SPEC>
+        constexpr auto registry = [](){
+            if constexpr (MODEL == REGISTRY::crazyflie){
+                return dynamics::crazyflie<typename SPEC::T, typename SPEC::TI>;
+            }else if constexpr (MODEL == REGISTRY::crazyflie_openmv){
+                return dynamics::crazyflie_openmv<typename SPEC::T, typename SPEC::TI>;
+            }else if constexpr (MODEL == REGISTRY::mrs){
+                return dynamics::mrs<typename SPEC::T, typename SPEC::TI>;
+            }else if constexpr (MODEL == REGISTRY::x500){
+                return dynamics::x500::remap<typename SPEC::T, typename SPEC::TI>;
+            }else if constexpr (MODEL == REGISTRY::x500_real){
+                return dynamics::x500::real<typename SPEC::T, typename SPEC::TI>;
+            }else if constexpr (MODEL == REGISTRY::x500_sim){
+                return dynamics::x500::sim<typename SPEC::T, typename SPEC::TI>;
+            }else if constexpr (MODEL == REGISTRY::arpl){
+                return dynamics::arpl<typename SPEC::T, typename SPEC::TI>;
+            }else if constexpr (MODEL == REGISTRY::fs_base){
+                return dynamics::fs::base<typename SPEC::T, typename SPEC::TI>;
+            }else if constexpr (MODEL == REGISTRY::flightmare){
+                return dynamics::flightmare<typename SPEC::T, typename SPEC::TI>;
+            }else if constexpr (MODEL == REGISTRY::soft){
+                return dynamics::soft<typename SPEC::T, typename SPEC::TI>;
+            }else if constexpr (MODEL == REGISTRY::soft_rigid){
+                return dynamics::soft_rigid<typename SPEC::T, typename SPEC::TI>;
+            }else{
+                static_assert(rl_tools::utils::typing::dependent_false<SPEC>, "Unknown model");
+            }
+        }();
+
+        template <auto THING>
+        struct Dependent{};
+
+        template <REGISTRY MODEL>
+        constexpr auto registry_name = [](){
+            if constexpr (MODEL == REGISTRY::crazyflie){
+                return "crazyflie";
+            }else if constexpr (MODEL == REGISTRY::crazyflie_openmv){
+                return "crazyflie_openmv";
+            }else if constexpr (MODEL == REGISTRY::mrs){
+                return "mrs";
+            }else if constexpr (MODEL == REGISTRY::x500){
+                return "x500";
+            }else if constexpr (MODEL == REGISTRY::x500_real){
+                return "x500_real";
+            }else if constexpr (MODEL == REGISTRY::x500_sim){
+                return "x500_sim";
+            }else if constexpr (MODEL == REGISTRY::fs_base){
+                return "fs_base";
+            }else if constexpr (MODEL == REGISTRY::flightmare){
+                return "flightmare";
+            }else if constexpr (MODEL == REGISTRY::soft){
+                return "soft";
+            }else if constexpr (MODEL == REGISTRY::soft_rigid){
+                return "soft_rigid";
+            }else{
+                static_assert(rl_tools::utils::typing::dependent_false<Dependent<MODEL>>, "Unknown model");
+            }
+        }();
+    }
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+#endif

hyperdrone/_vendor/rl-tools/include/rl_tools/rl/environments/l2f/parameters/reward_functions/default.h

@@ -0,0 +1,64 @@
+#include "../../../../../version.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_L2F_PARAMETERS_REWARD_FUNCTIONS_DEFAULT_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_RL_ENVIRONMENTS_L2F_PARAMETERS_REWARD_FUNCTIONS_DEFAULT_H
+
+#include "../../multirotor.h"
+#include "./squared/squared.h"
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools::rl::environments::l2f::parameters::reward_functions{
+    template<typename T>
+    constexpr Squared<T> constant = {
+            false, // non-negative
+            0.1, // scale
+            1, // constant
+            0, // termination penalty
+            0, // position
+            0, // position_clip
+            0, // orientation
+            0, // linear_velocity
+            0, // angular_velocity
+            0, // linear_acceleration
+            0, // angular_acceleration
+            {0, 0, 0, 0}, // action
+            {0.0, 0.0, 0.0, 0.0}, // d_action
+            0.0 // Position error integral
+    };
+    template<typename T>
+    constexpr Squared<T> squared = {
+            false, // non-negative
+            0.1, // scale
+            1.0, // constant
+            0, // termination penalty
+            10, // position
+            0, // position_clip
+            2.5, // orientation
+            0.05, // linear_velocity
+            0, // angular_velocity
+            0, // linear_acceleration
+            0, // angular_acceleration
+            {0.1, 0.1, 0.1, 0.1}, // action
+            {0.0, 0.0, 0.0, 0.0}, // d_action
+            0.0 // Position error integral
+    };
+    template<typename T>
+    constexpr Squared<T> squared_no_orientation = {
+            false, // non-negative
+            0.1, // scale
+            1, // constant
+            0, // termination penalty
+            10, // position
+            0, // position_clip
+            0, // orientation
+            1, // linear_velocity
+            0, // angular_velocity
+            0, // linear_acceleration
+            0, // angular_acceleration
+            {0.0, 0.0, 0.0, 0.0}, // action
+            {0.0, 0.0, 0.0, 0.0}, // d_action
+            0.0 // Position error integral
+    };
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+
+#endif

hyperdrone/_vendor/rl-tools/include/rl_tools/rl/environments/l2f/parameters/reward_functions/reward_functions.h

@@ -0,0 +1 @@
+#include "squared/squared.h"

hyperdrone/_vendor/rl-tools/include/rl_tools/rl/environments/l2f/parameters/reward_functions/squared/operations_generic.h

@@ -0,0 +1,202 @@
+#include "../../../../../../version.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_L2F_PARAMETERS_REWARD_FUNCTIONS_SQUARED_OPERATIONS_GENERIC_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_RL_ENVIRONMENTS_L2F_PARAMETERS_REWARD_FUNCTIONS_SQUARED_OPERATIONS_GENERIC_H
+
+#include "../../../multirotor.h"
+#include <rl_tools/utils/generic/vector_operations.h>
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools::rl::environments::l2f::parameters::reward_functions{
+    template<typename DEVICE, typename SPEC, typename PARAMETERS, typename STATE_SPEC, typename STATE, typename ACTION_SPEC, typename T, unsigned T_ACTION_DIM, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT void _reward_components(DEVICE& device, const Multirotor<SPEC>& env, const PARAMETERS& parameters, const Squared<T, T_ACTION_DIM>& reward_parameters, const StateBase<STATE_SPEC>& state_dispatch, const STATE& state, const Matrix<ACTION_SPEC>& action,  const STATE& next_state, typename Squared<T, T_ACTION_DIM>::Components& components, RNG& rng){
+        using TI = typename DEVICE::index_t;
+        constexpr TI ACTION_DIM = rl::environments::Multirotor<SPEC>::ACTION_DIM;
+        static_assert(ACTION_DIM == T_ACTION_DIM);
+        STATE desired_state;
+        get_desired_state(device, env, parameters, state, desired_state, rng);
+//        components.orientation_cost = 1 - state.orientation[0] * state.orientation[0]; //math::abs(device.math, 2 * math::acos(device.math, quaternion_w));
+        components.orientation_cost = 2*math::acos(device.math, 1-math::abs(device.math, state.orientation[3]));
+        T x = state.position[0] - desired_state.position[0];
+        T y = state.position[1] - desired_state.position[1];
+        T z = state.position[2] - desired_state.position[2];
+        components.position_cost = math::sqrt(device.math, x*x + y*y + z*z);
+        if(reward_parameters.position_clip > 0){
+            components.position_cost = math::min(device.math, components.position_cost, reward_parameters.position_clip);
+        }
+        T vx = state.linear_velocity[0] - desired_state.linear_velocity[0];
+        T vy = state.linear_velocity[1] - desired_state.linear_velocity[1];
+        T vz = state.linear_velocity[2] - desired_state.linear_velocity[2];
+        components.linear_vel_cost = math::sqrt(device.math, vx*vx + vy*vy + vz*vz);
+        components.angular_vel_cost = math::sqrt(device.math, state.angular_velocity[0] * state.angular_velocity[0] + state.angular_velocity[1] * state.angular_velocity[1] + state.angular_velocity[2] * state.angular_velocity[2]);
+        T linear_acc[3];
+        T angular_acc[3];
+        rl_tools::utils::vector_operations::sub<DEVICE, T, 3>(next_state.linear_velocity, state.linear_velocity, linear_acc);
+        components.linear_acc_cost = math::sqrt(device.math, linear_acc[0] * linear_acc[0] + linear_acc[1] * linear_acc[1] + linear_acc[2] * linear_acc[2]) / parameters.integration.dt;
+        rl_tools::utils::vector_operations::sub<DEVICE, T, 3>(next_state.angular_velocity, state.angular_velocity, angular_acc);
+        components.angular_acc_cost = math::sqrt(device.math, angular_acc[0] * angular_acc[0] + angular_acc[1] * angular_acc[1] + angular_acc[2] * angular_acc[2]) / parameters.integration.dt;
+
+        components.action_cost = 0;
+        components.weighted_action_cost = 0;
+        for(TI action_i = 0; action_i < ACTION_DIM; action_i++){
+            T normalized_action = math::clamp(device.math, get(action, 0, action_i), (T)-1, (T)1);
+            T action_diff;
+            if constexpr(SPEC::STATIC_PARAMETERS::ACTION_INTERFACE == rl_tools::rl::environments::l2f::parameters::ActionInterface::CTBR){
+                if(action_i == 0){
+                    T collective = (normalized_action + (T)1) / (T)2;
+                    collective = parameters.ctbr_controller.thrust_min + collective * (parameters.ctbr_controller.thrust_max - parameters.ctbr_controller.thrust_min);
+                    action_diff = collective - parameters.dynamics.hovering_throttle_relative;
+                }
+                else{
+                    action_diff = normalized_action;
+                }
+            }
+            else{
+                T action_throttle_relative = (normalized_action + (T)1) / (T)2;
+                action_diff = action_throttle_relative - parameters.dynamics.hovering_throttle_relative;
+            }
+            T action_cost = action_diff * action_diff;
+            T weighted_action_cost = reward_parameters.action[action_i] * action_cost;
+            components.action_costs[action_i] = action_cost;
+            components.weighted_action_costs[action_i] = weighted_action_cost;
+            components.action_cost += action_cost;
+            components.weighted_action_cost += weighted_action_cost;
+        }
+    }
+    template<typename DEVICE, typename SPEC, typename PARAMETERS, typename STATE_SPEC, typename STATE, typename ACTION_SPEC, typename T, unsigned T_ACTION_DIM, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT void _reward_components(DEVICE& device, const Multirotor<SPEC>& env, const PARAMETERS& parameters, const Squared<T, T_ACTION_DIM>& reward_parameters, const StateLastAction<STATE_SPEC>& state_dispatch, const STATE& state, const Matrix<ACTION_SPEC>& action,  const STATE& next_state, typename Squared<T, T_ACTION_DIM>::Components& components, RNG& rng) {
+        using TI = typename DEVICE::index_t;
+        constexpr TI ACTION_DIM = rl::environments::Multirotor<SPEC>::ACTION_DIM;
+        static_assert(ACTION_DIM == T_ACTION_DIM);
+        _reward_components(device, env, parameters, reward_parameters, static_cast<const typename STATE_SPEC::NEXT_COMPONENT&>(state), state, action, next_state, components, rng);
+        components.d_action_cost = 0;
+        components.weighted_d_action_cost = 0;
+        for(TI action_i = 0; action_i < ACTION_DIM; action_i++){
+            T normalized_action = math::clamp(device.math, get(action, 0, action_i), (T)-1, (T)1);
+            T d_action_value = normalized_action - state.last_action[action_i];
+            T d_action_cost = d_action_value * d_action_value;
+            T weighted_d_action_cost = reward_parameters.d_action[action_i] * d_action_cost;
+            components.d_action_costs[action_i] = d_action_cost;
+            components.weighted_d_action_costs[action_i] = weighted_d_action_cost;
+            components.d_action_cost += d_action_cost;
+            components.weighted_d_action_cost += weighted_d_action_cost;
+        }
+    }
+    template<typename DEVICE, typename SPEC, typename PARAMETERS, typename STATE_SPEC, typename STATE, typename ACTION_SPEC, typename T, unsigned T_ACTION_DIM, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT void _reward_components(DEVICE& device, const Multirotor<SPEC>& env, const PARAMETERS& parameters, const Squared<T, T_ACTION_DIM>& reward_parameters, const StatePoseErrorIntegral<STATE_SPEC>& state_dispatch, const STATE& state, const Matrix<ACTION_SPEC>& action,  const STATE& next_state, typename Squared<T, T_ACTION_DIM>::Components& components, RNG& rng){
+        _reward_components(device, env, parameters, reward_parameters, static_cast<const typename STATE_SPEC::NEXT_COMPONENT&>(state), state, action, next_state, components, rng);
+        static constexpr T EPSILON = 1e-4;
+        components.position_error_integral_cost = math::sqrt(device.math, next_state.position_integral[0]*next_state.position_integral[0] + next_state.position_integral[1]*next_state.position_integral[1] + next_state.position_integral[2]*next_state.position_integral[2] + EPSILON);
+    }
+    template<typename DEVICE, typename SPEC, typename PARAMETERS, typename STATE, typename ACTION_SPEC, typename T, unsigned T_ACTION_DIM, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT void reward_components(DEVICE& device, const Multirotor<SPEC>& env, const PARAMETERS& parameters, const Squared<T, T_ACTION_DIM>& reward_parameters, const STATE& state, const Matrix<ACTION_SPEC>& action,  const STATE& next_state, typename Squared<T, T_ACTION_DIM>::Components& components, RNG& rng){
+        using TI = typename DEVICE::index_t;
+        constexpr TI ACTION_DIM = rl::environments::Multirotor<SPEC>::ACTION_DIM;
+        components.d_action_cost = 0;
+        components.weighted_d_action_cost = 0;
+        components.position_error_integral_cost = 0;
+        for(TI action_i = 0; action_i < ACTION_DIM; action_i++){
+            components.d_action_costs[action_i] = 0;
+            components.weighted_d_action_costs[action_i] = 0;
+        }
+        _reward_components(device, env, parameters, reward_parameters, state, state, action, next_state, components, rng);
+        components.weighted_cost = 0;
+        components.weighted_cost += reward_parameters.position * components.position_cost;
+        components.weighted_cost += reward_parameters.orientation * components.orientation_cost;
+        components.weighted_cost += reward_parameters.linear_velocity * components.linear_vel_cost;
+        components.weighted_cost += reward_parameters.angular_velocity * components.angular_vel_cost;
+        components.weighted_cost += reward_parameters.linear_acceleration * components.linear_acc_cost;
+        components.weighted_cost += reward_parameters.angular_acceleration * components.angular_acc_cost;
+        components.weighted_cost += components.weighted_action_cost;
+        components.weighted_cost += components.weighted_d_action_cost;
+        components.weighted_cost += reward_parameters.position_error_integral * components.position_error_integral_cost;
+
+        bool terminated_flag = terminated(device, env, parameters, next_state, rng);
+        components.scaled_weighted_cost = reward_parameters.scale * components.weighted_cost;
+
+        if(terminated_flag){
+            components.reward = reward_parameters.termination_penalty;
+        }
+        else{
+            components.reward = -components.scaled_weighted_cost + reward_parameters.constant;
+            components.reward = (components.reward > 0 || !reward_parameters.non_negative) ? components.reward : 0;
+        }
+    }
+    template<typename DEVICE, typename SPEC, typename PARAMETERS, typename STATE, typename ACTION_SPEC, typename T, unsigned T_ACTION_DIM, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT void log_reward(DEVICE& device, const Multirotor<SPEC>& env, const PARAMETERS& parameters, const Squared<T, T_ACTION_DIM>& reward_parameters, const STATE& state, const Matrix<ACTION_SPEC>& action,  const STATE& next_state, RNG& rng, typename DEVICE::index_t cadence = 1){
+        using TI = typename DEVICE::index_t;
+        constexpr TI ACTION_DIM = rl::environments::Multirotor<SPEC>::ACTION_DIM;
+        constexpr TI LOGGED_ACTION_DIM = ACTION_DIM < 10 ? ACTION_DIM : 10;
+        typename Squared<T, T_ACTION_DIM>::Components components;
+        reward_components(device, env, parameters, reward_parameters, state, action, next_state, components, rng);
+        add_scalar(device, device.logger, "reward/orientation_cost", components.orientation_cost, cadence);
+        add_scalar(device, device.logger, "reward/position_cost",    components.position_cost, cadence);
+        add_scalar(device, device.logger, "reward/linear_vel_cost",  components.linear_vel_cost, cadence);
+        add_scalar(device, device.logger, "reward/angular_vel_cost", components.angular_vel_cost, cadence);
+        add_scalar(device, device.logger, "reward/linear_acc_cost",  components.linear_acc_cost, cadence);
+        add_scalar(device, device.logger, "reward/angular_acc_cost", components.angular_acc_cost, cadence);
+        add_scalar(device, device.logger, "reward/action_cost",      components.action_cost, cadence);
+        add_scalar(device, device.logger, "reward/d_action_cost",    components.d_action_cost, cadence);
+        for(TI action_i = 0; action_i < LOGGED_ACTION_DIM; action_i++){
+            char action_cost_key[] = "reward/action_cost/0";
+            char d_action_cost_key[] = "reward/d_action_cost/0";
+            action_cost_key[sizeof(action_cost_key) - 2] = static_cast<char>('0' + action_i);
+            d_action_cost_key[sizeof(d_action_cost_key) - 2] = static_cast<char>('0' + action_i);
+            add_scalar(device, device.logger, action_cost_key, components.action_costs[action_i], cadence);
+            add_scalar(device, device.logger, d_action_cost_key, components.d_action_costs[action_i], cadence);
+        }
+        add_scalar(device, device.logger, "reward/position_error_integral_cost", components.position_error_integral_cost, cadence);
+        add_scalar(device, device.logger, "reward/pre_exp",         -components.weighted_cost, cadence);
+
+        add_scalar(device, device.logger, "reward_weighted/orientation_cost", reward_parameters.orientation          * components.orientation_cost, cadence);
+        add_scalar(device, device.logger, "reward_weighted/position_cost",    reward_parameters.position             * components.position_cost,    cadence);
+        add_scalar(device, device.logger, "reward_weighted/linear_vel_cost",  reward_parameters.linear_velocity      * components.linear_vel_cost,  cadence);
+        add_scalar(device, device.logger, "reward_weighted/angular_vel_cost", reward_parameters.angular_velocity     * components.angular_vel_cost, cadence);
+        add_scalar(device, device.logger, "reward_weighted/linear_acc_cost" , reward_parameters.linear_acceleration  * components.linear_acc_cost,  cadence);
+        add_scalar(device, device.logger, "reward_weighted/angular_acc_cost", reward_parameters.angular_acceleration * components.angular_acc_cost, cadence);
+        add_scalar(device, device.logger, "reward_weighted/action_cost",      components.weighted_action_cost,      cadence);
+        add_scalar(device, device.logger, "reward_weighted/d_action_cost",    components.weighted_d_action_cost,    cadence);
+        for(TI action_i = 0; action_i < LOGGED_ACTION_DIM; action_i++){
+            char weighted_action_cost_key[] = "reward_weighted/action_cost/0";
+            char weighted_d_action_cost_key[] = "reward_weighted/d_action_cost/0";
+            weighted_action_cost_key[sizeof(weighted_action_cost_key) - 2] = static_cast<char>('0' + action_i);
+            weighted_d_action_cost_key[sizeof(weighted_d_action_cost_key) - 2] = static_cast<char>('0' + action_i);
+            add_scalar(device, device.logger, weighted_action_cost_key, components.weighted_action_costs[action_i], cadence);
+            add_scalar(device, device.logger, weighted_d_action_cost_key, components.weighted_d_action_costs[action_i], cadence);
+        }
+        add_scalar(device, device.logger, "reward_weighted/position_error_integral_cost", reward_parameters.position_error_integral * components.position_error_integral_cost, cadence);
+        // log share of the weighted abs cost
+        add_scalar(device, device.logger, "reward_share/orientation", reward_parameters.orientation          * components.orientation_cost / components.weighted_cost, cadence);
+        add_scalar(device, device.logger, "reward_share/position",    reward_parameters.position             * components.position_cost    / components.weighted_cost, cadence);
+        add_scalar(device, device.logger, "reward_share/linear_vel",  reward_parameters.linear_velocity      * components.linear_vel_cost  / components.weighted_cost, cadence);
+        add_scalar(device, device.logger, "reward_share/angular_vel", reward_parameters.angular_velocity     * components.angular_vel_cost / components.weighted_cost, cadence);
+        add_scalar(device, device.logger, "reward_share/linear_acc",  reward_parameters.linear_acceleration  * components.linear_acc_cost  / components.weighted_cost, cadence);
+        add_scalar(device, device.logger, "reward_share/angular_acc", reward_parameters.angular_acceleration * components.angular_acc_cost / components.weighted_cost, cadence);
+        add_scalar(device, device.logger, "reward_share/action",      components.weighted_action_cost      / components.weighted_cost, cadence);
+        add_scalar(device, device.logger, "reward_share/d_action",    components.weighted_d_action_cost    / components.weighted_cost, cadence);
+        add_scalar(device, device.logger, "reward_share/position_error_integral", reward_parameters.position_error_integral * components.position_error_integral_cost / components.weighted_cost, cadence);
+        add_scalar(device, device.logger, "reward_share/const",       components.reward/reward_parameters.constant, cadence);
+
+        add_scalar(device, device.logger, "reward/weighted_cost",        components.weighted_cost, cadence);
+        add_scalar(device, device.logger, "reward/scaled_weighted_cost", components.scaled_weighted_cost, cadence);
+        add_scalar(device, device.logger, "reward/reward",               components.reward, cadence);
+        add_scalar(device, device.logger, "reward/reward_zero",          components.reward == 0, cadence);
+    }
+    template<typename DEVICE, typename SPEC, typename PARAMETERS, typename ACTION_SPEC, typename STATE, typename T, unsigned T_ACTION_DIM, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT typename SPEC::T reward(DEVICE& device, const Multirotor<SPEC>& env, const PARAMETERS& parameters, const Squared<T, T_ACTION_DIM>& reward_parameters, const STATE& state, const Matrix<ACTION_SPEC>& action,  const STATE& next_state, RNG& rng){
+        typename Squared<T, T_ACTION_DIM>::Components components;
+        // initializing optional components
+        components.d_action_cost = 0;
+        components.position_error_integral_cost = 0;
+        reward_components(device, env, parameters, reward_parameters, state, action, next_state, components, rng);
+
+        return components.reward;
+    }
+    template<typename DEVICE, typename T, unsigned T_ACTION_DIM>
+    RL_TOOLS_FUNCTION_PLACEMENT constexpr auto name(DEVICE& device, const Squared<T, T_ACTION_DIM>& reward_parameters){
+        return "squared";
+    }
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+
+#endif

hyperdrone/_vendor/rl-tools/include/rl_tools/rl/environments/l2f/parameters/reward_functions/squared/squared.h

@@ -0,0 +1,52 @@
+#include "../../../../../../version.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_L2F_PARAMETERS_REWARD_FUNCTIONS_SQUARED_SQUARED_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_RL_ENVIRONMENTS_L2F_PARAMETERS_REWARD_FUNCTIONS_SQUARED_SQUARED_H
+
+#include <rl_tools/utils/generic/typing.h>
+#include <rl_tools/utils/generic/vector_operations.h>
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools::rl::environments::l2f::parameters::reward_functions{
+    template<typename T, unsigned T_ACTION_DIM=4>
+    struct Squared{
+        static constexpr unsigned ACTION_DIM = T_ACTION_DIM;
+        bool non_negative;
+        T scale;
+        T constant;
+        T termination_penalty;
+        T position;
+        T position_clip;
+        T orientation;
+        T linear_velocity;
+        T angular_velocity;
+        T linear_acceleration;
+        T angular_acceleration;
+        T action[ACTION_DIM];
+        T d_action[ACTION_DIM];
+        T position_error_integral;
+        struct Components{
+            T orientation_cost;
+            T position_cost;
+            T linear_vel_cost;
+            T angular_vel_cost;
+            T linear_acc_cost;
+            T angular_acc_cost;
+            T action_cost;
+            T weighted_action_cost;
+            T action_costs[ACTION_DIM];
+            T weighted_action_costs[ACTION_DIM];
+            T d_action_cost;
+            T weighted_d_action_cost;
+            T d_action_costs[ACTION_DIM];
+            T weighted_d_action_costs[ACTION_DIM];
+            T position_error_integral_cost;
+            T weighted_cost;
+            T scaled_weighted_cost;
+            T reward;
+        };
+    };
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+
+#endif

hyperdrone/_vendor/rl-tools/include/rl_tools/rl/environments/l2f/parameters/termination/default.h

@@ -0,0 +1,33 @@
+#include "../../../../../version.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_L2F_PARAMETERS_TERMINATION_DEFAULT_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_RL_ENVIRONMENTS_L2F_PARAMETERS_TERMINATION_DEFAULT_H
+#include "../../multirotor.h"
+
+#include <rl_tools/math/operations_generic.h>
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools::rl::environments::l2f::parameters::termination{
+    template<typename SPEC>
+    constexpr typename rl_tools::rl::environments::l2f::ParametersBase<SPEC>::MDP::Termination narrow = {
+        true,           // enable
+        1,            // position
+        0, // angle (0 = disabled)
+        10,         // linear velocity
+        35, // angular velocity
+        10000, // position integral
+        50000, // orientation integral
+    };
+    template<typename SPEC>
+    constexpr typename rl_tools::rl::environments::l2f::ParametersBase<SPEC>::MDP::Termination fast_learning = {
+        true,           // enable
+        5,            // position
+        0, // angle (0 = disabled)
+        10,         // linear velocity
+        35, // angular velocity
+        10000, // position integral
+        50000, // orientation integral
+    };
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+#endif