From d88b240d2f1144f32a0d7fbdd0678283be9a7f10 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 7 Jan 2022 17:39:37 +0100
Subject: [PATCH] fix: only optimizing orientation if intermediate points are
available
---
source/armarx/navigation/global_planning/AStar.cpp | 9 ++++++++-
1 file changed, 8 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/global_planning/AStar.cpp b/source/armarx/navigation/global_planning/AStar.cpp
index be670e24..1d13ae8a 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/source/armarx/navigation/global_planning/AStar.cpp
@@ -164,7 +164,8 @@ namespace armarx::navigation::global_planning
// planner.setRobotColModel("Platform-colmodel");
//planner.setRobotCollisionModel(robotCollisionModel());
- planner.setRobotCollisionModel(scene.robot->getRobotNode("Platform-navigation-colmodel")->getCollisionModel());
+ planner.setRobotCollisionModel(
+ scene.robot->getRobotNode("Platform-navigation-colmodel")->getCollisionModel());
const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
@@ -267,6 +268,12 @@ namespace armarx::navigation::global_planning
// ARMARX_CHECK(resampledTrajectory.hasMaxDistanceBetweenWaypoints(params.resampleDistance));
+ if (resampledTrajectory->points().size() == 2)
+ {
+ ARMARX_INFO << "Only start and goal provided. Not optimizing orientation";
+ ARMARX_TRACE;
+ return GlobalPlannerResult{.trajectory = resampledTrajectory.value()};
+ }
ARMARX_TRACE;
OrientationOptimizer optimizer(resampledTrajectory.value(), OrientationOptimizer::Params{});
--
GitLab