From 2188d923d6e63466058a81289a31ca9eafb70d50 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 22 Sep 2022 19:16:28 +0200
Subject: [PATCH] Add costmap with custom penalty to optimization
---
.../navigation/human/ProxemicZoneCreator.cpp | 15 ++++++++++-----
.../local_planning/TebObstacleManager.cpp | 1 +
.../local_planning/TimedElasticBands.cpp | 13 ++++++++++---
3 files changed, 21 insertions(+), 8 deletions(-)
diff --git a/source/armarx/navigation/human/ProxemicZoneCreator.cpp b/source/armarx/navigation/human/ProxemicZoneCreator.cpp
index 8a6aac53..acd453fa 100644
--- a/source/armarx/navigation/human/ProxemicZoneCreator.cpp
+++ b/source/armarx/navigation/human/ProxemicZoneCreator.cpp
@@ -9,10 +9,13 @@ namespace armarx::navigation::human
ProxemicZoneCreator::createProxemicZones(const Human& human)
{
// intimate zone
- ProxemicZone intimate{.pose = human.pose,
- .shape = {.a = params.intimateRadius, .b = params.intimateRadius},
- .penalty = intimatePenalty,
- .weight = params.intimateWeight};
+ ProxemicZone intimate{
+ .pose = human.pose,
+ .shape = {.a = params.intimateRadius, .b = params.intimateRadius},
+ .penalty = intimatePenalty,
+ .weight = params.intimateWeight,
+ .homotopicRelevance = true,
+ };
// personal zone
auto& global_R_human = human.pose.linear();
@@ -38,7 +41,9 @@ namespace armarx::navigation::human
.pose = pose,
.shape = {.a = params.personalRadius, .b = movementStretch * params.personalRadius},
.penalty = personalPenalty,
- .weight = params.personalWeight};
+ .weight = params.personalWeight,
+ .homotopicRelevance = false,
+ };
return {intimate, personal};
}
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index add2038f..e2402e12 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.cpp
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
@@ -37,6 +37,7 @@ namespace armarx::navigation::local_planning
obst->pushBackVertex(max.x(), min.y());
obst->finalizePolygon();
+ obst->setUseForOptimization(false);
container.push_back(obst);
// visualize bounding box if layer is available
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 7bd970f0..8f86171a 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -1,4 +1,5 @@
#include "TimedElasticBands.h"
+
#include <optional>
#include <SimoxUtility/algorithm/apply.hpp>
@@ -68,10 +69,16 @@ namespace armarx::navigation::local_planning
hcp_ = std::make_unique<teb_local_planner::HomotopyClassPlanner>();
hcp_->initialize(
- cfg_, &teb_obstacles, robot_model, teb_local_planner::TebVisualizationPtr(), nullptr);
+ cfg_, &teb_obstacles, robot_model, teb_local_planner::TebVisualizationPtr(), nullptr);
//set member teb_costmap
setTebCostmap();
- if (teb_costmap) {
+ if (teb_costmap)
+ {
+ // TODO: where to put all the parameters
+ const human::ExponentialPenaltyModel penalty{
+ .minDistance = 0.5, .epsilon = 0, .exponent = 1.2};
+
+ teb_costmap->setPenaltyModel(conv::toRos(penalty));
hcp_->setCostmap(&teb_costmap.value());
}
ros::Time::init(); // we have to init time before we can use the planner
@@ -130,7 +137,7 @@ namespace armarx::navigation::local_planning
return {};
}
- if(hcp_->getTrajectoryContainer().empty())
+ if (hcp_->getTrajectoryContainer().empty())
{
ARMARX_VERBOSE << "Did not find any trajectory!";
return std::nullopt;
--
GitLab