Skip to content
Snippets Groups Projects
Commit 4db8fdb7 authored by Tobias Gröger's avatar Tobias Gröger
Browse files

Merge branch 'feature/teb_config_as_json' into 'dev' 

Feature/teb config as json

See merge request ArmarX/skills/navigation!39
parents 6069a50c 81972ba7
No related branches found
No related tags found
2 merge requests!39Feature/teb config as json,!28Draft: Dev -> Main
Hide whitespace changes
Inline Side-by-side
Showing
with 683 additions and 51 deletions
data/armarx_navigation/local_planner_config/TimedElasticBands/default.json 0 → 100644
+ 115
− 0
View file @ 4db8fdb7
{
"robot_footprint_radius": 0.5,
"planning_distance": 3,
"teb_config": {
"pse": {
"pse_costum_obstacle_penalties": true,
"pse_costum_obstacle_penalties_dynamic": true,
"weight_global_path": 0.3,
"lrk_use_alternative_approach": false,
"lrk_enable_collision_check": true,
"hybrid_homotopy_calculation": true,
"robot_diff_circumscribed_inscribed_radius": 0.0
},
"trajectory": {
"teb_autosize": true,
"dt_ref": 0.3,
"dt_hysteresis": 0.1,
"min_samples": 3,
"max_samples": 500,
"global_plan_overwrite_orientation": true,
"allow_init_with_backwards_motion": false,
"exact_arc_length": false,
"force_reinit_new_goal_dist": 1,
"force_reinit_new_goal_angular": 1.5707963267948966,
"feasibility_check_no_poses": 5,
"feasibility_check_lookahead_distance": -1,
"min_resolution_collision_check_angular": 3.141592653589793
},
"robot": {
"max_vel_x": 0.5,
"max_vel_x_backwards": 0.5,
"max_vel_y": 0.5,
"max_vel_trans": 0.5,
"max_vel_theta": 0.5,
"acc_lim_x": 0.5,
"acc_lim_y": 0.5,
"acc_lim_theta": 0.5,
"min_turning_radius": 0
},
"goal_tolerance": {
"xy_goal_tolerance": 0.2
},
"obstacles": {
"min_obstacle_dist": 0.3,
"inflation_dist": 0.0,
"dynamic_obstacle_inflation_dist": 0.6,
"include_dynamic_obstacles": true,
"obstacle_association_force_inclusion_factor": 1.5,
"obstacle_association_cutoff_factor": 5,
"obstacle_proximity_ratio_max_vel": 1,
"obstacle_proximity_lower_bound": 0,
"obstacle_proximity_upper_bound": 0.5
},
"optim": {
"no_inner_iterations": 5,
"no_outer_iterations": 4,
"optimization_activate": true,
"optimization_verbose": false,
"penalty_epsilon": 0.05,
"weight_max_vel_x": 2,
"weight_max_vel_y": 2,
"weight_max_vel_theta": 1,
"weight_acc_lim_x": 1,
"weight_acc_lim_y": 1,
"weight_acc_lim_theta": 1,
"weight_kinematics_nh": 1000,
"weight_kinematics_forward_drive": 1,
"weight_kinematics_turning_radius": 1,
"weight_optimaltime": 1,
"weight_shortest_path": 1,
"weight_obstacle": 50,
"weight_inflation": 0.1,
"weight_dynamic_obstacle": 50,
"weight_dynamic_obstacle_inflation": 0.1,
"weight_velocity_obstacle_ratio": 0,
"weight_viapoint": 1,
"weight_prefer_rotdir": 50,
"weight_adapt_factor": 2.0,
"obstacle_cost_exponent": 1.0
},
"hcp": {
"enable_multithreading": true,
"simple_exploration": false,
"max_number_classes": 5,
"max_number_plans_in_current_class": 0,
"selection_cost_hysteresis": 1.0,
"selection_obst_cost_scale": 100.0,
"selection_viapoint_cost_scale": 1.0,
"selection_alternative_time_cost": false,
"selection_dropping_probability": 0.0,
"switching_blocking_period": 0.0,
"obstacle_heading_threshold": 0.45,
"roadmap_graph_no_samples": 20,
"roadmap_graph_area_width": 10,
"roadmap_graph_area_length_scale": 1.0,
"h_signature_prescaler": 1,
"h_signature_threshold": 0.1,
"viapoints_all_candidates": true,
"delete_detours_backwards": true,
"detours_orientation_tolerance": 1.5707963267948966,
"length_start_orientation_vector": 0.4,
"max_ratio_detours_duration_best_duration": 3.0
}
}
}
source/armarx/navigation/local_planning/TimedElasticBands.cpp
+ 37
− 44
View file @ 4db8fdb7
#include "TimedElasticBands.h" #include "TimedElasticBands.h"
#include <SimoxUtility/json/json.hpp>
#include <VirtualRobot/MathTools.h> #include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Robot.h> #include <VirtualRobot/Robot.h>
#include <VirtualRobot/SceneObjectSet.h> #include <VirtualRobot/SceneObjectSet.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/logging/Logging.h> #include <ArmarXCore/core/logging/Logging.h>
#include "RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h"
#include "armarx/navigation/conversions/eigen.h" #include "armarx/navigation/conversions/eigen.h"
#include "armarx/navigation/core/Trajectory.h" #include "armarx/navigation/core/Trajectory.h"
#include <armarx/navigation/local_planning/LocalPlanner.h> #include <armarx/navigation/local_planning/LocalPlanner.h>
...@@ -30,23 +34,16 @@ namespace armarx::navigation::local_planning ...@@ -30,23 +34,16 @@ namespace armarx::navigation::local_planning
aron::data::DictPtr aron::data::DictPtr
TimedElasticBandsParams::toAron() const TimedElasticBandsParams::toAron() const
{ {
arondto::TimedElasticBandsParams dto; return cfg.toAron();
TimedElasticBandsParams bo;
armarx::navigation::local_planning::toAron(dto, bo);
return dto.toAron();
} }
TimedElasticBandsParams TimedElasticBandsParams
TimedElasticBandsParams::FromAron(const aron::data::DictPtr& dict) TimedElasticBandsParams::FromAron(const aron::data::DictPtr& dict)
{ {
ARMARX_CHECK_NOT_NULL(dict); ARMARX_CHECK_NOT_NULL(dict);
arondto::TimedElasticBandsParams dto;
dto.fromAron(dict);
TimedElasticBandsParams bo; TimedElasticBandsParams bo;
fromAron(dto, bo); bo.cfg.fromAron(dict);
return bo; return bo;
} }
...@@ -54,12 +51,18 @@ namespace armarx::navigation::local_planning ...@@ -54,12 +51,18 @@ namespace armarx::navigation::local_planning
// TimedElasticBands // TimedElasticBands
TimedElasticBands::TimedElasticBands(const Params& params, const core::Scene& ctx) : TimedElasticBands::TimedElasticBands(const Params& i_params, const core::Scene& ctx) :
LocalPlanner(ctx), params(params), scene(ctx), obstManager(teb_obstacles) LocalPlanner(ctx), params(i_params), scene(ctx), obstManager(teb_obstacles)
{ {
//TODO (SALt): init configuration, robot footprint //TODO (SALt): find proper place to init with default config
initConfig(); if (true)
auto robot_model = boost::make_shared<teb_local_planner::CircularRobotFootprint>(0.2); {
readDefaultConfig(params.cfg);
}
toTebCfg(params.cfg, cfg_);
auto robot_model = boost::make_shared<teb_local_planner::CircularRobotFootprint>(
params.cfg.robot_footprint_radius);
hcp_ = std::make_unique<teb_local_planner::HomotopyClassPlanner>(); hcp_ = std::make_unique<teb_local_planner::HomotopyClassPlanner>();
hcp_->initialize( hcp_->initialize(
...@@ -69,32 +72,24 @@ namespace armarx::navigation::local_planning ...@@ -69,32 +72,24 @@ namespace armarx::navigation::local_planning
} }
void void
TimedElasticBands::initConfig() TimedElasticBands::readDefaultConfig(arondto::TimedElasticBandsParams& target)
{ {
cfg_.robot.max_vel_x = 1.0; const armarx::PackagePath configPath("armarx_navigation",
cfg_.robot.max_vel_x_backwards = 1.0; "local_planner_config/TimedElasticBands/default.json");
cfg_.robot.max_vel_y = 1.0; const std::filesystem::path file = configPath.toSystemPath();
cfg_.robot.max_vel_theta = 1.0;
cfg_.robot.acc_lim_x = 1.0; ARMARX_CHECK(std::filesystem::is_regular_file(file)) << file;
cfg_.robot.acc_lim_y = 1.0;
cfg_.robot.acc_lim_theta = 1.0; ARMARX_INFO << "Loading config from file `" << file << "`.";
std::ifstream ifs{file};
cfg_.optim.weight_max_vel_x = 1.0;
cfg_.optim.weight_max_vel_y = 1.0; nlohmann::json jsonConfig;
cfg_.optim.weight_max_vel_theta = 1.0; ifs >> jsonConfig;
cfg_.optim.weight_acc_lim_x = 1.0;
cfg_.optim.weight_acc_lim_y = 1.0; ARMARX_INFO << "Reading config";
cfg_.optim.weight_acc_lim_theta = 1.0;
armarx::aron::data::reader::NlohmannJSONReaderWithoutTypeCheck reader;
cfg_.optim.weight_optimaltime = 0.6; target.read(reader, jsonConfig);
cfg_.optim.weight_shortest_path = 0.6;
cfg_.hcp.selection_cost_hysteresis = 1.0;
cfg_.hcp.switching_blocking_period = 0;
cfg_.pse.weight_global_path = 0.6;
cfg_.trajectory.global_plan_overwrite_orientation = true;
} }
std::optional<LocalPlannerResult> std::optional<LocalPlannerResult>
...@@ -102,10 +97,9 @@ namespace armarx::navigation::local_planning ...@@ -102,10 +97,9 @@ namespace armarx::navigation::local_planning
{ {
const core::Pose currentPose{scene.robot->getGlobalPose()}; const core::Pose currentPose{scene.robot->getGlobalPose()};
// TODO (SALt): put distance parameter into config (note: distance is in mm)
// prune global trajectory // prune global trajectory
const auto& [prunedGoal, planToDest] = const auto& [prunedGoal, planToDest] = goal.getSubTrajectory(
goal.getSubTrajectory(currentPose.translation(), 2000); currentPose.translation(), params.cfg.planning_distance * 1000); // [m] to [mm]
const teb_local_planner::TimedElasticBand globalPath = conv::toRos(prunedGoal); const teb_local_planner::TimedElasticBand globalPath = conv::toRos(prunedGoal);
teb_globalPath = globalPath.poses(); teb_globalPath = globalPath.poses();
...@@ -122,7 +116,6 @@ namespace armarx::navigation::local_planning ...@@ -122,7 +116,6 @@ namespace armarx::navigation::local_planning
try try
{ {
// TODO (SALt): free goal velocity is not respected inside homotopy planner
hcp_->plan(start, end, &velocity_start, !planToDest); hcp_->plan(start, end, &velocity_start, !planToDest);
} }
catch (std::exception& e) catch (std::exception& e)
...@@ -146,7 +139,7 @@ namespace armarx::navigation::local_planning ...@@ -146,7 +139,7 @@ namespace armarx::navigation::local_planning
if (scene.staticScene.has_value()) if (scene.staticScene.has_value())
{ {
for (auto& obst : scene.staticScene.value().objects->getCollisionModels()) for (const auto& obst : scene.staticScene.value().objects->getCollisionModels())
{ {
obstManager.addBoxObstacle(obst->getGlobalBoundingBox()); obstManager.addBoxObstacle(obst->getGlobalBoundingBox());
} }
......
source/armarx/navigation/local_planning/TimedElasticBands.h
+ 4
− 4
View file @ 4db8fdb7
...@@ -28,6 +28,7 @@ ...@@ -28,6 +28,7 @@
#include <armarx/navigation/core/Trajectory.h> #include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/local_planning/LocalPlanner.h> #include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/local_planning/TebObstacleManager.h> #include <armarx/navigation/local_planning/TebObstacleManager.h>
#include <armarx/navigation/local_planning/aron/TimedElasticBands.aron.generated.h>
#include <armarx/navigation/local_planning/core.h> #include <armarx/navigation/local_planning/core.h>
#include <teb_local_planner/homotopy_class_planner.h> #include <teb_local_planner/homotopy_class_planner.h>
#include <teb_local_planner/teb_config.h> #include <teb_local_planner/teb_config.h>
...@@ -38,8 +39,7 @@ namespace armarx::navigation::local_planning ...@@ -38,8 +39,7 @@ namespace armarx::navigation::local_planning
struct TimedElasticBandsParams : public LocalPlannerParams struct TimedElasticBandsParams : public LocalPlannerParams
{ {
bool includeStartPose{false}; arondto::TimedElasticBandsParams cfg;
Algorithms algorithm() const override; Algorithms algorithm() const override;
aron::data::DictPtr toAron() const override; aron::data::DictPtr toAron() const override;
...@@ -59,11 +59,11 @@ namespace armarx::navigation::local_planning ...@@ -59,11 +59,11 @@ namespace armarx::navigation::local_planning
std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) override; std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) override;
private: private:
void initConfig(); void readDefaultConfig(arondto::TimedElasticBandsParams& target);
void fillObstacles(); void fillObstacles();
protected: protected:
const Params params; Params params;
private: private:
const core::Scene& scene; const core::Scene& scene;
......
source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
+ 391
− 1
View file @ 4db8fdb7
...@@ -5,10 +5,400 @@ ...@@ -5,10 +5,400 @@
</AronIncludes> </AronIncludes>
<GenerateTypes> <GenerateTypes>
<Object name='armarx::navigation::local_planning::arondto::pse'>
<ObjectChild key='pse_costum_obstacle_penalties'>
<bool />
</ObjectChild>
<ObjectChild key='pse_costum_obstacle_penalties_dynamic'>
<bool />
</ObjectChild>
<ObjectChild key='weight_global_path'>
<float />
</ObjectChild>
<ObjectChild key='lrk_use_alternative_approach'>
<bool />
</ObjectChild>
<ObjectChild key='lrk_enable_collision_check'>
<bool />
</ObjectChild>
<ObjectChild key='hybrid_homotopy_calculation'>
<bool />
</ObjectChild>
<ObjectChild key='robot_diff_circumscribed_inscribed_radius'>
<float />
</ObjectChild>
</Object>
<Object name='armarx::navigation::local_planning::arondto::trajectory'>
<ObjectChild key='teb_autosize'>
<bool />
</ObjectChild>
<ObjectChild key='dt_ref'>
<float />
</ObjectChild>
<ObjectChild key='dt_hysteresis'>
<float />
</ObjectChild>
<ObjectChild key='min_samples'>
<int />
</ObjectChild>
<ObjectChild key='max_samples'>
<int />
</ObjectChild>
<ObjectChild key='global_plan_overwrite_orientation'>
<bool />
</ObjectChild>
<ObjectChild key='allow_init_with_backwards_motion'>
<bool />
</ObjectChild>
<ObjectChild key='exact_arc_length'>
<bool />
</ObjectChild>
<ObjectChild key='force_reinit_new_goal_dist'>
<float />
</ObjectChild>
<ObjectChild key='force_reinit_new_goal_angular'>
<float />
</ObjectChild>
<ObjectChild key='feasibility_check_no_poses'>
<int />
</ObjectChild>
<ObjectChild key='feasibility_check_lookahead_distance'>
<float />
</ObjectChild>
<ObjectChild key='min_resolution_collision_check_angular'>
<float />
</ObjectChild>
</Object>
<Object name='armarx::navigation::local_planning::arondto::robot'>
<ObjectChild key='max_vel_x'>
<float />
</ObjectChild>
<ObjectChild key='max_vel_x_backwards'>
<float />
</ObjectChild>
<ObjectChild key='max_vel_y'>
<float />
</ObjectChild>
<ObjectChild key='max_vel_trans'>
<float />
</ObjectChild>
<ObjectChild key='max_vel_theta'>
<float />
</ObjectChild>
<ObjectChild key='acc_lim_x'>
<float />
</ObjectChild>
<ObjectChild key='acc_lim_y'>
<float />
</ObjectChild>
<ObjectChild key='acc_lim_theta'>
<float />
</ObjectChild>
<ObjectChild key='min_turning_radius'>
<float />
</ObjectChild>
</Object>
<Object name='armarx::navigation::local_planning::arondto::goal_tolerance'>
<ObjectChild key='xy_goal_tolerance'>
<float />
</ObjectChild>
</Object>
<Object name='armarx::navigation::local_planning::arondto::obstacles'>
<ObjectChild key='min_obstacle_dist'>
<float />
</ObjectChild>
<ObjectChild key='inflation_dist'>
<float />
</ObjectChild>
<ObjectChild key='dynamic_obstacle_inflation_dist'>
<float />
</ObjectChild>
<ObjectChild key='include_dynamic_obstacles'>
<bool />
</ObjectChild>
<ObjectChild key='obstacle_association_force_inclusion_factor'>
<float />
</ObjectChild>
<ObjectChild key='obstacle_association_cutoff_factor'>
<float />
</ObjectChild>
<ObjectChild key='obstacle_proximity_ratio_max_vel'>
<float />
</ObjectChild>
<ObjectChild key='obstacle_proximity_lower_bound'>
<float />
</ObjectChild>
<ObjectChild key='obstacle_proximity_upper_bound'>
<float />
</ObjectChild>
</Object>
<Object name='armarx::navigation::local_planning::arondto::optim'>
<ObjectChild key='no_inner_iterations'>
<int />
</ObjectChild>
<ObjectChild key='no_outer_iterations'>
<int />
</ObjectChild>
<ObjectChild key='optimization_activate'>
<bool />
</ObjectChild>
<ObjectChild key='optimization_verbose'>
<bool />
</ObjectChild>
<ObjectChild key='penalty_epsilon'>
<float />
</ObjectChild>
<ObjectChild key='weight_max_vel_x'>
<float />
</ObjectChild>
<ObjectChild key='weight_max_vel_y'>
<float />
</ObjectChild>
<ObjectChild key='weight_max_vel_theta'>
<float />
</ObjectChild>
<ObjectChild key='weight_acc_lim_x'>
<float />
</ObjectChild>
<ObjectChild key='weight_acc_lim_y'>
<float />
</ObjectChild>
<ObjectChild key='weight_acc_lim_theta'>
<float />
</ObjectChild>
<ObjectChild key='weight_kinematics_nh'>
<float />
</ObjectChild>
<ObjectChild key='weight_kinematics_forward_drive'>
<float />
</ObjectChild>
<ObjectChild key='weight_kinematics_turning_radius'>
<float />
</ObjectChild>
<ObjectChild key='weight_optimaltime'>
<float />
</ObjectChild>
<ObjectChild key='weight_shortest_path'>
<float />
</ObjectChild>
<ObjectChild key='weight_obstacle'>
<float />
</ObjectChild>
<ObjectChild key='weight_inflation'>
<float />
</ObjectChild>
<ObjectChild key='weight_dynamic_obstacle'>
<float />
</ObjectChild>
<ObjectChild key='weight_dynamic_obstacle_inflation'>
<float />
</ObjectChild>
<ObjectChild key='weight_velocity_obstacle_ratio'>
<float />
</ObjectChild>
<ObjectChild key='weight_viapoint'>
<float />
</ObjectChild>
<ObjectChild key='weight_prefer_rotdir'>
<float />
</ObjectChild>
<ObjectChild key='weight_adapt_factor'>
<float />
</ObjectChild>
<ObjectChild key='obstacle_cost_exponent'>
<float />
</ObjectChild>
</Object>
<Object name='armarx::navigation::local_planning::arondto::hcp'>
<ObjectChild key='enable_multithreading'>
<bool />
</ObjectChild>
<ObjectChild key='simple_exploration'>
<bool />
</ObjectChild>
<ObjectChild key='max_number_classes'>
<int />
</ObjectChild>
<ObjectChild key='max_number_plans_in_current_class'>
<int />
</ObjectChild>
<ObjectChild key='selection_cost_hysteresis'>
<float />
</ObjectChild>
<ObjectChild key='selection_obst_cost_scale'>
<float />
</ObjectChild>
<ObjectChild key='selection_viapoint_cost_scale'>
<float />
</ObjectChild>
<ObjectChild key='selection_alternative_time_cost'>
<bool />
</ObjectChild>
<ObjectChild key='selection_dropping_probability'>
<float />
</ObjectChild>
<ObjectChild key='switching_blocking_period'>
<float />
</ObjectChild>
<ObjectChild key='obstacle_heading_threshold'>
<float />
</ObjectChild>
<ObjectChild key='roadmap_graph_no_samples'>
<int />
</ObjectChild>
<ObjectChild key='roadmap_graph_area_width'>
<float />
</ObjectChild>
<ObjectChild key='roadmap_graph_area_length_scale'>
<float />
</ObjectChild>
<ObjectChild key='h_signature_prescaler'>
<float />
</ObjectChild>
<ObjectChild key='h_signature_threshold'>
<float />
</ObjectChild>
<ObjectChild key='viapoints_all_candidates'>
<bool />
</ObjectChild>
<ObjectChild key='delete_detours_backwards'>
<bool />
</ObjectChild>
<ObjectChild key='detours_orientation_tolerance'>
<float />
</ObjectChild>
<ObjectChild key='length_start_orientation_vector'>
<float />
</ObjectChild>
<ObjectChild key='max_ratio_detours_duration_best_duration'>
<float />
</ObjectChild>
</Object>
<Object name='armarx::navigation::local_planning::arondto::teb_config'>
<ObjectChild key='pse'>
<armarx::navigation::local_planning::arondto::pse />
</ObjectChild>
<ObjectChild key='trajectory'>
<armarx::navigation::local_planning::arondto::trajectory />
</ObjectChild>
<ObjectChild key='robot'>
<armarx::navigation::local_planning::arondto::robot />
</ObjectChild>
<ObjectChild key='goal_tolerance'>
<armarx::navigation::local_planning::arondto::goal_tolerance />
</ObjectChild>
<ObjectChild key='obstacles'>
<armarx::navigation::local_planning::arondto::obstacles />
</ObjectChild>
<ObjectChild key='optim'>
<armarx::navigation::local_planning::arondto::optim />
</ObjectChild>
<ObjectChild key='hcp'>
<armarx::navigation::local_planning::arondto::hcp />
</ObjectChild>
</Object>
<Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams'> <Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams'>
<ObjectChild key='foo'> <ObjectChild key='robot_footprint_radius'>
<float />
</ObjectChild>
<ObjectChild key='planning_distance'>
<float /> <float />
</ObjectChild> </ObjectChild>
<ObjectChild key='teb_config'>
<armarx::navigation::local_planning::arondto::teb_config />
</ObjectChild>
</Object> </Object>
</GenerateTypes> </GenerateTypes>
</AronTypeDefinition> </AronTypeDefinition>
source/armarx/navigation/local_planning/aron_conversions.cpp
+ 130
− 0
View file @ 4db8fdb7
...@@ -14,11 +14,141 @@ namespace armarx::navigation::local_planning ...@@ -14,11 +14,141 @@ namespace armarx::navigation::local_planning
void void
toAron(arondto::TimedElasticBandsParams& dto, const TimedElasticBandsParams& bo) toAron(arondto::TimedElasticBandsParams& dto, const TimedElasticBandsParams& bo)
{ {
dto = bo.cfg;
} }
void void
fromAron(const arondto::TimedElasticBandsParams& dto, TimedElasticBandsParams& bo) fromAron(const arondto::TimedElasticBandsParams& dto, TimedElasticBandsParams& bo)
{ {
bo.cfg = dto;
}
void
toTebCfg(const arondto::TimedElasticBandsParams& dto, teb_local_planner::TebConfig& bo)
{
bo.pse.pse_costum_obstacle_penalties = dto.teb_config.pse.pse_costum_obstacle_penalties;
bo.pse.pse_costum_obstacle_penalties_dynamic =
dto.teb_config.pse.pse_costum_obstacle_penalties_dynamic;
bo.pse.weight_global_path = dto.teb_config.pse.weight_global_path;
bo.pse.lrk_use_alternative_approach = dto.teb_config.pse.lrk_use_alternative_approach;
bo.pse.lrk_enable_collision_check = dto.teb_config.pse.lrk_enable_collision_check;
bo.pse.hybrid_homotopy_calculation = dto.teb_config.pse.hybrid_homotopy_calculation;
bo.pse.robot_diff_circumscribed_inscribed_radius =
dto.teb_config.pse.robot_diff_circumscribed_inscribed_radius;
// Trajectory
bo.trajectory.teb_autosize = bo.trajectory.teb_autosize;
bo.trajectory.dt_ref = dto.teb_config.trajectory.dt_ref;
bo.trajectory.dt_hysteresis = dto.teb_config.trajectory.dt_hysteresis;
bo.trajectory.min_samples = dto.teb_config.trajectory.min_samples;
bo.trajectory.max_samples = dto.teb_config.trajectory.max_samples;
bo.trajectory.global_plan_overwrite_orientation =
dto.teb_config.trajectory.global_plan_overwrite_orientation;
bo.trajectory.allow_init_with_backwards_motion =
dto.teb_config.trajectory.allow_init_with_backwards_motion;
bo.trajectory.exact_arc_length = dto.teb_config.trajectory.exact_arc_length;
bo.trajectory.force_reinit_new_goal_dist =
dto.teb_config.trajectory.force_reinit_new_goal_dist;
bo.trajectory.force_reinit_new_goal_angular =
dto.teb_config.trajectory.force_reinit_new_goal_angular;
bo.trajectory.feasibility_check_no_poses =
dto.teb_config.trajectory.feasibility_check_no_poses;
bo.trajectory.feasibility_check_lookahead_distance =
dto.teb_config.trajectory.feasibility_check_lookahead_distance;
bo.trajectory.min_resolution_collision_check_angular =
dto.teb_config.trajectory.min_resolution_collision_check_angular;
// Robot
bo.robot.max_vel_x = dto.teb_config.robot.max_vel_x;
bo.robot.max_vel_x_backwards = dto.teb_config.robot.max_vel_x_backwards;
bo.robot.max_vel_y = dto.teb_config.robot.max_vel_y;
bo.robot.max_vel_trans = dto.teb_config.robot.max_vel_trans;
bo.robot.max_vel_theta = dto.teb_config.robot.max_vel_theta;
bo.robot.acc_lim_x = dto.teb_config.robot.acc_lim_x;
bo.robot.acc_lim_y = dto.teb_config.robot.acc_lim_y;
bo.robot.acc_lim_theta = dto.teb_config.robot.acc_lim_theta;
bo.robot.min_turning_radius = dto.teb_config.robot.min_turning_radius;
// GoalTolerance
bo.goal_tolerance.xy_goal_tolerance = dto.teb_config.goal_tolerance.xy_goal_tolerance;
// Obstacles
bo.obstacles.min_obstacle_dist = dto.teb_config.obstacles.min_obstacle_dist;
bo.obstacles.inflation_dist = dto.teb_config.obstacles.inflation_dist;
bo.obstacles.dynamic_obstacle_inflation_dist =
dto.teb_config.obstacles.dynamic_obstacle_inflation_dist;
bo.obstacles.include_dynamic_obstacles = dto.teb_config.obstacles.include_dynamic_obstacles;
bo.obstacles.obstacle_association_force_inclusion_factor =
dto.teb_config.obstacles.obstacle_association_force_inclusion_factor;
bo.obstacles.obstacle_association_cutoff_factor =
dto.teb_config.obstacles.obstacle_association_cutoff_factor;
bo.obstacles.obstacle_proximity_ratio_max_vel =
dto.teb_config.obstacles.obstacle_proximity_ratio_max_vel;
bo.obstacles.obstacle_proximity_lower_bound =
dto.teb_config.obstacles.obstacle_proximity_lower_bound;
bo.obstacles.obstacle_proximity_upper_bound =
dto.teb_config.obstacles.obstacle_proximity_upper_bound;
// Optimization
bo.optim.no_inner_iterations = dto.teb_config.optim.no_inner_iterations;
bo.optim.no_outer_iterations = dto.teb_config.optim.no_outer_iterations;
bo.optim.optimization_activate = dto.teb_config.optim.optimization_activate;
bo.optim.optimization_verbose = dto.teb_config.optim.optimization_verbose;
bo.optim.penalty_epsilon = dto.teb_config.optim.penalty_epsilon;
bo.optim.weight_max_vel_x = dto.teb_config.optim.weight_max_vel_x;
bo.optim.weight_max_vel_y = dto.teb_config.optim.weight_max_vel_y;
bo.optim.weight_max_vel_theta = dto.teb_config.optim.weight_max_vel_theta;
bo.optim.weight_acc_lim_x = dto.teb_config.optim.weight_acc_lim_x;
bo.optim.weight_acc_lim_y = dto.teb_config.optim.weight_acc_lim_y;
bo.optim.weight_acc_lim_theta = dto.teb_config.optim.weight_acc_lim_theta;
bo.optim.weight_kinematics_nh = dto.teb_config.optim.weight_kinematics_nh;
bo.optim.weight_kinematics_forward_drive =
dto.teb_config.optim.weight_kinematics_forward_drive;
bo.optim.weight_kinematics_turning_radius =
dto.teb_config.optim.weight_kinematics_turning_radius;
bo.optim.weight_optimaltime = dto.teb_config.optim.weight_optimaltime;
bo.optim.weight_shortest_path = dto.teb_config.optim.weight_shortest_path;
bo.optim.weight_obstacle = dto.teb_config.optim.weight_obstacle;
bo.optim.weight_inflation = dto.teb_config.optim.weight_inflation;
bo.optim.weight_dynamic_obstacle = dto.teb_config.optim.weight_dynamic_obstacle;
bo.optim.weight_dynamic_obstacle_inflation =
dto.teb_config.optim.weight_dynamic_obstacle_inflation;
bo.optim.weight_velocity_obstacle_ratio =
dto.teb_config.optim.weight_velocity_obstacle_ratio;
bo.optim.weight_viapoint = dto.teb_config.optim.weight_viapoint;
bo.optim.weight_prefer_rotdir = dto.teb_config.optim.weight_prefer_rotdir;
bo.optim.weight_adapt_factor = dto.teb_config.optim.weight_adapt_factor;
bo.optim.obstacle_cost_exponent = dto.teb_config.optim.obstacle_cost_exponent;
// Homotopy Class Planner
bo.hcp.enable_multithreading = dto.teb_config.hcp.enable_multithreading;
bo.hcp.simple_exploration = dto.teb_config.hcp.simple_exploration;
bo.hcp.max_number_classes = dto.teb_config.hcp.max_number_classes;
bo.hcp.max_number_plans_in_current_class =
dto.teb_config.hcp.max_number_plans_in_current_class;
bo.hcp.selection_cost_hysteresis = dto.teb_config.hcp.selection_cost_hysteresis;
bo.hcp.selection_obst_cost_scale = dto.teb_config.hcp.selection_obst_cost_scale;
bo.hcp.selection_viapoint_cost_scale = dto.teb_config.hcp.selection_viapoint_cost_scale;
bo.hcp.selection_alternative_time_cost = dto.teb_config.hcp.selection_alternative_time_cost;
bo.hcp.selection_dropping_probability = dto.teb_config.hcp.selection_dropping_probability;
bo.hcp.switching_blocking_period = dto.teb_config.hcp.switching_blocking_period;
bo.hcp.obstacle_heading_threshold = dto.teb_config.hcp.obstacle_heading_threshold;
bo.hcp.roadmap_graph_no_samples = dto.teb_config.hcp.roadmap_graph_no_samples;
bo.hcp.roadmap_graph_area_width = dto.teb_config.hcp.roadmap_graph_area_width;
bo.hcp.roadmap_graph_area_length_scale = dto.teb_config.hcp.roadmap_graph_area_length_scale;
bo.hcp.h_signature_prescaler = dto.teb_config.hcp.h_signature_prescaler;
bo.hcp.h_signature_threshold = dto.teb_config.hcp.h_signature_threshold;
bo.hcp.viapoints_all_candidates = dto.teb_config.hcp.viapoints_all_candidates;
bo.hcp.delete_detours_backwards = dto.teb_config.hcp.delete_detours_backwards;
bo.hcp.detours_orientation_tolerance = dto.teb_config.hcp.detours_orientation_tolerance;
bo.hcp.length_start_orientation_vector = dto.teb_config.hcp.length_start_orientation_vector;
bo.hcp.max_ratio_detours_duration_best_duration =
dto.teb_config.hcp.max_ratio_detours_duration_best_duration;
bo.checkParameters();
} }
......
source/armarx/navigation/local_planning/aron_conversions.h
+ 4
− 0
View file @ 4db8fdb7
#pragma once #pragma once
#include <teb_local_planner/teb_config.h>
namespace armarx::navigation::local_planning namespace armarx::navigation::local_planning
{ {
// struct LocalPlannerParams; // struct LocalPlannerParams;
...@@ -19,4 +21,6 @@ namespace armarx::navigation::local_planning ...@@ -19,4 +21,6 @@ namespace armarx::navigation::local_planning
void toAron(arondto::TimedElasticBandsParams& dto, const TimedElasticBandsParams& bo); void toAron(arondto::TimedElasticBandsParams& dto, const TimedElasticBandsParams& bo);
void fromAron(const arondto::TimedElasticBandsParams& dto, TimedElasticBandsParams& bo); void fromAron(const arondto::TimedElasticBandsParams& dto, TimedElasticBandsParams& bo);
void toTebCfg(const arondto::TimedElasticBandsParams& dto, teb_local_planner::TebConfig& bo);
} // namespace armarx::navigation::local_planning } // namespace armarx::navigation::local_planning
source/armarx/navigation/local_planning/ros_conversions.cpp
+ 1
− 1
View file @ 4db8fdb7
...@@ -18,7 +18,7 @@ namespace armarx::navigation::conv ...@@ -18,7 +18,7 @@ namespace armarx::navigation::conv
Eigen::Vector2d Eigen::Vector2d
toRos(const Eigen::Vector3f& vec) toRos(const Eigen::Vector3f& vec)
{ {
return conv::to2D(vec).cast<double>() / 1000; return conv::to2D(vec).cast<double>() / 1000; // [mm] to [m]
} }
......
source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
+ 1
− 1
View file @ 4db8fdb7
...@@ -172,7 +172,7 @@ namespace armarx::navigation::server ...@@ -172,7 +172,7 @@ namespace armarx::navigation::server
simox::ColorMap cm = simox::color::cmaps::inferno(); simox::ColorMap cm = simox::color::cmaps::inferno();
cm.set_vmin(0); cm.set_vmin(0);
cm.set_vmax(1); cm.set_vmax(0.6);
for (size_t i = 0; i < trajectory.points().size() - 1; i++) for (size_t i = 0; i < trajectory.points().size() - 1; i++)
{ {
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment