io.h

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */
 
#pragma once
 
#include <string>
 
namespace armarx::navigation::algorithms::spfa::smoothing::io
{
    struct SmoothingParams
    {
        /**
         * Distance to obstacles [mm] the optimizer aims for.
         *
         * This is a target, not a limit: the obstacle barrier falls to zero here, so the
         * smoother has no reason to seek more room than this and a growing reason to seek more
         * as it drops below. Raising it pushes paths further from furniture.
         */
        double clearance = 75.0;
 
        /**
         * Distance to obstacles [mm] below which a path is actually refused.
         *
         * Kept separate from `clearance` because the two answer different questions. The
         * target says where the smoother should try to be; this says what is unsafe. Enforcing
         * the target as the limit throws away paths that are perfectly drivable -- the costmap
         * already has the robot's radius subtracted, so a path measured at 30 mm still clears
         * obstacles by 30 mm of real air, and refusing it means falling back to the unsmoothed
         * path, which is rougher and no safer.
         *
         * Its real job is to stop the smoother trading clearance for smoothness without bound:
         * straightening a corner is worth more to the cost function than the room it gives up,
         * so without a floor a path would cut corners into obstacles outright.
         *
         * Note the costmap is sampled at cell centres and read by nearest cell, so a reported
         * distance carries up to about half a cell diagonal of error -- 70 mm at the default
         * 100 mm cells. A limit far below that is being enforced within the noise of the
         * measurement rather than on the geometry.
         */
        double min_clearance = 25.0;
 
        // Width [mm] of the obstacle influence band beyond the clearance. The cost falls to
        // exactly zero at `clearance + obs_max_distance` and also sets how sharply penetration
        // into the clearance is punished.
        double obs_max_distance = 300.0;
 
        /**
         * Spacing [mm] of the obstacle samples along a segment.
         *
         * Must be no coarser than the trajectory checker's own sampling, or the optimizer is
         * judged at points it never evaluated: a segment can pass every sample the cost
         * function looks at and still dip below the clearance between two of them. This was a
         * fixed count of 3 per segment regardless of length, which matched the checker only
         * while segments happened to be about 200 mm long.
         */
        double segment_sample_spacing = 50.0;
 
        /**
         * Judge a path against what it was given, when the input is already inside the limit.
         *
         * The limit below is an absolute floor, and on a request whose own start, goal or
         * route already sits inside it the floor cannot be met by anything -- so the smoothed
         * path is refused and the unsmoothed one is driven instead. That trade is not a safety
         * decision. Measured over the requests it applies to, the path handed back is *the same
         * distance from obstacles* as the smoothed one would have been, and considerably
         * rougher; the rejection buys nothing at all.
         *
         * With this on, the floor for such a request becomes the clearance its own input
         * already had, so the smoother is required not to make the worst point worse rather
         * than to fix something it cannot reach. Requests whose input clears the limit are
         * unaffected -- for them the floor is the limit, exactly as before.
         */
        bool allow_degraded_input = true;
 
        /**
         * Scale the tracking weight by the clearance at each waypoint's reference position.
         *
         * Strong where clearance is scarce, fading to nothing where it is plentiful. The soft
         * counterpart to pinning: it discourages a tight waypoint from wandering without
         * removing its freedom, so it cannot create the half-constrained segments that pinning
         * does.
         */
        bool use_tracking_clearance_adaptive = false;
 
        /// Clearance [mm] at which the leash starts to relax.
        double tracking_fade_start_clearance = 100.0;
 
        /// Clearance [mm] beyond which tracking is off entirely.
        double tracking_fade_end_clearance = 300.0;
 
        /**
         * Hold interior waypoints below `obstacle_freeze_threshold` fixed during the solve.
         *
         * The intent is robustness: a waypoint already close to an obstacle is one the
         * optimizer could only make worse. Measure before enabling -- see the threshold below.
         */
        bool use_obstacle_freeze = false;
 
        /**
         * Clearance [mm] at or below which an interior waypoint is pinned.
         *
         * Pinning a waypoint makes both segments adjoining it half-constrained, which is
         * exactly the structure that produces the rejections this smoother already has at its
         * commanded endpoints: the checker samples those segments, and with one end immovable
         * the optimizer often cannot lift a dip out of them. Freezing turns every tight
         * waypoint into another such endpoint, and it does so precisely where clearance is
         * already scarce.
         */
        double obstacle_freeze_threshold = 100.0;
 
        /// Lower bound on the samples per segment, so a short segment is still evaluated.
        int segment_obstacle_samples = 3;
 
        /// Upper bound, so a long segment cannot make the problem huge.
        int max_segment_obstacle_samples = 16;
 
        // Pass 1: smooth near original path
        double pass1_w_obs = 100.0;
        double pass1_w_pose_smooth = 5.0;
        double pass1_w_spacing = 0.001;
        double pass1_w_tracking = 0.2;
        double pass1_w_safe_tracking = 0.05;
 
        // Pass 2: push away from obstacles
        double pass2_w_obs = 500.0;
        double pass2_w_pose_smooth = 15.0;
        double pass2_w_spacing = 0.001;
        double pass2_w_tracking = 0.1;
        double pass2_w_safe_tracking = 0.02;
 
        // Pass 3: final polish
        double pass3_w_obs = 1000.0;
        double pass3_w_pose_smooth = 25.0;
        double pass3_w_spacing = 0.0;
        double pass3_w_tracking = 0.05;
        double pass3_w_safe_tracking = 0.01;
 
        // Repair pass (used if collisions remain after pass 3)
        double repair_w_obs = 2000.0;
        double repair_w_pose_smooth = 30.0;
        double repair_w_spacing = 0.0;
        double repair_w_tracking = 0.2;
        double repair_w_safe_tracking = 0.05;
        int repair_max_iterations = 100;
 
        // Bounded tracking to safe (hill-climbed) path.
        // The hard deviation limit is also the maximum allowed movement from the
        // original SPFA path.
        double tracking_deadzone = 25.0;
        double tracking_max_deviation = 150.0;
        double w_tracking_in_bounds = 1.0;
        double w_tracking_hard = 10000.0;
 
        // Ceres solver
        int max_iterations = 150;
        int num_threads = 4;
        double initial_trust_region_radius = 100.0;
        double max_trust_region_radius = 500.0;
        double min_trust_region_radius = 1.0;
    };
 
    SmoothingParams
    loadSmoothingParams(const std::string& filePath = "algorithms/spfa-smoothing.cfg");
 
} // namespace armarx::navigation::algorithms::spfa::smoothing::io