drone_optimization.cpp

Go to the documentation of this file.

 
#include <iostream>
#include <vector>
#include <cmath>
#include <memory>
#include <iomanip>
#include <cstdint>
#include <omp.h>
#include <filesystem>
#include <random>
#include <limits>
#include <functional>
#include "../montecarlo/rng/rng_global.hpp"
#include "../montecarlo/rng/rng_factory.hpp"
 
#include "../montecarlo/geometry.hpp"
#include "../montecarlo/integrators/MCintegrator.hpp"
#include "../montecarlo/domains/hyperrectangle.hpp"
#include "../montecarlo/domains/hypersphere.hpp"
#include "../montecarlo/domains/hypercylinder.hpp"
#include "../montecarlo/domains/polytope.hpp"
#include "../montecarlo/optimizers/PSO.hpp"
#include "../montecarlo/utils/plotter.hpp"
#include <fstream>
#include <stdexcept>
 
constexpr size_t DIM = 3;
 
// =================================================================================
// HELPER FUNCTIONS FOR READING POLYTOPE DATA
// =================================================================================
template <int dim>
std::vector<mc::geom::Point<dim>> read_points_from_file_drone(const std::string& filename)
{
    std::ifstream in(filename);
    if (!in.is_open()) {
        throw std::runtime_error("Cannot open file: " + filename);
    }
 
    std::size_t num_points = 0;
    std::size_t file_dim   = 0;
 
    in >> num_points >> file_dim;
    if (!in.good()) {
        throw std::runtime_error("Error reading header from file: " + filename);
    }
 
    if (file_dim != static_cast<std::size_t>(dim)) {
        throw std::runtime_error(
            "Dimension mismatch: file has dim = " + std::to_string(file_dim) +
            " but template expects dim = " + std::to_string(dim));
    }
 
    std::vector<mc::geom::Point<dim>> points;
    points.reserve(num_points);
 
    for (std::size_t i = 0; i < num_points; ++i) {
        mc::geom::Point<dim> p;
        for (int k = 0; k < dim; ++k) {
            if (!(in >> p[k])) {
                throw std::runtime_error(
                    "Error reading coordinate " + std::to_string(k) +
                    " of point " + std::to_string(i) +
                    " from file: " + filename);
            }
        }
        points.push_back(p);
    }
 
    return points;
}
 
template <int dim>
void read_normals_and_offsets_from_file_drone(
    const std::string& filename,
    std::vector<std::array<double, dim>>& normals,
    std::vector<double>& offsets)
{
    std::ifstream in(filename);
    if (!in.is_open()) {
        throw std::runtime_error("Cannot open normals file: " + filename);
    }
 
    std::size_t file_dim = 0;
    std::size_t num_facets = 0;
 
    in >> file_dim >> num_facets;
    if (!in.good()) {
        throw std::runtime_error("Error reading header (dim, num_facets) from: " + filename);
    }
 
    if (file_dim != dim + 1) {
        throw std::runtime_error(
            "Dimension mismatch in normals file: file has dim = " +
            std::to_string(file_dim) + " but template expects dim = " +
            std::to_string(dim));
    }
 
    normals.clear();
    offsets.clear();
    normals.reserve(num_facets);
    offsets.reserve(num_facets);
 
    for (std::size_t f = 0; f < num_facets; ++f) {
        std::array<double, dim> n{};
        double d = 0.0;
 
        for (std::size_t k = 0; k < dim; ++k) {
            if (!(in >> n[k])) {
                throw std::runtime_error(
                    "Error reading normal component " + std::to_string(k) +
                    " for facet " + std::to_string(f) +
                    " from: " + filename);
            }
        }
 
        if (!(in >> d)) {
            throw std::runtime_error(
                "Error reading offset d for facet " + std::to_string(f) +
                " from: " + filename);
        }
 
        double b = -d;
        normals.push_back(n);
        offsets.push_back(b);
    }
}
 
// =================================================================================
// 1. COMPLEX DOMAIN DEFINITION (Arm + Motor Housing)
// =================================================================================
class DroneArmDomain : public mc::domains::IntegrationDomain<DIM> {
public:
    // Geometric components
    std::unique_ptr<mc::domains::HyperRectangle<DIM>> arm;
    std::unique_ptr<mc::domains::HyperCylinder<DIM>> motor_housing;
    std::unique_ptr<mc::domains::PolyTope<DIM>> cabin;  // Optional: PolyTope for cabin geometry
    
