[#29, #71] trajectory generation parameters overhaul - switched to us…

…age of amplifiers for raw parameters of the motion model instead of multiplying resultant model components

- added SFM raw parameters (with tuning multipliers)
- added SFM raw params divided into mean and standard deviation pairs
- each SFM and FIS param available can be customized for trajectory generation via dynamic reconfigure
- changed `SocialTrajectoryGenerator::SampleAmplifierSet` members, added default ctor
- `SocialTrajectoryGenerator::computeForces` takes extra argument
- `SocialForceModel` class extended with setter/getter methods for equation parameters updating/restoring
- deleted deprecated `use_tuned_params` parameter

cfg/HuberoPlanner.cfg

@@ -38,13 +38,36 @@ group_sfm.add("dynamic_interaction_force_factor", double_t, 0, "How much robot t
 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)
-group_sfm.add("use_tuned_params", bool_t, 0, "Whether to apply SFM parameter adjustments appointed in the tuning process", True)
 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("disable_interaction_forces", bool_t, 0, "Description", False)
 
+group_param_eqn_mean = group_sfm.add_group("EquationParams", type="hide")
+group_param_eqn_mean.add("an", double_t, 0, "Default multiplier: -8.0", -8.0 * 0.2615, -300.0, +300.0)
+group_param_eqn_mean.add("bn", double_t, 0, "Default multiplier: +5.0", +5.0 * 0.4026, -300.0, +300.0)
+group_param_eqn_mean.add("cn", double_t, 0, "Default multiplier: +1.5", +1.5 * 2.1614, -300.0, +300.0)
+group_param_eqn_mean.add("ap", double_t, 0, "Default multiplier: +1.0", +1.0 * 1.5375, -300.0, +300.0)
+group_param_eqn_mean.add("bp", double_t, 0, "Default multiplier: +2.0", +2.0 * 0.4938, -300.0, +300.0)
+group_param_eqn_mean.add("cp", double_t, 0, "Default multiplier: +0.8", +0.8 * 0.5710, -300.0, +300.0)
+group_param_eqn_mean.add("aw", double_t, 0, "Default multiplier: +123.139", +123.139 * 0.3280, -300.0, +300.0)
+group_param_eqn_mean.add("bw", double_t, 0, "Default multiplier: +1.2", +1.2 * 0.1871, -300.0, +300.0)
+group_param_eqn_mean.add("speed_desired", double_t, 0, "Default: 1.29", 1.29, 0.0, 3.0)
+group_param_eqn_mean.add("relaxation_time", double_t, 0, "Default: 0.54", 0.54, 0.0, 3.0)
+
+group_param_eqn_stddev = group_sfm.add_group("EquationParamsStdDev", type="hide")
+group_param_eqn_stddev.add("an_stddev", double_t, 0, "", 0.0551, -300.0, +300.0)
+group_param_eqn_stddev.add("bn_stddev", double_t, 0, "", 0.1238, -300.0, +300.0)
+group_param_eqn_stddev.add("cn_stddev", double_t, 0, "", 0.3728, -300.0, +300.0)
+group_param_eqn_stddev.add("ap_stddev", double_t, 0, "", 0.3084, -300.0, +300.0)
+group_param_eqn_stddev.add("bp_stddev", double_t, 0, "", 0.1041, -300.0, +300.0)
+group_param_eqn_stddev.add("cp_stddev", double_t, 0, "", 0.1409, -300.0, +300.0)
+group_param_eqn_stddev.add("aw_stddev", double_t, 0, "", 0.1481, -300.0, +300.0)
+group_param_eqn_stddev.add("bw_stddev", double_t, 0, "", 0.0563, -300.0, +300.0)
+group_param_eqn_stddev.add("speed_desired_stddev", double_t, 0, "", 0.19, -300.0, +300.0)
+group_param_eqn_stddev.add("relaxation_time_stddev", double_t, 0, "", 0.05, -300.0, +300.0)
+
 # Fuzzy Inference System group parameters
 group_fis = gen.add_group("FIS", type="tab")
 
@@ -54,22 +77,42 @@ group_fis.add("as", double_t, 0, "Levelling factor that also directly affects am
 
 # Trajectory generation group parameters
 group_traj = gen.add_group("Trajectory", type="tab")
-group_generator_internal = group_traj.add_group("Internal", type="hide")
-group_generator_internal.add("force_internal_amplifier_min", double_t, 0, "Description", 0.12, 0.0, 10.0)
-group_generator_internal.add("force_internal_amplifier_max", double_t, 0, "Bigger value may provide higher speeds and may help with stucks at cluttered areas", 2.12, 0.0, 10.0)
-group_generator_internal.add("force_internal_amplifier_granularity", double_t, 0, "Distance between values (within min and max) that should be selected for sampling", 0.2, 0.0, 10.0)
-group_generator_dynamic = group_traj.add_group("DynamicObject", type="hide")
-group_generator_dynamic.add("force_interaction_dynamic_amplifier_min", double_t, 0, "Description", 0.25, 0.0, 10.0)
-group_generator_dynamic.add("force_interaction_dynamic_amplifier_max", double_t, 0, "Description", 0.25, 0.0, 10.0)
-group_generator_dynamic.add("force_interaction_dynamic_amplifier_granularity", double_t, 0, "Distance between values (within min and max) that should be selected for sampling", 1.0, 0.0, 10.0)
-group_generator_social = group_traj.add_group("HumanBehaviour", type="hide")
-group_generator_social.add("force_interaction_social_amplifier_min", double_t, 0, "Description", 0.0, 0.0, 10.0)
-group_generator_social.add("force_interaction_social_amplifier_max", double_t, 0, "Description", 2.0, 0.0, 10.0)
-group_generator_social.add("force_interaction_social_amplifier_granularity", double_t, 0, "Distance between values (within min and max) that should be selected for sampling", 1.0, 0.0, 10.0)
-group_generator_static = group_traj.add_group("StaticObject", type="hide")
-group_generator_static.add("force_interaction_static_amplifier_min", double_t, 0, "Description", 0.025, 0.0, 10.0)
-group_generator_static.add("force_interaction_static_amplifier_max", double_t, 0, "Description", 3.275, 0.0, 10.0)
-group_generator_static.add("force_interaction_static_amplifier_granularity", double_t, 0, "Distance between values (within min and max) that should be selected for sampling", 0.65, 0.0, 10.0)
+group_generator_an = group_traj.add_group("An", type="hide")
+group_generator_an.add("sfm_an_amplifier_min", double_t, 0, "", 1.0, -300.0, +300.0)
+group_generator_an.add("sfm_an_amplifier_max", double_t, 0, "", 1.0, -300.0, +300.0)
+group_generator_an.add("sfm_an_amplifier_granularity", double_t, 0, "", 1.0, 0.0, +300.0)
+group_generator_bn = group_traj.add_group("Bn", type="hide")
+group_generator_bn.add("sfm_bn_amplifier_min", double_t, 0, "", 1.0, -300.0, +300.0)
+group_generator_bn.add("sfm_bn_amplifier_max", double_t, 0, "", 1.0, -300.0, +300.0)
+group_generator_bn.add("sfm_bn_amplifier_granularity", double_t, 0, "", 1.0, 0.0, +300.0)
+group_generator_cn = group_traj.add_group("Cn", type="hide")
+group_generator_cn.add("sfm_cn_amplifier_min", double_t, 0, "", 1.0, -300.0, +300.0)
+group_generator_cn.add("sfm_cn_amplifier_max", double_t, 0, "", 1.0, -300.0, +300.0)
+group_generator_cn.add("sfm_cn_amplifier_granularity", double_t, 0, "", 1.0, 0.0, +300.0)
+group_generator_ap = group_traj.add_group("Ap", type="hide")
+group_generator_ap.add("sfm_ap_amplifier_min", double_t, 0, "", 1.0, -300.0, +300.0)
+group_generator_ap.add("sfm_ap_amplifier_max", double_t, 0, "", 1.0, -300.0, +300.0)
+group_generator_ap.add("sfm_ap_amplifier_granularity", double_t, 0, "", 1.0, 0.0, +300.0)
+group_generator_bp = group_traj.add_group("Bp", type="hide")
+group_generator_bp.add("sfm_bp_amplifier_min", double_t, 0, "", 1.0, -300.0, +300.0)
+group_generator_bp.add("sfm_bp_amplifier_max", double_t, 0, "", 1.0, -300.0, +300.0)
+group_generator_bp.add("sfm_bp_amplifier_granularity", double_t, 0, "", 1.0, 0.0, +300.0)
+group_generator_cp = group_traj.add_group("Cp", type="hide")
+group_generator_cp.add("sfm_cp_amplifier_min", double_t, 0, "", 1.0, -300.0, +300.0)
+group_generator_cp.add("sfm_cp_amplifier_max", double_t, 0, "", 1.0, -300.0, +300.0)
+group_generator_cp.add("sfm_cp_amplifier_granularity", double_t, 0, "", 1.0, 0.0, +300.0)
+group_generator_aw = group_traj.add_group("Aw", type="hide")
+group_generator_aw.add("sfm_aw_amplifier_min", double_t, 0, "", -1.0, -300.0, +300.0)
+group_generator_aw.add("sfm_aw_amplifier_max", double_t, 0, "", +1.0, -300.0, +300.0)
+group_generator_aw.add("sfm_aw_amplifier_granularity", double_t, 0, "", 0.25,0.0, +300.0)
+group_generator_bw = group_traj.add_group("Bw", type="hide")
+group_generator_bw.add("sfm_bw_amplifier_min", double_t, 0, "", 1.0, -300.0, +300.0)
+group_generator_bw.add("sfm_bw_amplifier_max", double_t, 0, "", 1.0, -300.0, +300.0)
+group_generator_bw.add("sfm_bw_amplifier_granularity", double_t, 0, "", 1.0, 0.0, +300.0)
+group_generator_as = group_traj.add_group("As", type="hide")
+group_generator_as.add("fis_as_amplifier_min", double_t, 0, "", 1.0, -300.0, +300.0)
+group_generator_as.add("fis_as_amplifier_max", double_t, 0, "", 1.0, -300.0, +300.0)
+group_generator_as.add("fis_as_amplifier_granularity", double_t, 0, "", 1.0, 0.0, +300.0)
 
 # Trajectory generation group parameters
 group_cost = gen.add_group("Cost", type="tab")

include/hubero_local_planner/hubero_config.h

@@ -62,6 +62,36 @@ namespace hubero_local_planner {
 		/// \brief Defines whether interaction forces should be calculated;
 		/// setting to False will force robot to take the shortest possible path.
 		bool disable_interaction_forces	= false;
+
+		/**
+		 * @defgroup eqnparams Equation Params
+		 * Group contains equation parameter value pairs: mean and standard deviation
+		 * @{
+		 */
+		// mean values (CUSTOM TUNING_MULTIPLIER * reference value from article)
+		double speed_desired = 1.29;
+		double relaxation_time = 0.54;
+		double an = -8.0 * 0.2615;
+		double bn = +5.0 * 0.4026;
+		double cn = +1.5 * 2.1614;
+		double ap = +1.0 * 1.5375;
+		double bp = +2.0 * 0.4938;
+		double cp = +0.8 * 0.5710;
+		double aw = +123.139 * 0.3280;
+		double bw = +1.2 * 0.1871;
+
+		// standard deviation values
+		double speed_desired_stddev = 0.19;
+		double relaxation_time_stddev = 0.05;
+		double an_stddev = 0.0551;
+		double bn_stddev = 0.1238;
+		double cn_stddev = 0.3728;
+		double ap_stddev = 0.3084;
+		double bp_stddev = 0.1041;
+		double cp_stddev = 0.1409;
+		double aw_stddev = 0.1481;
+		double bw_stddev = 0.0563;
+		/// @} // end of eqnparams group
 	};
 
 	/// \brief Declaration of a FuzzyInferenceSystem Params typedef'ed struct;
@@ -82,21 +112,41 @@ namespace hubero_local_planner {
 	 * given dimension)
 	 */
 	struct TrajectorySamplingParams {
-		double force_internal_amplifier_min = 0.5;
-		double force_internal_amplifier_max = 1.5;
-		double force_internal_amplifier_granularity = 0.5;
+		double sfm_an_amplifier_min = 1.0;
+		double sfm_an_amplifier_max = 1.0;
+		double sfm_an_amplifier_granularity = 1.0;
+
+		double sfm_bn_amplifier_min = 1.0;
+		double sfm_bn_amplifier_max = 1.0;
+		double sfm_bn_amplifier_granularity = 1.0;
+
+		double sfm_cn_amplifier_min = 1.0;
+		double sfm_cn_amplifier_max = 1.0;
+		double sfm_cn_amplifier_granularity = 1.0;
+
+		double sfm_ap_amplifier_min = 1.0;
+		double sfm_ap_amplifier_max = 1.0;
+		double sfm_ap_amplifier_granularity = 1.0;
+
+		double sfm_bp_amplifier_min = 1.0;
+		double sfm_bp_amplifier_max = 1.0;
+		double sfm_bp_amplifier_granularity = 1.0;
+
+		double sfm_cp_amplifier_min = 1.0;
+		double sfm_cp_amplifier_max = 1.0;
+		double sfm_cp_amplifier_granularity = 1.0;
 
-		double force_interaction_dynamic_amplifier_min = 0.5;
-		double force_interaction_dynamic_amplifier_max = 1.5;
-		double force_interaction_dynamic_amplifier_granularity = 0.5;
+		double sfm_aw_amplifier_min = -1.25;
+		double sfm_aw_amplifier_max = 1.25;
+		double sfm_aw_amplifier_granularity = 0.25;
 
-		double force_interaction_social_amplifier_min = 0.5;
-		double force_interaction_social_amplifier_max = 1.5;
-		double force_interaction_social_amplifier_granularity = 0.5;
+		double sfm_bw_amplifier_min = 1.0;
+		double sfm_bw_amplifier_max = 1.0;
+		double sfm_bw_amplifier_granularity = 1.0;
 
-		double force_interaction_static_amplifier_min = 0.5;
-		double force_interaction_static_amplifier_max = 1.5;
-		double force_interaction_static_amplifier_granularity = 0.5;
+		double fis_as_amplifier_min = 1.0;
+		double fis_as_amplifier_max = 1.0;
+		double fis_as_amplifier_granularity = 1.0;
 	};
 
 	struct CostParams {

include/hubero_local_planner/sfm/social_force_model.h

@@ -137,6 +137,32 @@ class SocialForceModel {
 	/// \brief Function which sets internal parameters according to loaded parameters
 	void init(std::shared_ptr<const hubero_local_planner::SfmParams> cfg);
 
+	/// \brief Updates internal parameters
+	void setEquationParameters(
+		double an,
+		double bn,
+		double cn,
+		double ap,
+		double bp,
+		double cp,
+		double aw,
+		double bw
+	);
+
+	/// \brief Updates internal parameter
+	void setEquationParameters(
+		double an,
+		double bn,
+		double cn,
+		double ap,
+		double bp,
+		double cp,
+		double aw,
+		double bw,
+		double speed_desired,
+		double relaxation_time
+	);
+
 	/// \brief Returns true if interaction forces were disabled in the provided config struct
 	bool areInteractionForcesDisabled() const;
 
@@ -169,6 +195,51 @@ class SocialForceModel {
 		return (force_combined_);
 	}
 
+	/**
+	 * @defgroup eqnparams Equation parameter getters
+	 * @{
+	 */
+	double getParameterAn() const {
+		return An_;
+	};
+
+	double getParameterBn() const {
+		return Bn_;
+	};
+
+	double getParameterCn() const {
+		return Cn_;
+	};
+
+	double getParameterAp() const {
+		return Ap_;
+	};
+
+	double getParameterBp() const {
+		return Bp_;
+	};
+
+	double getParameterCp() const {
+		return Cp_;
+	};
+
+	double getParameterAw() const {
+		return Aw_;
+	};
+
+	double getParameterBw() const {
+		return Bw_;
+	};
+
+	double getParameterDesiredSpeed() const {
+		return speed_desired_;
+	};
+
+	double getParameterRelaxationTime() const {
+		return relaxation_time_;
+	};
+	/// @} // end of eqnparams
+
 	/// \brief Default destructor
 	virtual ~SocialForceModel() = default;
 

include/hubero_local_planner/social_trajectory_generator.h

@@ -26,10 +26,27 @@ class SocialTrajectoryGenerator: public base_local_planner::TrajectorySampleGene
 public:
 	/// Helper struct to pass a custom amplifier set to @ref initialise
 	struct SampleAmplifierSet {
-		double force_internal_amplifier;
-		double force_interaction_dynamic_amplifier;
-		double force_interaction_social_amplifier;
-		double force_interaction_static_amplifier;
+		double sfm_an_amplifier;
+		double sfm_bn_amplifier;
+		double sfm_cn_amplifier;
+		double sfm_ap_amplifier;
+		double sfm_bp_amplifier;
+		double sfm_cp_amplifier;
+		double sfm_aw_amplifier;
+		double sfm_bw_amplifier;
+		double fis_as_amplifier;
+
+		/// Default ctor - unit amplifiers do not affect results
+		SampleAmplifierSet():
+			sfm_an_amplifier(1.0),
+			sfm_bn_amplifier(1.0),
+			sfm_cn_amplifier(1.0),
+			sfm_ap_amplifier(1.0),
+			sfm_bp_amplifier(1.0),
+			sfm_cp_amplifier(1.0),
+			sfm_aw_amplifier(1.0),
+			sfm_bw_amplifier(1.0),
+			fis_as_amplifier(1.0) {}
 	};
 
 	SocialTrajectoryGenerator();
@@ -249,6 +266,7 @@ class SocialTrajectoryGenerator: public base_local_planner::TrajectorySampleGene
 	void computeForces(
 		const World& world_model,
 		const double& dt,
+		const SampleAmplifierSet& sample_amplifiers,
 		geometry::Vector& force_internal,
 		geometry::Vector& force_interaction_dynamic,
 		geometry::Vector& force_interaction_static,

src/hubero_config_ros.cpp

@@ -95,10 +95,31 @@ void HuberoConfigROS::reconfigure(HuberoPlannerConfig& cfg) {
 	sfm_->min_force = cfg.min_force;
 	sfm_->max_force = cfg.max_force;
 	sfm_->heterogenous_population = cfg.heterogenous_population;
-	sfm_->use_tuned_params = cfg.use_tuned_params;
 	sfm_->static_obj_interaction = cfg.static_object_interaction_type;
 	sfm_->disable_interaction_forces = cfg.disable_interaction_forces;
 
+	sfm_->speed_desired = cfg.speed_desired;
+	sfm_->relaxation_time = cfg.relaxation_time;
+	sfm_->an = cfg.an;
+	sfm_->bn = cfg.bn;
+	sfm_->cn = cfg.cn;
+	sfm_->ap = cfg.ap;
+	sfm_->bp = cfg.bp;
+	sfm_->cp = cfg.cp;
+	sfm_->aw = cfg.aw;
+	sfm_->bw = cfg.bw;
+
+	sfm_->speed_desired_stddev = cfg.speed_desired_stddev;
+	sfm_->relaxation_time_stddev = cfg.relaxation_time_stddev;
+	sfm_->an_stddev = cfg.an_stddev;
+	sfm_->bn_stddev = cfg.bn_stddev;
+	sfm_->cn_stddev = cfg.cn_stddev;
+	sfm_->ap_stddev = cfg.ap_stddev;
+	sfm_->bp_stddev = cfg.bp_stddev;
+	sfm_->cp_stddev = cfg.cp_stddev;
+	sfm_->aw_stddev = cfg.aw_stddev;
+	sfm_->bw_stddev = cfg.bw_stddev;
+
 	fis_->force_factor = cfg.force_factor;
 	fis_->human_action_range = cfg.human_action_range;
 	fis_->as = cfg.as;
@@ -123,18 +144,33 @@ void HuberoConfigROS::reconfigure(HuberoPlannerConfig& cfg) {
 	limits_->yaw_goal_tolerance = cfg.yaw_goal_tolerance;
 	limits_->twist_rotation_compensation = cfg.twist_rotation_compensation;
 
-	traj_sampling_->force_internal_amplifier_min = cfg.force_internal_amplifier_min;
-	traj_sampling_->force_internal_amplifier_max = cfg.force_internal_amplifier_max;
-	traj_sampling_->force_internal_amplifier_granularity = cfg.force_internal_amplifier_granularity;
-	traj_sampling_->force_interaction_static_amplifier_min = cfg.force_interaction_static_amplifier_min;
-	traj_sampling_->force_interaction_static_amplifier_max = cfg.force_interaction_static_amplifier_max;
-	traj_sampling_->force_interaction_static_amplifier_granularity = cfg.force_interaction_static_amplifier_granularity;
-	traj_sampling_->force_interaction_dynamic_amplifier_min = cfg.force_interaction_dynamic_amplifier_min;
-	traj_sampling_->force_interaction_dynamic_amplifier_max = cfg.force_interaction_dynamic_amplifier_max;
-	traj_sampling_->force_interaction_dynamic_amplifier_granularity = cfg.force_interaction_dynamic_amplifier_granularity;
-	traj_sampling_->force_interaction_social_amplifier_min = cfg.force_interaction_social_amplifier_min;
-	traj_sampling_->force_interaction_social_amplifier_max = cfg.force_interaction_social_amplifier_max;
-	traj_sampling_->force_interaction_social_amplifier_granularity = cfg.force_interaction_social_amplifier_granularity;
+	traj_sampling_->sfm_an_amplifier_min = cfg.sfm_an_amplifier_min;
+	traj_sampling_->sfm_an_amplifier_max = cfg.sfm_an_amplifier_max;
+	traj_sampling_->sfm_an_amplifier_granularity = cfg.sfm_an_amplifier_granularity;
+	traj_sampling_->sfm_bn_amplifier_min = cfg.sfm_bn_amplifier_min;
+	traj_sampling_->sfm_bn_amplifier_max = cfg.sfm_bn_amplifier_max;
+	traj_sampling_->sfm_bn_amplifier_granularity = cfg.sfm_bn_amplifier_granularity;
+	traj_sampling_->sfm_cn_amplifier_min = cfg.sfm_cn_amplifier_min;
+	traj_sampling_->sfm_cn_amplifier_max = cfg.sfm_cn_amplifier_max;
+	traj_sampling_->sfm_cn_amplifier_granularity = cfg.sfm_cn_amplifier_granularity;
+	traj_sampling_->sfm_ap_amplifier_min = cfg.sfm_ap_amplifier_min;
+	traj_sampling_->sfm_ap_amplifier_max = cfg.sfm_ap_amplifier_max;
+	traj_sampling_->sfm_ap_amplifier_granularity = cfg.sfm_ap_amplifier_granularity;
+	traj_sampling_->sfm_bp_amplifier_min = cfg.sfm_bp_amplifier_min;
+	traj_sampling_->sfm_bp_amplifier_max = cfg.sfm_bp_amplifier_max;
+	traj_sampling_->sfm_bp_amplifier_granularity = cfg.sfm_bp_amplifier_granularity;
+	traj_sampling_->sfm_cp_amplifier_min = cfg.sfm_cp_amplifier_min;
+	traj_sampling_->sfm_cp_amplifier_max = cfg.sfm_cp_amplifier_max;
+	traj_sampling_->sfm_cp_amplifier_granularity = cfg.sfm_cp_amplifier_granularity;
+	traj_sampling_->sfm_aw_amplifier_min = cfg.sfm_aw_amplifier_min;
+	traj_sampling_->sfm_aw_amplifier_max = cfg.sfm_aw_amplifier_max;
+	traj_sampling_->sfm_aw_amplifier_granularity = cfg.sfm_aw_amplifier_granularity;
+	traj_sampling_->sfm_bw_amplifier_min = cfg.sfm_bw_amplifier_min;
+	traj_sampling_->sfm_bw_amplifier_max = cfg.sfm_bw_amplifier_max;
+	traj_sampling_->sfm_bw_amplifier_granularity = cfg.sfm_bw_amplifier_granularity;
+	traj_sampling_->fis_as_amplifier_min = cfg.fis_as_amplifier_min;
+	traj_sampling_->fis_as_amplifier_max = cfg.fis_as_amplifier_max;
+	traj_sampling_->fis_as_amplifier_granularity = cfg.fis_as_amplifier_granularity;
 
 	costs_->path_distance_scale = cfg.path_distance_scale;
 	costs_->goal_distance_scale = cfg.goal_distance_scale;