hyperdrone 0.1.0__py3-none-any.whl

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

@@ -0,0 +1,119 @@
+#include "../../../version.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_CAR_CAR_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_RL_ENVIRONMENTS_CAR_CAR_H
+
+#include "../../../math/operations_generic.h"
+#include "../environments.h"
+#include "../observation.h"
+
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools::rl::environments::car {
+    template <typename T>
+    struct Tire{
+        T B;
+        T C;
+        T D;
+    };
+    template <typename T>
+    struct Parameters{
+        T g   = 9.81;
+        T m   = 0.041;
+        T I   = 27.8e-6;
+        T lf  = 0.029;
+        T lr  = 0.033;
+        Tire<T> tf = {2.5790, 1.2000, 0.1920};
+        Tire<T> tr = {3.3852, 1.2691, 0.1737};
+        T cm  = 0.287;
+        T cr0 = 0.0;
+        T cr2 = 0.00035;
+        T vt  = 0.01;
+        T dt  = 0.01;
+    };
+
+    template <typename T, typename TI, TI T_HEIGHT, TI T_WIDTH, TI T_TRACK_SCALE>
+    struct ParametersTrack: Parameters<T>{
+        static constexpr TI HEIGHT = T_HEIGHT;
+        static constexpr TI WIDTH = T_WIDTH;
+        static constexpr T TRACK_SCALE = T_TRACK_SCALE / 1000.0;
+        bool track[HEIGHT][WIDTH];
+    };
+
+    template <typename T_T, typename T_TI, typename T_PARAMETERS = car::Parameters<T_T>>
+    struct Specification{
+        using T = T_T;
+        using TI = T_TI;
+        using PARAMETERS = T_PARAMETERS;
+    };
+
+    template <typename T_T, typename T_TI, T_TI T_HEIGHT, T_TI T_WIDTH, T_TI T_TRACK_SCALE>
+    struct SpecificationTrack: Specification<T_T, T_TI>{
+        using T = T_T;
+        using TI = T_TI;
+        static constexpr T_TI HEIGHT = T_HEIGHT;
+        static constexpr T_TI WIDTH = T_WIDTH;
+        static constexpr T TRACK_SCALE = T_TRACK_SCALE/1000.0;
+        static constexpr T BOUND_X_LOWER = -(T)WIDTH * TRACK_SCALE / 2.0;
+        static constexpr T BOUND_Y_LOWER = -(T)HEIGHT * TRACK_SCALE / 2.0;
+        static constexpr T BOUND_X_UPPER = (T)WIDTH * TRACK_SCALE / 2.0;
+        static constexpr T BOUND_Y_UPPER = (T)HEIGHT * TRACK_SCALE / 2.0;
+        using PARAMETERS = ParametersTrack<T, TI, HEIGHT, WIDTH, T_TRACK_SCALE>;
+    };
+
+    template <typename T, typename TI>
+    struct State{
+        static constexpr TI DIM = 6;
+        T x;
+        T y;
+        T mu;
+        T vx;
+        T vy;
+        T omega;
+    };
+    template <typename TI>
+    struct ObservationCar{
+        static constexpr TI DIM = 6;
+        using SHAPE = tensor::Shape<TI, DIM>;
+    };
+    template <typename TI>
+    struct ObservationCarTrack{
+        static constexpr TI DIM = 6 + 3;
+        using SHAPE = tensor::Shape<TI, DIM>;
+    };
+
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools::rl::environments{
+    template <typename T_SPEC>
+    struct Car: Environment<typename T_SPEC::T, typename T_SPEC::TI>{
+        using SPEC = T_SPEC;
+        using T = typename SPEC::T;
+        using TI = typename SPEC::TI;
+        using State = car::State<T, TI>;
+        using Observation = car::ObservationCar<TI>;
+        using ObservationPrivileged = Observation;
+        static constexpr TI ACTION_DIM = 2;
+        using Parameters = typename SPEC::PARAMETERS;
+    };
+
+    template <typename T_SPEC>
+    struct CarTrack: Car<T_SPEC>{
+        using TI = typename T_SPEC::TI;
+        using Observation = car::ObservationCarTrack<TI>;
+        using ObservationPrivileged = Observation;
+        bool initialized = false;
+        bool track[T_SPEC::PARAMETERS::HEIGHT][T_SPEC::PARAMETERS::WIDTH];
+    };
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+
+
+
+
+
+
+
+#endif

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

@@ -0,0 +1,110 @@
+#include "../../../version.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_CAR_OPERATIONS_CPU_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_RL_ENVIRONMENTS_CAR_OPERATIONS_CPU_H
+
+#include <stdint.h>
+#include <fstream>
+#include "operations_generic.h"
+#include <cassert>
+#include <filesystem>
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools::rl::environments::car{
+    uint16_t read_u16(std::ifstream& stream) {
+        uint16_t result;
+        stream.read(reinterpret_cast<char*>(&result), sizeof(result));
+        return result;
+    }
+
+    uint32_t read_u32(std::ifstream& stream) {
+        uint32_t result;
+        stream.read(reinterpret_cast<char*>(&result), sizeof(result));
+        return result;
+    }
+
+    int32_t read_i32(std::ifstream& stream) {
+        int32_t result;
+        stream.read(reinterpret_cast<char*>(&result), sizeof(result));
+        return result;
+    }
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools{
+    template <typename DEV_SPEC, typename SPEC>
+    void init(devices::CPU<DEV_SPEC>& device, rl::environments::CarTrack<SPEC>& env){
+        using DEVICE = devices::CPU<DEV_SPEC>;
+        using TI = typename DEVICE::index_t;
+        using T = typename SPEC::T;
+        using namespace rl::environments::car;
+        std::filesystem::path file_path = "src/rl/environments/car/track.bmp";
+        std::ifstream file(file_path, std::ios::binary);
+
+        if(!file){
+            std::cout << "Error opening file" << file_path << " (the working directory for this executable is assumed to be the root of the repo)" << std::endl;
+            std::abort();
+        }
+
+        uint16_t fileType = read_u16(file);
+        assert(fileType == 0x4D42);  //File is not a bmp
+
+        uint32_t fileSize = read_u32(file);
+        /* uint16_t reserved1 = */ read_u16(file);
+        /* uint16_t reserved2 = */ read_u16(file);
+        uint32_t dataOffset = read_u32(file);
+
+        // Read BMP info header
+        /* uint32_t size = */ read_u32(file);
+        int32_t width = read_i32(file);
+        int32_t height = read_i32(file);
+        /* uint16_t planes = */ read_u16(file);
+        uint16_t bitCount = read_u16(file);
+        /* uint32_t compression = */ read_u32(file);
+        /* uint32_t sizeImage = */ read_u32(file);
+        /* int32_t xPelsPerMeter = */ read_i32(file);
+        /* int32_t yPelsPerMeter = */ read_i32(file);
+        /* uint32_t clrUsed = */ read_u32(file);
+        /* uint32_t clrImportant = */ read_u32(file);
+        assert(bitCount == 24);
+        assert(height == SPEC::HEIGHT);
+        assert(width == SPEC::WIDTH);
+
+        std::cout << "BMP file size: " << fileSize << "\n";
+        std::cout << "Data offset: " << dataOffset << "\n";
+        std::cout << "Image width: " << width << "\n";
+        std::cout << "Image height: " << height << "\n";
+        std::cout << "Bit count: " << bitCount
+                  << "\n";  // This value determines the number of bits that define each pixel and the maximum number of colors in the bitmap.
+
+        file.seekg(dataOffset, file.beg); // Move read position to start of pixel data
+
+        constexpr TI ROW_PADDED = (SPEC::WIDTH * 3 + 3) & (~3);
+        constexpr TI TOTAL_SIZE = ROW_PADDED * SPEC::HEIGHT;
+        uint8_t allPixels[TOTAL_SIZE];
+
+        file.read(reinterpret_cast<char *>(allPixels), TOTAL_SIZE);
+
+        std::cout << "Track: ";
+        for(int i = height - 1; i >= 0; i--){
+            uint8_t *pixelRow = allPixels + (i * ROW_PADDED);
+
+            for (int j = 0; j < width; j++) {
+                uint8_t blue = pixelRow[j * 3];
+                uint8_t green = pixelRow[j * 3 + 1];
+                uint8_t red = pixelRow[j * 3 + 2];
+
+//                std::cout << "Pixel (" << j << "," << height - 1 - i << ") : R=" << (int) red << " G=" << (int) green << " B=" << (int) blue << "\n";
+                bool drivable = (red > 0 || green > 0 || blue > 0) ? 1 : 0;
+                env.parameters.track[height - 1 - i][j] = drivable;
+                std::cout << (drivable ? "  " : "xx");
+            }
+            std::cout << std::endl;
+        }
+        env.initialized = true;
+    }
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+
+#endif

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

@@ -0,0 +1,206 @@
+#include "../../../version.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_CAR_OPERATIONS_GENERIC_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_RL_ENVIRONMENTS_CAR_OPERATIONS_GENERIC_H
+#include "car.h"
+#include "track.h"
+#include "../operations_generic.h"
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools::rl::environments::car {
+    template <typename DEVICE, typename T>
+    RL_TOOLS_FUNCTION_PLACEMENT T f_mod_python(const DEVICE& dev, T a, T b){
+        return a - b * math::floor(dev, a / b);
+    }
+
+    template <typename DEVICE, typename T>
+    RL_TOOLS_FUNCTION_PLACEMENT T angle_normalize(const DEVICE& dev, T x){
+        return f_mod_python(dev, (x + math::PI<T>), (2 * math::PI<T>)) - math::PI<T>;
+    }
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools{
+    template<typename DEVICE, typename SPEC>
+    static void malloc(DEVICE& device, const rl::environments::Car<SPEC>& env){ }
+    template<typename DEVICE, typename SPEC>
+    static void free(DEVICE& device, const rl::environments::Car<SPEC>& env){ }
+    template<typename DEVICE, typename SPEC>
+    static void init(DEVICE& device, const rl::environments::Car<SPEC>& env){ }
+    template<typename DEVICE, typename SPEC>
+    static void init(DEVICE& device, rl::environments::CarTrack<SPEC>& env){
+        env.initialized = true;
+        using T = typename SPEC::T;
+        using TI = typename SPEC::TI;
+        static_assert(decltype(rl::environments::car::tracks::default_track<TI>)::HEIGHT == SPEC::HEIGHT);
+        static_assert(decltype(rl::environments::car::tracks::default_track<TI>)::WIDTH == SPEC::WIDTH);
+        for(TI row_i=0; row_i < SPEC::HEIGHT; row_i++){
+            for(TI col_i=0; col_i < SPEC::WIDTH; col_i++){
+                env.track[row_i][col_i] = rl::environments::car::tracks::default_track<TI>.track[row_i][col_i];
+            }
+        }
+    }
+    template<typename DEVICE, typename SPEC>
+    static void initial_parameters(DEVICE& device, const rl::environments::CarTrack<SPEC>& env, typename rl::environments::CarTrack<SPEC>::Parameters& parameters) {
+        using TI = typename SPEC::TI;
+        for(TI row_i=0; row_i < SPEC::HEIGHT; row_i++){
+            for(TI col_i=0; col_i < SPEC::WIDTH; col_i++){
+                parameters.track[row_i][col_i] = env.track[row_i][col_i];
+            }
+        }
+    }
+    template<typename DEVICE, typename SPEC>
+    static void initial_parameters(DEVICE& device, const rl::environments::Car<SPEC>& env, typename rl::environments::Car<SPEC>::Parameters& parameters) {
+    }
+    template<typename DEVICE, typename SPEC, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT static void sample_initial_parameters(DEVICE& device, const rl::environments::Car<SPEC>& env, typename rl::environments::Car<SPEC>::Parameters& parameters, RNG& rng) {
+        initial_parameters(device, env, parameters);
+    }
+    template<typename DEVICE, typename SPEC, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT static void sample_initial_parameters(DEVICE& device, const rl::environments::CarTrack<SPEC>& env, typename rl::environments::CarTrack<SPEC>::Parameters& parameters, RNG& rng) {
+        initial_parameters(device, env, parameters);
+    }
+    template<typename DEVICE, typename SPEC>
+    static void initial_state(DEVICE& device, const rl::environments::Car<SPEC>& env, typename rl::environments::Car<SPEC>::Parameters& parameters, typename rl::environments::Car<SPEC>::State& state){
+        state.x = 0;
+        state.y = 0;
+        state.mu = 0;
+        state.vx = 0;
+        state.vy = 0;
+        state.omega = 0;
+    }
+    template<typename DEVICE, typename SPEC, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT static void sample_initial_state(DEVICE& device, const rl::environments::Car<SPEC>& env, typename rl::environments::Car<SPEC>::Parameters& parameters, typename rl::environments::Car<SPEC>::State& state, RNG& rng){
+        using T = typename SPEC::T;
+        initial_state(device, env, state);
+        constexpr T dist = 0.2;
+        state.x = random::uniform_real_distribution(typename DEVICE::SPEC::RANDOM(), -dist, dist, rng);
+        state.y = random::uniform_real_distribution(typename DEVICE::SPEC::RANDOM(), -dist, dist, rng);
+        state.mu = random::uniform_real_distribution(typename DEVICE::SPEC::RANDOM(), -math::PI<T>, math::PI<T>, rng);
+    }
+    template<typename DEVICE, typename SPEC, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT static bool terminated(DEVICE& device, const rl::environments::Car<SPEC>& env, typename rl::environments::Car<SPEC>::Parameters& parameters, const typename rl::environments::Car<SPEC>::State state, RNG& rng){
+        using T = typename SPEC::T;
+        return state.x > 1.0 || state.x < -1.0 || state.y > 1.0 || state.y < -1.0;
+    }
+    template<typename DEVICE, typename SPEC, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT static bool terminated(DEVICE& device, const rl::environments::CarTrack<SPEC>& env, typename rl::environments::Car<SPEC>::Parameters& parameters, const typename rl::environments::CarTrack<SPEC>::State state, RNG& rng){
+#ifdef RL_TOOLS_DEBUG
+        utils::assert_exit(device, env.initialized, "Environment not initialized");
+#endif
+        using T = typename SPEC::T;
+        using TI = typename SPEC::TI;
+        T x_coord = (state.x + SPEC::TRACK_SCALE * SPEC::WIDTH / 2.0) / ((T)SPEC::TRACK_SCALE);
+        T y_coord = (-state.y + SPEC::TRACK_SCALE * SPEC::HEIGHT / 2.0) / ((T)SPEC::TRACK_SCALE);
+        if(x_coord > 0 && x_coord < SPEC::WIDTH && y_coord > 0 && y_coord < SPEC::HEIGHT){
+            return !parameters.track[(TI)y_coord][(TI)x_coord];
+        }
+        else{
+            return true;
+        }
+    }
+    template<typename DEVICE, typename SPEC, typename ACTION_SPEC, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT typename SPEC::T step(DEVICE& device, const rl::environments::Car<SPEC>& env, typename rl::environments::Car<SPEC>::Parameters& parameters, const typename rl::environments::Car<SPEC>::State& s, const Matrix<ACTION_SPEC>& action, typename rl::environments::Car<SPEC>::State& next_state, RNG& rng) {
+        using ENVIRONMENT = rl::environments::Car<SPEC>;
+        static_assert(ACTION_SPEC::ROWS == 1);
+        static_assert(ACTION_SPEC::COLS == ENVIRONMENT::ACTION_DIM);
+        using namespace rl::environments::car;
+        using T = typename SPEC::T;
+
+        T throttle_break = math::clamp(device.math, get(action, 0, 0), (T)-1, (T)1);
+        T delta = math::clamp(device.math, get(action, 0, 1), (T)-1, (T)1);
+        
+        auto& p = parameters;
+        
+        T alpha_f = math::atan2(device.math, (s.vy + p.lf * s.omega), s.vx) - delta;
+        T alpha_r = math::atan2(device.math, (s.vy - p.lr * s.omega), s.vx);
+        T FnF = p.lr / (p.lf + p.lr) * p.m * p.g;
+        T FnR = p.lf / (p.lf + p.lr) * p.m * p.g;
+        T FyF = s.vx > p.vt || s.vy > p.vt ? FnF * p.tf.D * math::sin(device.math, p.tf.C * atan(p.tf.B * (-alpha_f))) : 0;
+        T FyR = s.vx > p.vt || s.vy > p.vt ? FnR * p.tr.D * math::sin(device.math, p.tr.C * atan(p.tr.B * (-alpha_r))) : 0;
+
+        T Fx = p.cm * throttle_break - p.cr0 - p.cr2 * s.vx * s.vx;
+        next_state = s;
+        next_state.x     += p.dt * (math::cos(device.math, s.mu) * s.vx - math::sin(device.math, s.mu) * s.vy);
+        next_state.y     += p.dt * (math::sin(device.math, s.mu) * s.vx + math::cos(device.math, s.mu) * s.vy);
+        next_state.mu    += p.dt * (s.omega);
+        next_state.vx    += p.dt * (1/p.m*(Fx - FyF * math::sin(device.math, delta)) + s.omega * s.vy);
+        next_state.vy    += p.dt * (1/p.m*(FyF * math::cos(device.math, delta) + FyR) - s.omega * s.vx);
+        next_state.omega += p.dt * (1/p.I*(FyF * p.lf * math::cos(device.math, delta) - FyR * p.lr));
+
+        return p.dt;
+    }
+    template<typename DEVICE, typename SPEC, typename ACTION_SPEC, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT static typename SPEC::T reward(DEVICE& device, const rl::environments::Car<SPEC>& env, typename rl::environments::Car<SPEC>::Parameters& parameters, const typename rl::environments::Car<SPEC>::State& state, const Matrix<ACTION_SPEC>& action, const typename rl::environments::Car<SPEC>::State& next_state, RNG& rng){
+        using namespace rl::environments::car;
+        typedef typename SPEC::T T;
+        T angle_norm = angle_normalize(device.math, state.mu);
+        T cost = 0.1*angle_norm * angle_norm + state.x * state.x + state.y * state.y;
+        return math::exp(device.math, -5*cost);
+    }
+    template<typename DEVICE, typename SPEC, typename ACTION_SPEC, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT static typename SPEC::T reward(DEVICE& device, const rl::environments::CarTrack<SPEC>& env, typename rl::environments::Car<SPEC>::Parameters& parameters, const typename rl::environments::Car<SPEC>::State& state, const Matrix<ACTION_SPEC>& action, const typename rl::environments::Car<SPEC>::State& next_state, RNG& rng){
+        using namespace rl::environments::car;
+        typedef typename SPEC::T T;
+        return state.vx;
+    }
+
+    template<typename DEVICE, typename SPEC, typename OBS_TYPE_SPEC, typename OBS_SPEC, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT static void observe(DEVICE& device, const rl::environments::Car<SPEC>& env, const typename rl::environments::Car<SPEC>::Parameters& parameters, const typename rl::environments::Car<SPEC>::State& state, const typename rl::environments::car::ObservationCar<OBS_TYPE_SPEC>&, Matrix<OBS_SPEC>& observation, RNG& rng){
+        using ENVIRONMENT = rl::environments::Car<SPEC>;
+        static_assert(OBS_SPEC::ROWS == 1);
+        static_assert(OBS_SPEC::COLS == ENVIRONMENT::Observation::DIM);
+        typedef typename SPEC::T T;
+        set(observation, 0, 0, state.x);
+        set(observation, 0, 1, state.y);
+        set(observation, 0, 2, state.mu);
+        set(observation, 0, 3, state.vx);
+        set(observation, 0, 4, state.vy);
+        set(observation, 0, 5, state.omega);
+    }
+    template<typename DEVICE, typename SPEC, typename OBS_TYPE_SPEC, typename OBS_SPEC, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT static void observe(DEVICE& device, const rl::environments::CarTrack<SPEC>& env, const typename rl::environments::Car<SPEC>::Parameters& parameters, const typename rl::environments::CarTrack<SPEC>::State& state, const typename rl::environments::car::ObservationCarTrack<OBS_TYPE_SPEC>&, Matrix<OBS_SPEC>& observation, RNG& rng){
+#ifdef RL_TOOLS_DEBUG
+        utils::assert_exit(device, env.initialized, "Environment not initialized");
+#endif
+        using ENVIRONMENT = rl::environments::CarTrack<SPEC>;
+        static_assert(OBS_SPEC::ROWS == 1);
+        static_assert(OBS_SPEC::COLS == ENVIRONMENT::Observation::DIM);
+        using T = typename SPEC::T;
+        using TI = typename SPEC::TI;
+        auto observation_base = view(device, observation, matrix::ViewSpec<1, rl::environments::car::ObservationCar<TI>::DIM>{});
+        observe(device, (ENVIRONMENT&)env, parameters, static_cast<const typename rl::environments::Car<SPEC>::State&>(state), rl::environments::car::ObservationCar<TI>{}, observation_base, rng);
+        constexpr TI N_DIRECTIONS = 3;
+        constexpr TI NUM_STEPS = 50;
+        constexpr T step_size = SPEC::TRACK_SCALE / 2;
+        T directions[N_DIRECTIONS] = {-20/180.0*math::PI<T>, 0, 20/180.0*math::PI<T>};
+
+        for(TI direction_i=0; direction_i < N_DIRECTIONS; direction_i++){
+            T direction = directions[direction_i];
+            T direction_x = math::cos(device.math, state.mu + direction) * step_size;
+            T direction_y = math::sin(device.math, state.mu + direction) * step_size;
+            T distance = NUM_STEPS * step_size;
+
+            for(TI step_i = 0; step_i < NUM_STEPS; step_i++){
+                T x_coord = (+state.x + direction_x * step_i + SPEC::TRACK_SCALE * SPEC::WIDTH  / 2.0) / ((T)SPEC::TRACK_SCALE);
+                T y_coord = (-state.y - direction_y * step_i + SPEC::TRACK_SCALE * SPEC::HEIGHT / 2.0) / ((T)SPEC::TRACK_SCALE);
+                if(!parameters.track[(TI)y_coord][(TI)x_coord]){
+                    distance = step_i * step_size;
+                    break;
+                }
+            }
+            set(observation, 0, 6 + direction_i, distance / (NUM_STEPS * step_size));
+        }
+    }
+    template<typename DEVICE, typename SPEC, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT static void sample_initial_state(DEVICE& device, const rl::environments::CarTrack<SPEC>& env, typename rl::environments::Car<SPEC>::Parameters& parameters, typename rl::environments::Car<SPEC>::State& state, RNG& rng){
+        using T = typename SPEC::T;
+        initial_state(device, env, parameters, state);
+        do{
+            state.x = random::uniform_real_distribution(typename DEVICE::SPEC::RANDOM(), SPEC::BOUND_X_LOWER, SPEC::BOUND_X_UPPER, rng);
+            state.y = random::uniform_real_distribution(typename DEVICE::SPEC::RANDOM(), SPEC::BOUND_Y_LOWER, SPEC::BOUND_Y_UPPER, rng);
+            state.mu = random::uniform_real_distribution(typename DEVICE::SPEC::RANDOM(), -math::PI<T>, math::PI<T>, rng);
+        } while(terminated(device, env, parameters, state, rng));
+    }
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+#endif

hyperdrone/_vendor/rl-tools/include/rl_tools/rl/environments/car/operations_json.h

@@ -0,0 +1,59 @@
+#include "../../../version.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_CAR_OPERATIONS_JSON_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_RL_ENVIRONMENTS_CAR_OPERATIONS_JSON_H
+
+#include "car.h"
+#include <nlohmann/json.hpp>
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools::rl::environments::car{
+    template <typename DEVICE, typename SPEC, typename T>
+    nlohmann::json json_object(const DEVICE& device, const rl::environments::Car<SPEC>){
+        nlohmann::json j;
+        return j;
+    }
+    template <typename DEVICE, typename SPEC, typename T>
+    nlohmann::json json_object(const DEVICE& device, const rl::environments::Car<SPEC>, const rl::environments::car::Parameters<T>& parameters){
+        nlohmann::json j;
+        j["lf"]  = parameters.lf;
+        j["lr"]  = parameters.lr;
+        j["dt"]  = parameters.dt;
+        return j;
+    }
+    template <typename DEVICE, typename SPEC, typename T>
+    std::string json(const DEVICE& device, const rl::environments::Car<SPEC>& env, const rl::environments::car::Parameters<T>& parameters){
+        return json_object(device, env, parameters).dump();
+    }
+    template <typename DEVICE, typename SPEC, typename T, typename TI, TI T_HEIGHT, TI T_WIDTH, TI T_TRACK_SCALE>
+    std::string json(const DEVICE& device, const rl::environments::CarTrack<SPEC>& env, const rl::environments::car::ParametersTrack<T, TI, T_HEIGHT, T_WIDTH, T_TRACK_SCALE>& parameters){
+        nlohmann::json j = json_object(device, env, static_cast<const rl::environments::car::Parameters<T>&>(parameters));
+        j["height"] = T_HEIGHT;
+        j["width"] = T_WIDTH;
+        j["track_scale"] = T_TRACK_SCALE/(T)1000.0;
+        std::vector<std::vector<bool>> track;
+        for(TI row_i = 0; row_i < T_HEIGHT; row_i++){
+            std::vector<bool> row;
+            for(TI col_j = 0; col_j < T_WIDTH; col_j++){
+                row.push_back(parameters.track[row_i][col_j]);
+            }
+            track.push_back(row);
+        }
+        j["track"] = track;
+        return j.dump();
+    }
+    template <typename DEVICE, typename SPEC, typename T, typename TI>
+    std::string json(const DEVICE& device, const rl::environments::Car<SPEC>, const typename rl::environments::Car<SPEC>::Parameters& parameters, const rl::environments::car::State<T, TI>& state){
+        nlohmann::json j;
+        j["x"] = state.x;
+        j["y"] = state.y;
+        j["mu"] = state.mu;
+        j["vx"] = state.vx;
+        j["vy"] = state.vy;
+        j["omega"] = state.omega;
+        return j.dump();
+    }
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+
+#endif