F config - parameters tuned for socially-complaint behaviour and re…

…asonable performance [#92]

cfg/HuberoPlanner.cfg

@@ -41,21 +41,22 @@ 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", 2.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", 5.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)
 soi_enum = gen.enum([ gen.const("elliptical",      	int_t, 0, "a.k.a. v2014"),
                        gen.const("repulsive_evasive",	int_t, 1, "a.k.a. v2011")],
                      "Static Object Interaction")
 group_sfm.add("static_object_interaction_type", int_t, 0, "Description", 0, 0, 1, edit_method=soi_enum)
-group_sfm.add("human_force_formulation_dynamic", bool_t, 0, "Whether to use a force formulation designated for an interaction with dynamic objects regardless of the human's speed", False)
-group_sfm.add("filter_forces", bool_t, 0, "Whether to apply filtering operations to the resultant forces", True)
+group_sfm.add("human_force_formulation_dynamic", bool_t, 0, "Whether to use a force formulation designated for an interaction with dynamic objects regardless of the human's speed", True)
+group_sfm.add("filter_forces", bool_t, 0, "Whether to apply filtering operations to the resultant forces", False)
 group_sfm.add("disable_interaction_forces", bool_t, 0, "Description", False)
 
 group_param_eqn_mean = group_sfm.add_group("EquationParams", type="hide")
 SFM_PARAM_MIN = -50.0
 SFM_PARAM_MAX = +50.0
+# Values below include custom multipliers as these are directly used in the non-planning approach
 group_param_eqn_mean.add("an", double_t, 0, "Base parameter value estimated in a paper Seer et al., 2014, here multiplied by a custom multiplier", 0.2615 *   -8.000, SFM_PARAM_MIN, SFM_PARAM_MAX)
 group_param_eqn_mean.add("bn", double_t, 0, "Base parameter value estimated in a paper Seer et al., 2014, here multiplied by a custom multiplier", 0.4026 *   +5.000, SFM_PARAM_MIN, SFM_PARAM_MAX)
 group_param_eqn_mean.add("cn", double_t, 0, "Base parameter value estimated in a paper Seer et al., 2014, here multiplied by a custom multiplier", 2.1614 *   +1.500, SFM_PARAM_MIN, SFM_PARAM_MAX)
@@ -104,46 +105,60 @@ 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")
-STG_AMP_MIN = -20.0 # default minimum value of an amplifier
-STG_AMP_MAX = +20.0 # default maximum value of an amplifier
+STG_AMP_MIN = -200.0 # default minimum value of an amplifier
+STG_AMP_MAX = +200.0 # default maximum value of an amplifier
 STG_GRN_MIN = 0.0 # default minimum value of an amplifier's granularity
-STG_GRN_MAX = +20.0 # default minimum value of an amplifier's granularity
+STG_GRN_MAX = +50.0 # default minimum value of an amplifier's granularity
+# Meaningful range: 0.50 - 1.50 with a granularity of 0.50
 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.50, STG_AMP_MIN, STG_AMP_MAX)
-group_traj_social_speed.add("sfm_desired_speed_amplifier_max",         double_t, 0, "", +1.50, STG_AMP_MIN, STG_AMP_MAX)
-group_traj_social_speed.add("sfm_desired_speed_amplifier_granularity", double_t, 0, "", +0.50, STG_GRN_MIN, STG_GRN_MAX)
+group_traj_social_speed.add("sfm_desired_speed_amplifier_min",         double_t, 0, "", +1.00, STG_AMP_MIN, STG_AMP_MAX)
+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.
 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_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_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)
 group_traj_social_bn.add("sfm_bn_amplifier_granularity", double_t, 0, "", +1.00, STG_GRN_MIN, STG_GRN_MAX)
+## Cn: negative amp. values produce failures in static obstacle interaction forces (NaNs).
+## Introduces some effects for values from -1.0 to 1.0 with a granularity of 0.1, but still, rather very similar
+## trajectories will be obtained.
 group_traj_social_cn = group_traj_social.add_group("Cn", type="hide")
