config - updated baseline parameters for further tuning (somehow re…

…lated to #92)

cfg/HuberoPlanner.cfg

@@ -12,14 +12,14 @@ group_limits = gen.add_group("Limits", type="tab")
 group_limits_generic_lp = group_limits.add_group("BaseLocalPlanner", type="hide")
 add_generic_localplanner_params(group_limits_generic_lp)
 group_limits_custom = group_limits.add_group("Custom", type="hide")
-group_limits_custom.add("twist_rotation_compensation", double_t, 0, "Defines how much robot tries to additionally follow force direction compared to pure transformation of force into local velocity. This is related to experimental twist computation heuristics that strains vector field assumptions.", 0.0, 0.0, 2.0)
+group_limits_custom.add("twist_rotation_compensation", double_t, 0, "Damping factor (inverted?) of the forceToVelocity transformation, ref. to (Ferrer, 2017). Defines how much the robot tries to additionally follow force direction compared to pure transformation of force into local velocity.", 0.25, 0.0, 2.0)
 group_limits_custom.add("maintain_vel_components_rate", bool_t, 0, "When true and any vel. component is not within limits, others will be modified according to the one that cannot be changed as requested; others will be proportionally scaled; setting this to true loosely corresponds to agreeing with dynamics violation, but keeping the path as intended.", True)
 
 #
 group_general = gen.add_group("General", type="tab")
 
 group_general.add("planning_approach", bool_t, 0, "Whether to use planning (True) or proactive approach (False) for trajectory generation", True)
-group_general.add("sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 2.0, 0)
+group_general.add("sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds. Too high with impede high velocities as traj. points will exceed the local costmap.", 3.0, 0)
 group_general.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.2, 0)
 group_general.add("angular_sim_granularity", double_t, 0, "The granularity with which to check for collisions for rotations in radians", 0.1, 0)
 group_general.add("local_goal_distance_multiplier", double_t, 0, "The factor for the distance from the center point of the robot to place the local goal for self-driven force calculation; consider reducing it for proactive approach; resultant distance must not exceed local costmap bounds.", 1.1, 0.10, 8.0)
@@ -41,7 +41,7 @@ group_sfm.add("sfm_fov_factor_method", int_t, 0, "Select the formulation that co
 group_sfm.add("mass", double_t, 0, "Description", 14.5, 0.1, 250.0)
 group_sfm.add("internal_force_factor", double_t, 0, "How much robot tries to reach the goal position using shortest path to goal. For non-planning approach keep it at approx. 4.2", 0.75, 0.0, 50.0)
 group_sfm.add("static_interaction_force_factor", double_t, 0, "How much robot tries to maintain safe distance from static obstacles. For non-planning approach keep it at approx. 11.0", 4.9, 0.0, 300.0)
-group_sfm.add("dynamic_interaction_force_factor", double_t, 0, "How much robot tries to maintain safe distance from dynamic obstacles", 1.0, 0.0, 300.0)
+group_sfm.add("dynamic_interaction_force_factor", double_t, 0, "How much robot tries to maintain safe distance from dynamic obstacles", 2.0, 0.0, 300.0)
 group_sfm.add("min_force", double_t, 0, "Minimum force - vectors shorter than that will be extended, which may prevent from being stuck.", 5.0, 0.0, 100.0)
 group_sfm.add("max_force", double_t, 0, "Maximum force - vectors longer than that will be truncated", 300.0, 0.0, 1500.0)
 group_sfm.add("heterogenous_population", bool_t, 0, "Setting true causes SFM params to be defined in non-deterministic way (std deviation around mean value)", False)
@@ -93,7 +93,7 @@ group_fis.add("as", double_t, 0, "Levelling factor that also directly affects am
 group_traj_gen = gen.add_group("TrajectoryGeneration", type="tab")
 group_traj_gen.add("use_equisampled_velocities_generator", bool_t, 0, "True enables generator that produces evenly spaced elements from a list of feasible velocities", True)
 group_traj_gen.add("use_social_trajectory_generator", bool_t, 0, "True enables generator that produces model-based trajectories", True)
-group_traj_gen.add("equisampled_continued_acceleration", bool_t, 0, "If set to true, the generator will recompute feasible velocities in each step and will restrict the velocities to those that do not overshoot the goal in sim_time. Otherwise, when false, the generator will sample velocities during the first iteration and will not take the goal into account (as in DWA approach). Legacy parameter name (nav. stack) `use_dwa`", True)
+group_traj_gen.add("equisampled_continued_acceleration", bool_t, 0, "If set to true, the generator will recompute feasible velocities in each step and will restrict the velocities to those that do not overshoot the goal in sim_time. Otherwise, when false, the generator will sample velocities during the first iteration and will not take the goal into account (as in DWA approach). Legacy parameter name (nav. stack) `use_dwa`", False)
 group_traj_gen.add("equisampled_vx", int_t, 0, "How many velocity samples along platform's X axis will be checked", 3, 0, 40)
 group_traj_gen.add("equisampled_vy", int_t, 0, "How many velocity samples along platform's Y axis will be checked", 1, 0, 40)
 group_traj_gen.add("equisampled_vth", int_t, 0, "How many velocity samples around platform's Z axis will be checked", 10, 0, 40)
@@ -102,8 +102,8 @@ group_traj_gen.add("equisampled_min_vel_x", double_t, 0, "Minimum linear velocit
 # Trajectory sampling group parameters
 group_traj_social = gen.add_group("TrajectorySocial", type="tab")
 group_traj_social_speed = group_traj_social.add_group("SpeedDesired", type="hide")
-group_traj_social_speed.add("sfm_desired_speed_amplifier_min", double_t, 0, "", 0.0, -300.0, +300.0)
-group_traj_social_speed.add("sfm_desired_speed_amplifier_max", double_t, 0, "", 1.0, -300.0, +300.0)
+group_traj_social_speed.add("sfm_desired_speed_amplifier_min", double_t, 0, "", 0.5, -300.0, +300.0)
+group_traj_social_speed.add("sfm_desired_speed_amplifier_max", double_t, 0, "", 1.5, -300.0, +300.0)
 group_traj_social_speed.add("sfm_desired_speed_amplifier_granularity", double_t, 0, "", 0.5, -300.0, +300.0)
 group_traj_social_an = group_traj_social.add_group("An", type="hide")
 group_traj_social_an.add("sfm_an_amplifier_min", double_t, 0, "", 1.0, -300.0, +300.0)
@@ -132,11 +132,11 @@ group_traj_social_cp.add("sfm_cp_amplifier_granularity", double_t, 0, "", 1.0, 0
 group_traj_social_aw = group_traj_social.add_group("Aw", type="hide")
 group_traj_social_aw.add("sfm_aw_amplifier_min", double_t, 0, "", -1.0, -300.0, +300.0)
 group_traj_social_aw.add("sfm_aw_amplifier_max", double_t, 0, "", +1.0, -300.0, +300.0)
-group_traj_social_aw.add("sfm_aw_amplifier_granularity", double_t, 0, "", 0.5, 0.0, +300.0)
+group_traj_social_aw.add("sfm_aw_amplifier_granularity", double_t, 0, "", 1.0, 0.0, +300.0)
 group_traj_social_bw = group_traj_social.add_group("Bw", type="hide")
-group_traj_social_bw.add("sfm_bw_amplifier_min", double_t, 0, "", -0.5, -300.0, +300.0)
-group_traj_social_bw.add("sfm_bw_amplifier_max", double_t, 0, "", 2.5, -300.0, +300.0)
-group_traj_social_bw.add("sfm_bw_amplifier_granularity", double_t, 0, "", 0.5, 0.0, +300.0)
+group_traj_social_bw.add("sfm_bw_amplifier_min", double_t, 0, "", -1.0, -300.0, +300.0)
+group_traj_social_bw.add("sfm_bw_amplifier_max", double_t, 0, "", 2.0, -300.0, +300.0)
+group_traj_social_bw.add("sfm_bw_amplifier_granularity", double_t, 0, "", 0.25, 0.0, +300.0)
 group_traj_social_as = group_traj_social.add_group("As", type="hide")
 group_traj_social_as.add("fis_as_amplifier_min", double_t, 0, "", 1.0, -300.0, +300.0)
 group_traj_social_as.add("fis_as_amplifier_max", double_t, 0, "", 1.0, -300.0, +300.0)
@@ -145,39 +145,43 @@ group_traj_social_as.add("fis_as_amplifier_granularity", double_t, 0, "", 1.0, 0
 # Trajectory generation group parameters
 group_cost = gen.add_group("Cost", type="tab")
 
-group_cost.add("path_distance_scale", double_t, 0, "The weight for the path distance part of the cost function", 1.5, 0.0)
-group_cost.add("goal_distance_scale", double_t, 0, "The weight for the goal distance part of the cost function", 8.8, 0.0)
+## Weights/scales of the cost functions
+group_cost.add("path_distance_scale", double_t, 0, "The weight for the path distance part of the cost function", 7.9, 0.0)
+group_cost.add("goal_distance_scale", double_t, 0, "The weight for the goal distance part of the cost function", 10.58, 0.0)
 group_cost.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.02, 0.0)
 group_cost.add("occdist_sum_scores", bool_t, 0, "Whether to sum up the scores along the path or use the maximum one", False)
-group_cost.add("occdist_separation", double_t, 0, "The minimum separation that should be kept from obstacles", 0.05, 0.0)
+group_cost.add("occdist_separation", double_t, 0, "The minimum separation that should be kept from obstacles", 0.025, 0.0)
 occdist_kernel = gen.enum([
   gen.const("SeparationKernelCross", int_t, 0, "Kernel checking points located at `occdist_separation`: in front of, behind, on the right, and on the left."),
   gen.const("SeparationKernelRectangle", int_t, 1, "Kernel checking points located at `occdist_separation` at 8 surrounding directions.")
   ],
   "Occdist Separation Kernel Type")
-group_cost.add("occdist_separation_kernel", int_t, 0, "Type of kernel that the robot's footprint is enlarged with.", 0, 0, 1, edit_method=occdist_kernel)
+group_cost.add("occdist_separation_kernel", int_t, 0, "Type of kernel that the robot's footprint is enlarged with.", 1, 0, 1, edit_method=occdist_kernel)
+group_cost.add("ttc_scale", double_t, 0, "The weight for the time to collision cost function (scores a whole trajectory instead of a single cell)", 3.0, 0.0)
 group_cost.add("alignment_scale", double_t, 0, "The weight for the alignment with the global plan. In a classical DWA implementation equal to `path_distance_scale`", 1.4, 0.0)
-group_cost.add("goal_front_scale", double_t, 0, "The weight for the achievement of the additional goal point placed in front of the mobile base. In a classical DWA implementation equal to `goal_distance_scale`", 0.0, 0.0)
+group_cost.add("goal_front_scale", double_t, 0, "The weight for the achievement of the additional goal point placed in front of the mobile base. In a classical DWA implementation equal to `goal_distance_scale`", 5.0, 0.0)
+
 group_cost.add("backward_scale", double_t, 0, "The weight for the backward motion penalisation", 0.08, 0.0)
-group_cost.add("ttc_scale", double_t, 0, "The weight for the time to collision cost function (scores whole trajectory instead of a single cell)", 3.0, 0.0)
 group_cost.add("heading_change_smoothness_scale", double_t, 0, "The weight for the cost function that penalizes robot rotational velocity changes", 11.0, 0.0)
 group_cost.add("velocity_smoothness_scale", double_t, 0, "The weight for the cost function that penalizes velocity changes along trajectory", 15.0, 0.0)
-group_cost.add("heading_dir_scale", double_t, 0, "The weight for the cost function that penalizes trajectories that leading towards human center", 7.0, 0.0)
-group_cost.add("personal_space_scale", double_t, 0, "The weight for the cost function that penalizes trajectories that intrude human personal space", 7.0, 0.0)
-group_cost.add("fformation_space_scale", double_t, 0, "The weight for the cost function that penalizes trajectories that intrude F-formation space", 7.0, 0.0)
+group_cost.add("backward_penalty", double_t, 0, "Penalty (cost) value assigned to trajectory that starts with backward motion", 25.0, 0.0)
+group_cost.add("heading_dir_scale", double_t, 0, "The weight for the cost function that penalizes trajectories that leading towards human center", 10.0, 0.0)
+group_cost.add("personal_space_scale", double_t, 0, "The weight for the cost function that penalizes trajectories that intrude human personal space", 25.0, 0.0)
+group_cost.add("fformation_space_scale", double_t, 0, "The weight for the cost function that penalizes trajectories that intrude F-formation space", 30.0, 0.0)
 group_cost.add("passing_speed_scale", double_t, 0, "The weight for the cost function that penalizes robot trajectories that are not compliant with proper speeds of passing people", 1.0, 0.0)
 
+## Parameters of the cost functions
 group_cost.add("oscillation_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0.0)
 group_cost.add("oscillation_reset_angle", double_t, 0, "The angle the robot must turn before oscillation flags are reset, in radians", 0.2, 0.0)
 
+# Unused, see issue #113 for 2 parameters below
 group_cost.add("scaling_speed", double_t, 0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s", 0.25, 0.0)
 group_cost.add("max_scaling_factor", double_t, 0, "The maximum factor to scale the robot's footprint by", 0.2, 0.0)
-group_cost.add("backward_penalty", double_t, 0, "Penalty (cost) value assigned to trajectory that starts with backward motion", 25.0, 0.0)
 
 group_cost.add("ttc_rollout_time", double_t, 0, "The duration of maximum world state prediction to detect robot collision (further beyond `sim_time`)", 0.0, 0.0)
-group_cost.add("ttc_collision_distance", double_t, 0, "The distance threshold for collision detection in TTC prediction", 0.015, 0.0)
+group_cost.add("ttc_collision_distance", double_t, 0, "The distance threshold for collision detection in TTC prediction", 0.05, 0.0)
 
-group_cost.add("forward_point_distance", double_t, 0, "The distance from the center point of the robot to place an additional scoring point, in meters", 0.325, 0.1, 2.0)
+group_cost.add("forward_point_distance", double_t, 0, "The distance from the center point of the robot to place an additional scoring point, in meters. Set to zero to discard alignment.", 0.325, 0.1, 2.0)
 
 # Diagnostics
 group_diagnostics = gen.add_group("Diagnostics", type="tab")