    // Offsets relative to origin
    std::array<double, DIM> motor_offset;
    std::array<double, DIM> cabin_offset;
 
    DroneArmDomain() {
        // 1. Arm: 10 units long, 2 units wide, 1 unit tall, centered at origin
        std::array<double, DIM> arm_dims = {10.0, 2.0, 1.0}; 
        arm = std::make_unique<mc::domains::HyperRectangle<DIM>>(arm_dims);
 
        // 2. Motor housing: cylinder radius 1.5, height 1.2 (along Z-axis)
        motor_housing = std::make_unique<mc::domains::HyperCylinder<DIM>>(1.5, 1.2);
        
        // Position motor at arm tip (x = 5.0)
        motor_offset = {5.0, 0.0, -0.6};
        
        // 3. CABIN LOADING (PolyTope) - OPTIONAL
        // Attempts to load cabin from files if available
        try {
            auto points = read_points_from_file_drone<DIM>("../drone_assets/cabin_points.txt");
            
            std::vector<std::array<double, DIM>> normals;
            std::vector<double> offsets;
            read_normals_and_offsets_from_file_drone<DIM>("../drone_assets/cabin_hull.txt", normals, offsets);
 
            cabin = std::make_unique<mc::domains::PolyTope<DIM>>(points, normals, offsets);
            cabin_offset = {-2.0, 0.0, 0.5};
        } catch (const std::exception&) {
            cabin = nullptr;
            cabin_offset = {0.0, 0.0, 0.0};
        }
    }
 
    // Bounding box for Monte Carlo sampling (union of component bounds)
    mc::geom::Bounds<DIM> getBounds() const override {
        mc::geom::Bounds<DIM> b;
        b[0] = {-6.0, 8.0};   // X
        b[1] = {-3.0, 3.0};   // Y
        b[2] = {-2.0, 2.0};   // Z
        return b;
    }
 
    double getBoxVolume() const override {
        return (14.0) * (6.0) * (4.0); 
    }
 
    // Constructive Solid Geometry (CSG): Union of Arm ∪ Motor ∪ Cabin
    bool isInside(const mc::geom::Point<DIM> &p) const override {
        // Check arm
        if (arm->isInside(p)) return true;
 
        // Check motor (with offset)
        mc::geom::Point<DIM> p_local;
        for(size_t i=0; i<DIM; ++i) p_local[i] = p[i] - motor_offset[i];
        if (motor_housing->isInside(p_local)) return true;
 
        // Check cabin (PolyTope) if loaded
        if (cabin) {
            mc::geom::Point<DIM> p_cabin;
            for(size_t i=0; i<DIM; ++i) p_cabin[i] = p[i] - cabin_offset[i];
            if (cabin->isInside(p_cabin)) return true;
        }
 
        return false;
    }
    
