io.h
Go to the documentation of this file.
1/**
2 * This file is part of ArmarX.
3 *
4 * ArmarX is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License version 2 as
6 * published by the Free Software Foundation.
7 *
8 * ArmarX is distributed in the hope that it will be useful, but
9 * WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 */
16
17#pragma once
18
19#include <string>
20
21namespace armarx::navigation::algorithms::spfa::smoothing::io
22{
23 struct SmoothingParams
24 {
25 // Pre-processing
26 double clearance = 75.0;
27 double hill_climb_step_size = 50.0;
28 int hill_climb_max_iterations = 100;
29
30 // Obstacle influence range
31 double obs_max_distance = 300.0;
32
33 // Number of interior obstacle samples per segment.
34 // With 3, the segment is evaluated at t=0.25, 0.5, 0.75.
35 int segment_obstacle_samples = 3;
36
37 // Pass 1: smooth near original path
38 double pass1_w_obs = 100.0;
39 double pass1_w_pose_smooth = 5.0;
40 double pass1_w_spacing = 0.001;
41 double pass1_w_tracking = 0.2;
42 double pass1_w_safe_tracking = 0.05;
43
44 // Pass 2: push away from obstacles
45 double pass2_w_obs = 500.0;
46 double pass2_w_pose_smooth = 15.0;
47 double pass2_w_spacing = 0.001;
48 double pass2_w_tracking = 0.1;
49 double pass2_w_safe_tracking = 0.02;
50
51 // Pass 3: final polish
52 double pass3_w_obs = 1000.0;
53 double pass3_w_pose_smooth = 25.0;
54 double pass3_w_spacing = 0.0;
55 double pass3_w_tracking = 0.05;
56 double pass3_w_safe_tracking = 0.01;
57
58 // Repair pass (used if collisions remain after pass 3)
59 double repair_w_obs = 2000.0;
60 double repair_w_pose_smooth = 30.0;
61 double repair_w_spacing = 0.0;
62 double repair_w_tracking = 0.2;
63 double repair_w_safe_tracking = 0.05;
64 int repair_max_iterations = 100;
65
66 // Bounded tracking to safe (hill-climbed) path.
67 // The hard deviation limit is also the maximum allowed movement from the
68 // original SPFA path.
69 double tracking_deadzone = 25.0;
70 double tracking_max_deviation = 150.0;
71 double w_tracking_in_bounds = 1.0;
72 double w_tracking_hard = 10000.0;
73
74 // Ceres solver
75 int max_iterations = 150;
76 int num_threads = 4;
77 double initial_trust_region_radius = 100.0;
78 double max_trust_region_radius = 500.0;
79 double min_trust_region_radius = 1.0;
80 };
81
82 SmoothingParams
83 loadSmoothingParams(const std::string& filePath = "algorithms/spfa-smoothing.cfg");
84
85} // namespace armarx::navigation::algorithms::spfa::smoothing::io
armarx::navigation::algorithms::spfa::smoothing::io::loadSmoothingParams
SmoothingParams loadSmoothingParams(const std::string &filePath)
Definition io.cpp:29