-group_traj_social_cn.add("sfm_cn_amplifier_min",         double_t, 0, "", +1.00, STG_AMP_MIN, STG_AMP_MAX)
+group_traj_social_cn.add("sfm_cn_amplifier_min",         double_t, 0, "", -1.00, STG_AMP_MIN, STG_AMP_MAX)
 group_traj_social_cn.add("sfm_cn_amplifier_max",         double_t, 0, "", +1.00, STG_AMP_MIN, STG_AMP_MAX)
-group_traj_social_cn.add("sfm_cn_amplifier_granularity", double_t, 0, "", +1.00, STG_GRN_MIN, STG_GRN_MAX)
+group_traj_social_cn.add("sfm_cn_amplifier_granularity", double_t, 0, "", +1.00, STG_GRN_MIN, STG_GRN_MAX) # consider +2.0 for performance
+## Ap: the effects are visible when dynamic objects are nearby.
+## Effective range from -60 to 80 (with granularity of 5.0).
 group_traj_social_ap = group_traj_social.add_group("Ap", type="hide")
-group_traj_social_ap.add("sfm_ap_amplifier_min",         double_t, 0, "", +1.00, STG_AMP_MIN, STG_AMP_MAX)
-group_traj_social_ap.add("sfm_ap_amplifier_max",         double_t, 0, "", +1.00, STG_AMP_MIN, STG_AMP_MAX)
-group_traj_social_ap.add("sfm_ap_amplifier_granularity", double_t, 0, "", +1.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).
 group_traj_social_bp = group_traj_social.add_group("Bp", type="hide")
-group_traj_social_bp.add("sfm_bp_amplifier_min",         double_t, 0, "", +1.00, 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 very similar trajectories, except the negative values.
+## Meaningful range is from -6.0 to 1.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, "", +1.00, STG_AMP_MIN, STG_AMP_MAX)
+group_traj_social_cp.add("sfm_cp_amplifier_min",         double_t, 0, "", -2.00, STG_AMP_MIN, STG_AMP_MAX) # gives a wider spread than -4.0
 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, "", +1.00, STG_GRN_MIN, STG_GRN_MAX)
+group_traj_social_cp.add("sfm_cp_amplifier_granularity", double_t, 0, "", +3.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 - +65.0 with a granularity of 5.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.00, STG_AMP_MIN, STG_AMP_MAX)
+group_traj_social_aw.add("sfm_aw_amplifier_min",         double_t, 0, "", +0.00, STG_AMP_MIN, STG_AMP_MAX)
 group_traj_social_aw.add("sfm_aw_amplifier_max",         double_t, 0, "", +1.00, STG_AMP_MIN, STG_AMP_MAX)
-group_traj_social_aw.add("sfm_aw_amplifier_granularity", double_t, 0, "", +1.00, STG_GRN_MIN, STG_GRN_MAX)
+## Bw: effective range is 0.0 - 4.0 (visible at a granularity up to 1.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, "", -1.00, STG_AMP_MIN, STG_AMP_MAX)
+group_traj_social_bw.add("sfm_bw_amplifier_min",         double_t, 0, "", +1.00, STG_AMP_MIN, STG_AMP_MAX)
 group_traj_social_bw.add("sfm_bw_amplifier_max",         double_t, 0, "", +2.00, STG_AMP_MIN, STG_AMP_MAX)
-group_traj_social_bw.add("sfm_bw_amplifier_granularity", double_t, 0, "", +0.25, STG_GRN_MIN, STG_GRN_MAX)
+group_traj_social_bw.add("sfm_bw_amplifier_granularity", double_t, 0, "", +1.00, STG_GRN_MIN, STG_GRN_MAX)
+## As: tested with dynamic objects in front of the robot. Slight differences in trajectories with amplifiers
+## within 1.0 - 51.0 (gran. 50.0). Increasing the main As factor makes more sense here.
 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.00, STG_AMP_MIN, STG_AMP_MAX)
 group_traj_social_as.add("fis_as_amplifier_max",         double_t, 0, "", +1.00, STG_AMP_MIN, STG_AMP_MAX)