    // Export domain geometry to file for visualization
    // Export geometry with hole visualization
    // Parameters: hole center (hx, hy, hz) and radius (hr)
    void exportGeometry(const std::string& output_dir, 
                        double hx, double hy, double hz, double hr) const {
        // Create output directory if it doesn't exist
        try {
            std::filesystem::create_directories(output_dir);
        } catch (const std::exception& e) {
            std::cerr << "Could not create output directory: " << e.what() << std::endl;
            return;
        }
        
        std::string filename = output_dir + "/drone_domain.txt";
        std::ofstream out(filename);
        if (!out.is_open()) {
            std::cerr << "Could not open file for export: " << filename << std::endl;
            return;
        }
        
        out << "# Drone arm domain geometry export (with optimized hole)\n";
        out << "# Format: point_type x y z\n";
        out << "# Hole: center=(" << hx << "," << hy << "," << hz << "), radius=" << hr << "\n";
        
        // Lambda to check if a point is inside the hole
        auto is_in_hole = [&](double x, double y, double z) {
            double dist_sq = std::pow(x - hx, 2) + std::pow(y - hy, 2) + std::pow(z - hz, 2);
            return dist_sq <= (hr * hr);
        };
        
        // Sample arm with higher resolution, excluding hole
        out << "# Arm (sampled points, hole excluded)\n";
        double step = 0.15;  // Fine sampling for good visual quality
        
        // Sample over arm bounding region
        for (double x = -5.0; x <= 5.0; x += step) {
            for (double y = -1.0; y <= 1.0; y += step) {
                for (double z = -0.5; z <= 0.5; z += step) {
                    mc::geom::Point<DIM> p;
                    p[0] = x;
                    p[1] = y;
                    p[2] = z;
                    
                    // Draw only if inside arm AND outside hole
                    if (arm->isInside(p) && !is_in_hole(x, y, z)) {
                        out << "arm " << x << " " << y << " " << z << "\n";
                    }
                }
            }
        }
        
        // Sample motor with higher resolution, excluding hole
        out << "# Motor (sampled points, hole excluded)\n";
        double step_motor = 0.15;  // Fine sampling for cylindrical shape
        
        // Sample over motor bounding region (centered around x=5, radius=1.5, height=1.2)
        for (double x = 3.5; x <= 6.5; x += step_motor) {
            for (double y = -1.5; y <= 1.5; y += step_motor) {
                for (double z = -0.6; z <= 0.6; z += step_motor) {
                    mc::geom::Point<DIM> p;
                    p[0] = x;
                    p[1] = y;
                    p[2] = z;
                    
                    // Transform to motor local coordinates
                    mc::geom::Point<DIM> p_motor;
                    for(size_t i=0; i<DIM; ++i) p_motor[i] = p[i] - motor_offset[i];
                    
                    // Draw only if inside motor AND outside hole
                    if (motor_housing->isInside(p_motor) && !is_in_hole(x, y, z)) {
                        out << "motor " << x << " " << y << " " << z << "\n";
                    }
                }
            }
        }
        
        out.close();
        std::cout << "Geometry (with hole) exported to: " << filename << std::endl;
    }
};
 
// =================================================================================
// 2. OBJECTIVE FUNCTION FOR OPTIMIZER
// =================================================================================
// Input: optimization variables [x, y, z, radius] of hole
// Output: distance between calculated center of mass and target
 
// Deterministic hash of parameters to stabilize Monte Carlo noise across identical queries
uint32_t hash_params(const std::vector<double>& params) {
    uint32_t seed = 0;
    std::hash<double> hasher;
    for (double p : params) {
        seed ^= hasher(p) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
    }
    return seed;
}
 
double objective_function(const std::vector<double>& params) {
    double hole_x = params[0];
    double hole_y = params[1];
    double hole_z = params[2];
    double hole_r = params[3];
 
    // Geometric guard: prevent optimizing a hole center outside the body
    DroneArmDomain domain;
    mc::geom::Point<DIM> hole_center;
    hole_center[0] = hole_x;
    hole_center[1] = hole_y;
    hole_center[2] = hole_z;
    if (!domain.isInside(hole_center)) {
        return 1e6; // Soft penalty to push PSO back into the body
    }
 
    // Deterministic RNG: same params -> same seed -> stationary noise surface for PSO
    uint32_t local_seed = hash_params(params) + mc::rng::get_global_seed();
    std::mt19937 rng(local_seed);
 
    auto bounds = domain.getBounds();
    std::uniform_real_distribution<double> dist_x(bounds[0].first, bounds[0].second);
    std::uniform_real_distribution<double> dist_y(bounds[1].first, bounds[1].second);
    std::uniform_real_distribution<double> dist_z(bounds[2].first, bounds[2].second);
 
    int n_samples = 20000; // Fast during PSO; high-precision check is later
    double hole_r_sq = hole_r * hole_r;
 
    double sum_mass = 0.0;
    double sum_mx = 0.0;
    double sum_my = 0.0;
    double sum_mz = 0.0;
 
    // Single Monte Carlo pass: inside domain and outside hole -> accumulate mass and moments
    for (int i = 0; i < n_samples; ++i) {
        mc::geom::Point<DIM> p;
        p[0] = dist_x(rng);
        p[1] = dist_y(rng);
        p[2] = dist_z(rng);
 
        if (domain.isInside(p)) {
            double dist_sq = std::pow(p[0] - hole_x, 2) +
                             std::pow(p[1] - hole_y, 2) +
                             std::pow(p[2] - hole_z, 2);
            if (dist_sq > hole_r_sq) {
                sum_mass += 1.0;
                sum_mx   += p[0];
                sum_my   += p[1];
                sum_mz   += p[2];
            }
        }
    }
 
    if (sum_mass <= 1e-9) return 1e6; // Penalty if we remove all material
 
    double cm_x = sum_mx / sum_mass;
    double cm_y = sum_my / sum_mass;
    double cm_z = sum_mz / sum_mass;
 
    double error = std::sqrt(std::pow(cm_x - 1.0, 2) +
                             std::pow(cm_y - 0.0, 2) +
                             std::pow(cm_z - 0.0, 2));
 
    return error;
}
 
