io.h

Go to the documentation of this file.

#pragma once
 
#include <string>
 
namespace armarx::navigation::algorithms::orientation_aware::smoothing::io
{
    struct SmoothingParams
    {
        // Pre-processing (hill climb)
        double clearance = 100.0;
        double hill_climb_step_size = 10.0;
        int hill_climb_max_iterations = 80;
 
        // Obstacle avoidance
        double w_obs = 2000.0;
        double obs_max_distance = 500.0;
        double obs_barrier_eps_mm = 1.0;
        bool use_obs = true;
        bool use_fuzzy_orientation = true;
        int fuzzy_orientation_window_bins = 2;
        double fuzzy_orientation_alpha = 5.0;
        double w_theta_track = 10.0;
 
        // Obstacle-based pose freezing: hold interior waypoints constant during
        // pose optimization if their pre-processed clearance is below threshold.
        bool use_obstacle_freeze = true;
        double obstacle_freeze_threshold = 100.0;
 
        // Pose smoothness
        double w_boundary = 10.0;
        double w_pose_smooth = 10.0;
        double w_orientation_smooth = 100.0;
        double w_pose_jerk = 0.0;
        bool use_jerk = false;
        double w_robot_smooth = 0.0;
        bool use_robot_smooth = true;
 
        // Waypoint spacing
        double w_spacing = 1.0;
        bool use_soft_spacing = true;
        double spacing_relative_deviation_threshold = 0.3;
        double w_spacing_linear = 1.0;
        double w_spacing_quadratic = 5.0;
 
        // Velocity profile
        double start_velocity = 100.0;
        double w_accel_decel_limit = 1.0;
        double max_accel_per_segment = 100.0;
        double max_decel_per_segment = 100.0;
        double w_nominal_velocity = 0.01;
        double w_velocity_proximity = 0.5;
        double slow_down_distance = 300.0;
        double min_velocity_near_obstacle = 100.0;
 
        // Tracking
        double w_track = 0.05;
        bool use_tracking = true;
 
        // Clearance-adaptive tracking: fade out tracking residuals in open space
        bool use_tracking_clearance_adaptive = true;
        double tracking_fade_start_clearance = 100.0;
        double tracking_fade_end_clearance = 300.0;
 
        // Robot-pose proximity to pre-processed trajectory
        bool use_robot_pose_proximity = false;
        double robot_pose_proximity_threshold_mm = 200.0;
        double w_robot_pose_proximity = 10.0;
        double robot_pose_proximity_eps_mm = 1.0;
 
        // Ceres solver
        int max_iterations = 15;
        int num_threads = 4;
        double initial_trust_region_radius = 100.0;
        double max_trust_region_radius = 500.0;
        double min_trust_region_radius = 10.0;
 
        // Planner behavior
        bool always_return_smoothed = false;
    };
 
    SmoothingParams
    loadSmoothingParams(const std::string& filePath = "algorithms/orientation-aware-smoothing.cfg");
 
} // namespace armarx::navigation::algorithms::orientation_aware::smoothing::io