config - parameters retuned for socially-complaint behaviour and re…

…asonable performance [#92]

- tested in "passing in front" scenario (TIAGo simulation)

cfg/HumapPlanner.cfg

@@ -103,7 +103,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`", False)
+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_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", 12, 0, 40)
@@ -121,13 +121,14 @@ group_traj_social_speed.add("sfm_desired_speed_amplifier_min",         double_t,
 group_traj_social_speed.add("sfm_desired_speed_amplifier_max",         double_t, 0, "", +1.00, STG_AMP_MIN, STG_AMP_MAX)
 group_traj_social_speed.add("sfm_desired_speed_amplifier_granularity", double_t, 0, "", +1.00, STG_GRN_MIN, STG_GRN_MAX)
 # Parameters related to the interaction forces with dynamic objects
-## An: slightly affects trajectories but only when there are dynamic objects nearby.
-## Meaningful range: 0.0 - 200.00 with a granularity of 100.0.
+## An: **slightly** affects trajectories but only when there are dynamic objects nearby.
+## Meaningful range: -50.0 - 200.00 with a granularity of 100.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.00, STG_AMP_MIN, STG_AMP_MAX)
+group_traj_social_an.add("sfm_an_amplifier_min",         double_t, 0, "", -12.00, STG_AMP_MIN, STG_AMP_MAX)
 group_traj_social_an.add("sfm_an_amplifier_max",         double_t, 0, "", +1.00, STG_AMP_MIN, STG_AMP_MAX)
-group_traj_social_an.add("sfm_an_amplifier_granularity", double_t, 0, "", +1.00, STG_GRN_MIN, STG_GRN_MAX)
-## Bn: negligible effect of manipulating the amplifier values
+group_traj_social_an.add("sfm_an_amplifier_granularity", double_t, 0, "", +13.00, STG_GRN_MIN, STG_GRN_MAX)
+## Bn: some effect of manipulating the amplifier values between -1.0 and 1.0
+## When the performance allows, set "min" to 0.0 and "max" to 1.0.
 group_traj_social_bn = group_traj_social.add_group("Bn", type="hide")
 group_traj_social_bn.add("sfm_bn_amplifier_min",         double_t, 0, "", +1.00, STG_AMP_MIN, STG_AMP_MAX)
 group_traj_social_bn.add("sfm_bn_amplifier_max",         double_t, 0, "", +1.00, STG_AMP_MIN, STG_AMP_MAX)
@@ -146,30 +147,36 @@ group_traj_social_ap.add("sfm_ap_amplifier_min",         double_t, 0, "", -10.00
 group_traj_social_ap.add("sfm_ap_amplifier_max",         double_t, 0, "Even +30.0 makes sense here when performance is not an issue",  +5.00, STG_AMP_MIN, STG_AMP_MAX)
 group_traj_social_ap.add("sfm_ap_amplifier_granularity", double_t, 0, "", +15.00, STG_GRN_MIN, STG_GRN_MAX)
 ## Bp: negligible (minimal effect observed for the range 0.0 - 5.0 with a granularity of 5.0).
+## Including 0 amplifier is cricital to differentiate trajectories but the additional ones are not very reasonable.
+## Setting 1.0 in "max" might be sufficient when only static obstacles are nearby.
 group_traj_social_bp = group_traj_social.add_group("Bp", type="hide")
-group_traj_social_bp.add("sfm_bp_amplifier_min",         double_t, 0, "", +0.00, STG_AMP_MIN, STG_AMP_MAX)
-group_traj_social_bp.add("sfm_bp_amplifier_max",         double_t, 0, "", +4.00, STG_AMP_MIN, STG_AMP_MAX)
-group_traj_social_bp.add("sfm_bp_amplifier_granularity", double_t, 0, "", +4.00, STG_GRN_MIN, STG_GRN_MAX)
-## Cp: changing this parameter produces very similar trajectories, except the negative values.
-## Meaningful range is from -6.0 to 1.0.
+group_traj_social_bp.add("sfm_bp_amplifier_min",         double_t, 0, "", +0.50, STG_AMP_MIN, STG_AMP_MAX)
+group_traj_social_bp.add("sfm_bp_amplifier_max",         double_t, 0, "", +1.00, STG_AMP_MIN, STG_AMP_MAX)
+group_traj_social_bp.add("sfm_bp_amplifier_granularity", double_t, 0, "", +1.00, STG_GRN_MIN, STG_GRN_MAX)
+## Cp: changing this parameter produces quite similar trajectories, but increasing the values helps to produce
+## trajectories avoiding people more.
+## Meaningful range is from -6.0 to 10.0.
 ## The biggest spread (with dynamic objects) observed at -6.0 - -4.0 with a granularity of 0.5. However, such a range
 ## might lead to trajectories that are not feasible (e.g., violate min vel. limits).
 group_traj_social_cp = group_traj_social.add_group("Cp", type="hide")
-group_traj_social_cp.add("sfm_cp_amplifier_min",         double_t, 0, "Starting from -1.0 gives a wider spread than -4.0 or -2.0", -1.00, STG_AMP_MIN, STG_AMP_MAX)
-group_traj_social_cp.add("sfm_cp_amplifier_max",         double_t, 0, "", +1.00, STG_AMP_MIN, STG_AMP_MAX)
-group_traj_social_cp.add("sfm_cp_amplifier_granularity", double_t, 0, "", +2.00, STG_GRN_MIN, STG_GRN_MAX)
+group_traj_social_cp.add("sfm_cp_amplifier_min",         double_t, 0, "Starting from -1.0 gives a wider spread than -4.0 or -2.0", +1.00, STG_AMP_MIN, STG_AMP_MAX)
+group_traj_social_cp.add("sfm_cp_amplifier_max",         double_t, 0, "", +10.00, STG_AMP_MIN, STG_AMP_MAX)
+group_traj_social_cp.add("sfm_cp_amplifier_granularity", double_t, 0, "", +9.00, STG_GRN_MIN, STG_GRN_MAX)
 # Parameters related to the interaction forces with static objects
 ## Aw: amplyfing the parameter has a significant impact on generated trajectories for the range
 ## of -10.0 - +200.0 with a granularity of 5.0. Remember that this won't affect trajectories when only dynamic objects
 ## are nearby.
+## Setting the maximum amplifier value higher than 1.0 will most likely produce mostly backward trajectories (when
+## stationary).
 ## Slightly performance-affecting, but also a good choice is a triplet of (min: +0.50, max: +4.50, gran.: +2.00).
 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 might also be tried if performance is not an issue", +5.00, STG_AMP_MIN, STG_AMP_MAX)
-group_traj_social_aw.add("sfm_aw_amplifier_max",         double_t, 0, "+2.50 with a granularity of 1.0 is also a reasonable pair", +75.00, STG_AMP_MIN, STG_AMP_MAX)
-group_traj_social_aw.add("sfm_aw_amplifier_granularity", double_t, 0, "", +70.00, STG_GRN_MIN, STG_GRN_MAX)
+group_traj_social_aw.add("sfm_aw_amplifier_min",         double_t, 0, "-1.0 might also be tried if performance is not an issue", +0.50, STG_AMP_MIN, STG_AMP_MAX)
+group_traj_social_aw.add("sfm_aw_amplifier_max",         double_t, 0, "+2.50 with a granularity of 1.0 is also a reasonable pair", +1.00, STG_AMP_MIN, STG_AMP_MAX)
+group_traj_social_aw.add("sfm_aw_amplifier_granularity", double_t, 0, "", +0.50, STG_GRN_MIN, STG_GRN_MAX)
 ## Bw: effective range is 0.0 - 4.0 (visible at a granularity up to 1.0).
+## With some sets of other parameters, setting only 1.0 may affect producing only non-feasible trajectories!
 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.50, STG_AMP_MIN, STG_AMP_MAX)
+group_traj_social_bw.add("sfm_bw_amplifier_min",         double_t, 0, "", +2.50, STG_AMP_MIN, STG_AMP_MAX)
 group_traj_social_bw.add("sfm_bw_amplifier_max",         double_t, 0, "", +4.50, STG_AMP_MIN, STG_AMP_MAX)
 group_traj_social_bw.add("sfm_bw_amplifier_granularity", double_t, 0, "", +2.00, STG_GRN_MIN, STG_GRN_MAX)
 ## As: tested with dynamic objects in front of the robot. Slight differences in trajectories with amplifiers