// =================================================================================
// 3. MAIN
// =================================================================================
int main(int argc, char* argv[]) {
    // Parse seed from command line if provided
    int num_threads = omp_get_max_threads();  // Default: max available threads
    std::uint32_t seed = 12345;  // Default seed
    
    if (argc > 1) {
        std::string seed_arg = argv[1];
        // Check if user wants to keep default seed (using "-" as placeholder)
        if (seed_arg == "-") {
            std::cout << "Using default seed: " << seed << std::endl;
        } else {
            try {
                seed = static_cast<std::uint32_t>(std::stoul(seed_arg));
                std::cout << "Using custom seed: " << seed << std::endl;
            } catch (const std::exception&) {
                std::cerr << "Invalid seed argument. Using default seed: " << seed << std::endl;
            }
        }
    } else {
        std::cout << "Using default seed: " << seed << std::endl;
    }
    
    // Set the global seed for all library components
    mc::rng::set_global_seed(seed);
    
    // Second argument: number of threads (0 = sequential, positive = number of threads)
    if (argc > 2) {
        try {
            int requested_threads = std::stoi(argv[2]);
            if (requested_threads == 0) {
                num_threads = 1;
                omp_set_num_threads(1);
                std::cout << "Running SEQUENTIAL (1 thread)" << std::endl;
            } else if (requested_threads > 0) {
                num_threads = requested_threads;
                omp_set_num_threads(requested_threads);
                std::cout << "Running with " << num_threads << " threads" << std::endl;
            }
        } catch (const std::exception&) {
            std::cout << "Invalid thread argument. Using max available: " << num_threads << std::endl;
        }
    } else {
        std::cout << "Using max available threads: " << num_threads << std::endl;
        std::cout << "(Usage: ./drone_optimization [seed|-] [num_threads])" << std::endl;
        std::cout << "  <num_threads>: 0=sequential, N>0=N threads" << std::endl;
    }
    std::cout << std::endl;
 
    std::cout << "--- Drone Arm Center of Mass Optimization ---" << std::endl;
    std::cout << "Optimizing hole position and size to shift CM to (1.0, 0, 0)..." << std::endl;
    std::cout << std::endl;
 
    // =================================================================================
    // PRE-COMPUTE BASELINE: Full drone mass/moments (no hole) using high-precision MC
    // =================================================================================
    std::cout << "--- Pre-computing Static Drone Properties (Baseline) ---" << std::endl;
 
    DroneArmDomain static_domain;
    mc::integrators::MontecarloIntegrator<DIM> static_integrator(static_domain);
 
    // High sample count for accurate baseline (reused later for ground truth)
    int n_baseline = 1000000;
 
    // Density functions for full body (no hole consideration)
    auto f_mass = [](const mc::geom::Point<DIM>& p) { return 1.0; };
    auto f_mx   = [](const mc::geom::Point<DIM>& p) { return p[0]; };
    auto f_my   = [](const mc::geom::Point<DIM>& p) { return p[1]; };
    auto f_mz   = [](const mc::geom::Point<DIM>& p) { return p[2]; };
 
    double base_mass = static_integrator.OLDintegrate(f_mass, n_baseline);
    double base_mx   = static_integrator.OLDintegrate(f_mx, n_baseline);
    double base_my   = static_integrator.OLDintegrate(f_my, n_baseline);
    double base_mz   = static_integrator.OLDintegrate(f_mz, n_baseline);
 
    std::cout << std::fixed << std::setprecision(8);
    std::cout << "Baseline Mass (Full Body): " << base_mass << std::endl;
    std::cout << "Baseline CM: (" << base_mx/base_mass << ", "
                                  << base_my/base_mass << ", "
                                  << base_mz/base_mass << ")" << std::endl;
    std::cout << std::endl;
 
    // PSO configuration
    mc::optim::PSOConfig config;
    config.population_size = 30;
    config.max_iterations = 50;
    config.cognitive_coeff = 1.5;
    config.social_coeff = 1.5;
    config.inertia_weight = 0.7;
 
    mc::optim::PSO pso(config);
 
    // Set objective function
    pso.setObjectiveFunction(objective_function);
 
    // Define search bounds for hole [x, y, z, r]
    std::vector<double> lower_bounds = {-4.0, -1.0, -0.5, 0.1};
    std::vector<double> upper_bounds = { 4.0,  1.0,  0.5, 0.9};
 
    pso.setBounds(lower_bounds, upper_bounds);
    pso.setMode(mc::optim::OptimizationMode::MINIMIZE);
 
    // Progress callback
    pso.setCallback([](const mc::optim::Solution& best, int iter) {
        if (iter % 10 == 0) {
            std::cout << "Iter " << iter << " | Error: " << best.value << " | ";
            std::cout << "Hole: x=" << best.params[0] << " r=" << best.params[3] << std::endl;
        }
    });
 
    // Run optimization
    mc::optim::Solution best_sol = pso.optimize();
 
    std::cout << "\n--- Optimization Complete ---" << std::endl;
    std::cout << "Final Error: " << best_sol.value << std::endl;
    std::cout << "Optimal Hole Configuration:" << std::endl;
    std::cout << "  Position: (" << best_sol.params[0] << ", " 
                                  << best_sol.params[1] << ", " 
                                  << best_sol.params[2] << ")" << std::endl;
    std::cout << "  Radius:   " << best_sol.params[3] << std::endl;
 
    // Retrieve optimal parameters
    double best_x = best_sol.params[0];
    double best_y = best_sol.params[1];
    double best_z = best_sol.params[2];
    double best_r = best_sol.params[3];
 
    // =================================================================================
    // HIGH-PRECISION VERIFICATION (single-pass, correlated mass/moment sampling)
    // =================================================================================
    std::cout << "\n--- HIGH-PRECISION VERIFICATION ---" << std::endl;
 
    DroneArmDomain domain_verify;
 
    auto bounds = domain_verify.getBounds();
    int n_verify = 1000000;
    double best_r_sq = best_r * best_r;
 
    long double total_hits = 0.0;
    long double total_mx = 0.0;
    long double total_my = 0.0;
    long double total_mz = 0.0;
 
    #pragma omp parallel reduction(+:total_hits, total_mx, total_my, total_mz)
    {
        int tid = omp_get_thread_num();
        // Deterministic verification RNG per thread; stable across runs
        auto local_rng = mc::rng::make_thread_engine(9999);
 
        #pragma omp for
        for (int i = 0; i < n_verify; ++i) {
            mc::geom::Point<DIM> p;
            double rx = std::generate_canonical<double, 32>(local_rng);
            double ry = std::generate_canonical<double, 32>(local_rng);
            double rz = std::generate_canonical<double, 32>(local_rng);
            p[0] = bounds[0].first + rx * (bounds[0].second - bounds[0].first);
            p[1] = bounds[1].first + ry * (bounds[1].second - bounds[1].first);
            p[2] = bounds[2].first + rz * (bounds[2].second - bounds[2].first);
 
            if (domain_verify.isInside(p)) {
                double dist_sq = std::pow(p[0] - best_x, 2) +
                                 std::pow(p[1] - best_y, 2) +
                                 std::pow(p[2] - best_z, 2);
                if (dist_sq > best_r_sq) {
                    total_hits += 1.0;
                    total_mx += p[0];
                    total_my += p[1];
                    total_mz += p[2];
                }
            }
        }
    }
 
    if (total_hits <= 0.0) {
        std::cout << "No mass remains after hole removal; verification aborting." << std::endl;
        return 0;
    }
 
    double box_vol = domain_verify.getBoxVolume();
    double mass_high_prec = (static_cast<double>(total_hits) / n_verify) * box_vol;
 
    double cm_x_final = static_cast<double>(total_mx / total_hits);
    double cm_y_final = static_cast<double>(total_my / total_hits);
    double cm_z_final = static_cast<double>(total_mz / total_hits);
 
    const mc::geom::Point<DIM> CM_target_verify = [](){ mc::geom::Point<DIM> p; p[0] = 1.0; p[1] = 0.0; p[2] = 0.0; return p; }();
    double error_final = std::sqrt(std::pow(cm_x_final - CM_target_verify[0], 2) +
                                   std::pow(cm_y_final - CM_target_verify[1], 2) +
                                   std::pow(cm_z_final - CM_target_verify[2], 2));
 
    std::cout << std::fixed << std::setprecision(8);
    std::cout << "\nResults with " << n_verify << " samples (high-precision verification):" << std::endl;
    std::cout << "Total mass estimated:     " << mass_high_prec << std::endl;
    std::cout << "Current center of mass:   (" << cm_x_final << ", "
                                              << cm_y_final << ", "
                                              << cm_z_final << ")" << std::endl;
    std::cout << "Target center of mass:    (" << CM_target_verify[0] << ", "
                                              << CM_target_verify[1] << ", "
                                              << CM_target_verify[2] << ")" << std::endl;
    std::cout << "Residual error (verified):" << error_final << std::endl;
    std::cout << "\nComparison:" << std::endl;
    std::cout << "  Optimization error (few samples):    " << best_sol.value << std::endl;
    std::cout << "  Verification error (many samples):   " << error_final << std::endl;
 
    // =================================================================================
    // HYBRID GROUND TRUTH: Baseline Monte Carlo - Analytic Hole Subtraction
    // =================================================================================
    std::cout << "\n--- HYBRID VERIFICATION (Baseline MC + Analytic Hole) ---" << std::endl;
 
    // Exact spherical hole volume and moments (analytic)
    const double pi = std::acos(-1.0);
    double vol_hole = (4.0 / 3.0) * pi * std::pow(best_r, 3);
    double mom_x_hole = vol_hole * best_x;
    double mom_y_hole = vol_hole * best_y;
    double mom_z_hole = vol_hole * best_z;
 
    // Subtract hole from baseline (measured earlier with high precision)
    double final_mass_exact = base_mass - vol_hole;
    double final_mx_exact = base_mx - mom_x_hole;
    double final_my_exact = base_my - mom_y_hole;
    double final_mz_exact = base_mz - mom_z_hole;
 
    if (final_mass_exact <= 0.0) {
        std::cout << "Invalid: hole removes all mass (should not happen with valid PSO solution)" << std::endl;
    } else {
        double true_cm_x = final_mx_exact / final_mass_exact;
        double true_cm_y = final_my_exact / final_mass_exact;
        double true_cm_z = final_mz_exact / final_mass_exact;
 
        double exact_error = std::sqrt(std::pow(true_cm_x - 1.0, 2) +
                                       std::pow(true_cm_y - 0.0, 2) +
                                       std::pow(true_cm_z - 0.0, 2));
 
        std::cout << "Hybrid Mass (Baseline - Hole):   " << final_mass_exact << std::endl;
        std::cout << "Hybrid CM (Ground Truth):        (" << true_cm_x << ", "
                                                          << true_cm_y << ", "
                                                          << true_cm_z << ")" << std::endl;
        std::cout << "Hybrid Error from Target:        " << exact_error << std::endl;
 
        double method_diff = std::abs(error_final - exact_error);
        std::cout << "Delta (MC Verify vs Hybrid):     " << method_diff << std::endl;
 
        if (method_diff < 0.05) {
            std::cout << "\n✓ SUCCESS: Monte Carlo converges to physical ground truth." << std::endl;
        } else {
            std::cout << "\n⚠ WARNING: Residual discrepancy detected (consider increasing n_baseline or n_verify)." << std::endl;
        }
    }
 
    if (error_final < 0.01) {
        std::cout << "\n✓ ROBUST solution: error remains small even with many samples!" << std::endl;
    } else if (error_final < 0.1) {
        std::cout << "\n◎ MODERATE solution: could improve with more PSO iterations." << std::endl;
    } else {
        std::cout << "\n✗ UNSTABLE solution: consider increasing PSO population/iterations." << std::endl;
    }
    
    // Export domain geometry for visualization (with optimized hole)
    std::cout << "\nExporting domain geometry to drone_frames/" << std::endl;
    domain_verify.exportGeometry("./drone_frames", best_x, best_y, best_z, best_r);
    
    // Generate gnuplot visualization script
    mc::utils::createDroneVisualizationScript("visualize_drone.gp", "drone_frames/drone_domain.txt");
 
    return 0;
}