diff --git a/cfg/HuberoPlanner.cfg b/cfg/HuberoPlanner.cfg index 27b947c6..66fe1ad8 100755 --- a/cfg/HuberoPlanner.cfg +++ b/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") diff --git a/include/hubero_local_planner/hubero_config.h b/include/hubero_local_planner/hubero_config.h index a01cd5e1..ee046de3 100644 --- a/include/hubero_local_planner/hubero_config.h +++ b/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 { diff --git a/include/hubero_local_planner/sfm/social_force_model.h b/include/hubero_local_planner/sfm/social_force_model.h index 08d73f54..ca981248 100644 --- a/include/hubero_local_planner/sfm/social_force_model.h +++ b/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 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; diff --git a/include/hubero_local_planner/social_trajectory_generator.h b/include/hubero_local_planner/social_trajectory_generator.h index 125eeb64..f024eed6 100644 --- a/include/hubero_local_planner/social_trajectory_generator.h +++ b/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, diff --git a/src/hubero_config_ros.cpp b/src/hubero_config_ros.cpp index e011a64e..5a2ad305 100644 --- a/src/hubero_config_ros.cpp +++ b/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; diff --git a/src/sfm/social_force_model.cpp b/src/sfm/social_force_model.cpp index bb423360..60e46d62 100644 --- a/src/sfm/social_force_model.cpp +++ b/src/sfm/social_force_model.cpp @@ -68,6 +68,43 @@ void SocialForceModel::init(std::shared_ptr dist_spd_desired(1.29f, 0.19f); + std::normal_distribution dist_spd_desired(cfg_->speed_desired, cfg_->speed_desired_stddev); speed_desired_ = dist_spd_desired(rand_gen); // relaxation time (based on (Moussaid et al. (2009)) - std::normal_distribution dist_tau(0.54f, 0.05f); + std::normal_distribution dist_tau(cfg_->relaxation_time, cfg_->relaxation_time_stddev); relaxation_time_ = dist_tau(rand_gen); // ----------------------------- Model C ------------------------------------------------------ // // Rudloff et al. (2011) model's parameters based on S. Seer et al. (2014) // Generate random value of mean a and standard deviation b - std::normal_distribution dist_an(0.2615f, 0.0551f); + std::normal_distribution dist_an(cfg_->an, cfg_->an_stddev); An_ = dist_an(rand_gen); - std::normal_distribution dist_bn(0.4026f, 0.1238f); + std::normal_distribution dist_bn(cfg_->bn, cfg_->bn_stddev); Bn_ = dist_bn(rand_gen); - std::normal_distribution dist_cn(2.1614f, 0.3728f); + std::normal_distribution dist_cn(cfg_->cn, cfg_->cn_stddev); Cn_ = dist_cn(rand_gen); - std::normal_distribution dist_ap(1.5375f, 0.3084f); + std::normal_distribution dist_ap(cfg_->ap, cfg_->ap_stddev); Ap_ = dist_ap(rand_gen); - std::normal_distribution dist_bp(0.4938f, 0.1041f); + std::normal_distribution dist_bp(cfg_->bp, cfg_->bp_stddev); Bp_ = dist_bp(rand_gen); - std::normal_distribution dist_cp(0.5710f, 0.1409f); + std::normal_distribution dist_cp(cfg_->cp, cfg_->cp_stddev); Cp_ = dist_cp(rand_gen); - std::normal_distribution dist_aw(0.3280f, 0.1481f); + std::normal_distribution dist_aw(cfg_->aw, cfg_->aw_stddev); Aw_ = dist_aw(rand_gen); - std::normal_distribution dist_bw(0.1871f, 0.0563f); + std::normal_distribution dist_bw(cfg_->bw, cfg_->bw_stddev); Bw_ = dist_bw(rand_gen); printf( "SFM parameters computed in a non-deterministic way. Values are as follows:\r\n" @@ -209,33 +246,20 @@ void SocialForceModel::setParameters() { Aw_, Bw_ ); - } else { - // homogenous (deterministic) mode selected - speed_desired_ = 1.29f; - relaxation_time_ = 0.54f; - An_ = 0.2615f; - Bn_ = 0.4026f; - Cn_ = 2.1614f; - Ap_ = 1.5375f; - Bp_ = 0.4938f; - Cp_ = 0.5710f; - Aw_ = 0.3280f; - Bw_ = 0.1871f; - } - - if (!cfg_->use_tuned_params) { return; } - // See article description for details - An_ *= -8.0; - Bn_ *= +5.0; - Cn_ *= +1.5; - // Ap_ does not require changes - Bp_ *= +2.0; - Cp_ *= +0.8; - Aw_ *= +123.139; - Bw_ *= +1.2; + // homogenous (deterministic) mode selected + speed_desired_ = cfg_->speed_desired; + relaxation_time_ = cfg_->relaxation_time; + An_ = cfg_->an; + Bn_ = cfg_->bn; + Cn_ = cfg_->cn; + Ap_ = cfg_->ap; + Bp_ = cfg_->bp; + Cp_ = cfg_->cp; + Aw_ = cfg_->aw; + Bw_ = cfg_->bw; } // ------------------------------------------------------------------- // diff --git a/src/social_trajectory_generator.cpp b/src/social_trajectory_generator.cpp index a3db72f9..89fe72a6 100644 --- a/src/social_trajectory_generator.cpp +++ b/src/social_trajectory_generator.cpp @@ -88,59 +88,117 @@ void SocialTrajectoryGenerator::initialise( sample_amplifier_params_v_.clear(); // prepare all amplifier values to be evaluated later - auto force_internal_amps = computeAmplifierSamples( - limits_amplifiers_.force_internal_amplifier_min, - limits_amplifiers_.force_internal_amplifier_max, - limits_amplifiers_.force_internal_amplifier_granularity, - "internal force" + auto sfm_an_amps = computeAmplifierSamples( + limits_amplifiers_.sfm_an_amplifier_min, + limits_amplifiers_.sfm_an_amplifier_max, + limits_amplifiers_.sfm_an_amplifier_granularity, + "An parameter of the SFM (dyn. objects)" ); - auto force_interaction_static_amps = computeAmplifierSamples( - limits_amplifiers_.force_interaction_static_amplifier_min, - limits_amplifiers_.force_interaction_static_amplifier_max, - limits_amplifiers_.force_interaction_static_amplifier_granularity, - "static forces" + auto sfm_bn_amps = computeAmplifierSamples( + limits_amplifiers_.sfm_bn_amplifier_min, + limits_amplifiers_.sfm_bn_amplifier_max, + limits_amplifiers_.sfm_bn_amplifier_granularity, + "Bn parameter of the SFM (dyn. objects)" ); - auto force_interaction_dynamic_amps = computeAmplifierSamples( - limits_amplifiers_.force_interaction_dynamic_amplifier_min, - limits_amplifiers_.force_interaction_dynamic_amplifier_max, - limits_amplifiers_.force_interaction_dynamic_amplifier_granularity, - "dynamic forces" + auto sfm_cn_amps = computeAmplifierSamples( + limits_amplifiers_.sfm_cn_amplifier_min, + limits_amplifiers_.sfm_cn_amplifier_max, + limits_amplifiers_.sfm_cn_amplifier_granularity, + "Cn parameter of the SFM (dyn. objects)" ); - auto force_interaction_social_amps = computeAmplifierSamples( - limits_amplifiers_.force_interaction_social_amplifier_min, - limits_amplifiers_.force_interaction_social_amplifier_max, - limits_amplifiers_.force_interaction_social_amplifier_granularity, - "social forces" + auto sfm_ap_amps = computeAmplifierSamples( + limits_amplifiers_.sfm_ap_amplifier_min, + limits_amplifiers_.sfm_ap_amplifier_max, + limits_amplifiers_.sfm_ap_amplifier_granularity, + "Ap parameter of the SFM (dyn. objects)" + ); + + auto sfm_bp_amps = computeAmplifierSamples( + limits_amplifiers_.sfm_bp_amplifier_min, + limits_amplifiers_.sfm_bp_amplifier_max, + limits_amplifiers_.sfm_bp_amplifier_granularity, + "Bp parameter of the SFM (dyn. objects)" + ); + + auto sfm_cp_amps = computeAmplifierSamples( + limits_amplifiers_.sfm_cp_amplifier_min, + limits_amplifiers_.sfm_cp_amplifier_max, + limits_amplifiers_.sfm_cp_amplifier_granularity, + "Cp parameter of the SFM (dyn. objects)" + ); + + auto sfm_aw_amps = computeAmplifierSamples( + limits_amplifiers_.sfm_aw_amplifier_min, + limits_amplifiers_.sfm_aw_amplifier_max, + limits_amplifiers_.sfm_aw_amplifier_granularity, + "Aw parameter of the SFM (stat. objects)" + ); + + auto sfm_bw_amps = computeAmplifierSamples( + limits_amplifiers_.sfm_bw_amplifier_min, + limits_amplifiers_.sfm_bw_amplifier_max, + limits_amplifiers_.sfm_bw_amplifier_granularity, + "Bw parameter of the SFM (stat. objects)" + ); + + auto fis_as_amps = computeAmplifierSamples( + limits_amplifiers_.fis_as_amplifier_min, + limits_amplifiers_.fis_as_amplifier_max, + limits_amplifiers_.fis_as_amplifier_granularity, + "As parameter of the FIS (people)" ); // if using continuous acceleration, trim velocities so they surely not exceed limits computeVelocityLimitsWithCA(world_model_, vels_min_, vels_max_); // prepare vector of amplifiers to investigate - for (const auto& amp_internal: force_internal_amps) { - for (const auto& amp_static: force_interaction_static_amps) { - for (const auto& amp_dynamic: force_interaction_dynamic_amps) { - for (const auto& amp_social: force_interaction_social_amps) { - SampleAmplifierSet amp_set {}; - amp_set.force_internal_amplifier = amp_internal; - amp_set.force_interaction_static_amplifier = amp_static; - amp_set.force_interaction_dynamic_amplifier = amp_dynamic; - amp_set.force_interaction_social_amplifier = amp_social; - sample_amplifier_params_v_.push_back(amp_set); - - ROS_INFO_COND_NAMED( - log_generation_samples_, - "SocTrajGen", - "Sample %3lu - internal %2.5f, static %2.5f, dynamic %2.5f, social %2.5f", - sample_amplifier_params_v_.size(), - amp_set.force_internal_amplifier, - amp_set.force_interaction_static_amplifier, - amp_set.force_interaction_dynamic_amplifier, - amp_set.force_interaction_social_amplifier - ); + for (const auto& an: sfm_an_amps) { + for (const auto& bn: sfm_bn_amps) { + for (const auto& cn: sfm_cn_amps) { + for (const auto& ap: sfm_ap_amps) { + for (const auto& bp: sfm_bp_amps) { + for (const auto& cp: sfm_cp_amps) { + for (const auto& aw: sfm_aw_amps) { + for (const auto& bw: sfm_bw_amps) { + for (const auto& as: fis_as_amps) { + SampleAmplifierSet amp_set {}; + amp_set.sfm_an_amplifier = an; + amp_set.sfm_bn_amplifier = bn; + amp_set.sfm_cn_amplifier = cn; + amp_set.sfm_ap_amplifier = ap; + amp_set.sfm_bp_amplifier = bp; + amp_set.sfm_cp_amplifier = cp; + amp_set.sfm_aw_amplifier = aw; + amp_set.sfm_bw_amplifier = bw; + amp_set.fis_as_amplifier = as; + sample_amplifier_params_v_.push_back(amp_set); + + ROS_INFO_COND_NAMED( + log_generation_samples_, + "SocTrajGen", + "Sample %3lu - An %2.5f, Bn %2.5f, Cn %2.5f, " + "Ap %2.5f, Bp %2.5f, Cp %2.5f, " + "Aw %2.5f, Bw %2.5f, " + "As %2.5f", + sample_amplifier_params_v_.size(), + amp_set.sfm_an_amplifier, + amp_set.sfm_bn_amplifier, + amp_set.sfm_cn_amplifier, + amp_set.sfm_ap_amplifier, + amp_set.sfm_bp_amplifier, + amp_set.sfm_cp_amplifier, + amp_set.sfm_aw_amplifier, + amp_set.sfm_bw_amplifier, + amp_set.fis_as_amplifier + ); + } + } + } + } + } } } } @@ -149,12 +207,18 @@ void SocialTrajectoryGenerator::initialise( ROS_INFO_COND_NAMED( log_generation_samples_, "SocTrajGen", - "Initialized %lu parameter samples (|p_i0| %lu, |p_ij| %lu, |p_is| %lu, |p_ik| %lu)", + "Initialized %lu parameter samples " + "(|An| %lu, |Bn| %lu, |Cn| %lu, |Ap| %lu, |Bp| %lu, |Cp| %lu, |Aw| %lu, |Bw| %lu, |As| %lu)", sample_amplifier_params_v_.size(), - force_internal_amps.size(), - force_interaction_static_amps.size(), - force_interaction_dynamic_amps.size(), - force_interaction_social_amps.size() + sfm_an_amps.size(), + sfm_bn_amps.size(), + sfm_cn_amps.size(), + sfm_ap_amps.size(), + sfm_bp_amps.size(), + sfm_cp_amps.size(), + sfm_aw_amps.size(), + sfm_bw_amps.size(), + fis_as_amps.size() ); } @@ -198,11 +262,17 @@ bool SocialTrajectoryGenerator::generateTrajectory( ROS_INFO_COND_NAMED( log_generation_details_, "SocTrajGen", - "Trajectory generation with: {p_i0: %3.5f, p_ij: %3.5f, p_is: %3.5f, p_ik: %3.5f}", - sample_amplifiers.force_internal_amplifier, - sample_amplifiers.force_interaction_dynamic_amplifier, - sample_amplifiers.force_interaction_social_amplifier, - sample_amplifiers.force_interaction_static_amplifier + "Trajectory generation with: " + "{An: %3.5f, Bn: %3.5f, Cn: %3.5f, Ap: %3.5f, Bp: %3.5f, Cp: %3.5f, Aw: %3.5f, Bw: %3.5f, As: %3.5f}", + sample_amplifiers.sfm_an_amplifier, + sample_amplifiers.sfm_bn_amplifier, + sample_amplifiers.sfm_cn_amplifier, + sample_amplifiers.sfm_ap_amplifier, + sample_amplifiers.sfm_bp_amplifier, + sample_amplifiers.sfm_cp_amplifier, + sample_amplifiers.sfm_aw_amplifier, + sample_amplifiers.sfm_bw_amplifier, + sample_amplifiers.fis_as_amplifier ); // speed i.e. velocity magnitude @@ -232,6 +302,7 @@ bool SocialTrajectoryGenerator::generateTrajectory( computeForces( world_model_plan, dt, + sample_amplifiers, force_internal, force_interaction_dynamic, force_interaction_static, @@ -239,12 +310,6 @@ bool SocialTrajectoryGenerator::generateTrajectory( i == 0 // motion data updated only at first iteration ); - // multiply SFM and Fuzzy Inference outputs - force_internal *= sample_amplifiers.force_internal_amplifier; - force_interaction_dynamic *= sample_amplifiers.force_interaction_dynamic_amplifier; - force_interaction_static *= sample_amplifiers.force_interaction_static_amplifier; - force_human_action *= sample_amplifiers.force_interaction_social_amplifier; - // force vectors are already multiplied by proper factors geometry::Vector twist_cmd; computeTwist( @@ -383,6 +448,7 @@ bool SocialTrajectoryGenerator::generateTrajectoryWithoutPlanning(base_local_pla computeForces( world_model_, dt, + SampleAmplifierSet(), force_internal, force_interaction_dynamic, force_interaction_static, @@ -503,6 +569,7 @@ int SocialTrajectoryGenerator::computeStepsNumber(const double& speed_linear, co void SocialTrajectoryGenerator::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, @@ -515,6 +582,31 @@ void SocialTrajectoryGenerator::computeForces( std::vector meaningful_interaction_static; std::vector meaningful_interaction_dynamic; + // backup SFM internal parameters, change them and restore after calculations + double sfm_param_an = sfm_.getParameterAn(); + double sfm_param_bn = sfm_.getParameterBn(); + double sfm_param_cn = sfm_.getParameterCn(); + double sfm_param_ap = sfm_.getParameterAp(); + double sfm_param_bp = sfm_.getParameterBp(); + double sfm_param_cp = sfm_.getParameterCp(); + double sfm_param_aw = sfm_.getParameterAw(); + double sfm_param_bw = sfm_.getParameterBw(); + sfm_.setEquationParameters( + sfm_param_an * sample_amplifiers.sfm_an_amplifier, + sfm_param_bn * sample_amplifiers.sfm_bn_amplifier, + sfm_param_cn * sample_amplifiers.sfm_cn_amplifier, + sfm_param_ap * sample_amplifiers.sfm_ap_amplifier, + sfm_param_bp * sample_amplifiers.sfm_bp_amplifier, + sfm_param_cp * sample_amplifiers.sfm_cp_amplifier, + sfm_param_aw * sample_amplifiers.sfm_aw_amplifier, + sfm_param_bw * sample_amplifiers.sfm_bw_amplifier + ); + + // same with FIS + double fis_param_as = social_conductor_.getParameterAs(); + social_conductor_.setEquationParameters(sample_amplifiers.fis_as_amplifier); + + // begin calculations sfm_.computeSocialForce( world_model, dt, @@ -576,6 +668,19 @@ void SocialTrajectoryGenerator::computeForces( force_interaction_static = sfm_.getForceInteractionStatic(); force_human_action = social_conductor_.getSocialVector(); + // restore initial values of params + sfm_.setEquationParameters( + sfm_param_an, + sfm_param_bn, + sfm_param_cn, + sfm_param_ap, + sfm_param_bp, + sfm_param_cp, + sfm_param_aw, + sfm_param_bw + ); + social_conductor_.setEquationParameters(fis_param_as); + if (update_motion_data) { diag_force_internal_ = sfm_.getForceInternal(); diag_force_interaction_dynamic_ = sfm_.getForceInteractionDynamic();