From cc76b8e3258a0598acd38ef8c40848eafb3ee4f9 Mon Sep 17 00:00:00 2001 From: Ramy Date: Sat, 21 Sep 2024 00:11:03 +0000 Subject: [PATCH 01/13] connect only --- src/ompl/tools/thunder/src/Thunder.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/ompl/tools/thunder/src/Thunder.cpp b/src/ompl/tools/thunder/src/Thunder.cpp index 6d5437f7a4..72be4055d2 100644 --- a/src/ompl/tools/thunder/src/Thunder.cpp +++ b/src/ompl/tools/thunder/src/Thunder.cpp @@ -116,10 +116,6 @@ void ompl::tools::Thunder::setup() cforest_planner->setNumThreads(cforest_n_threads_); planner = cforest_planner; } - else if (counter % 2 == 0) - { - planner = std::make_shared(si_); - } else { planner = std::make_shared(si_); From 40adc12d2ac5a2b1ed31423be998c284cb85ad12 Mon Sep 17 00:00:00 2001 From: Ramy Date: Sat, 21 Sep 2024 03:18:11 +0000 Subject: [PATCH 02/13] sparse_delta/2 --- src/ompl/tools/thunder/src/SPARSdb.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ompl/tools/thunder/src/SPARSdb.cpp b/src/ompl/tools/thunder/src/SPARSdb.cpp index ba0a1e7207..d5e23e139a 100644 --- a/src/ompl/tools/thunder/src/SPARSdb.cpp +++ b/src/ompl/tools/thunder/src/SPARSdb.cpp @@ -200,7 +200,7 @@ bool ompl::geometric::SPARSdb::getSimilarPaths(int /*nearestK*/, const base::Sta // Start OMPL_INFORM("Looking for a node near the problem start"); - if (!findGraphNeighbors(start, startVertexCandidateNeighbors_)) + if (!findGraphNeighbors(start, startVertexCandidateNeighbors_, sparseDelta_/2)) { OMPL_INFORM("No graph neighbors found for start within radius %f", sparseDelta_); return false; @@ -210,7 +210,7 @@ bool ompl::geometric::SPARSdb::getSimilarPaths(int /*nearestK*/, const base::Sta // Goal OMPL_INFORM("Looking for a node near the problem goal"); - if (!findGraphNeighbors(goal, goalVertexCandidateNeighbors_)) + if (!findGraphNeighbors(goal, goalVertexCandidateNeighbors_, sparseDelta_/2)) { OMPL_INFORM("No graph neighbors found for goal within radius %f", sparseDelta_); return false; From 3e3a23f60d461143499b3b97091b34f0da1e9329 Mon Sep 17 00:00:00 2001 From: Ramy Date: Sat, 21 Sep 2024 03:30:45 +0000 Subject: [PATCH 03/13] /4 --- src/ompl/tools/thunder/src/SPARSdb.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ompl/tools/thunder/src/SPARSdb.cpp b/src/ompl/tools/thunder/src/SPARSdb.cpp index d5e23e139a..5598fdaeab 100644 --- a/src/ompl/tools/thunder/src/SPARSdb.cpp +++ b/src/ompl/tools/thunder/src/SPARSdb.cpp @@ -200,7 +200,7 @@ bool ompl::geometric::SPARSdb::getSimilarPaths(int /*nearestK*/, const base::Sta // Start OMPL_INFORM("Looking for a node near the problem start"); - if (!findGraphNeighbors(start, startVertexCandidateNeighbors_, sparseDelta_/2)) + if (!findGraphNeighbors(start, startVertexCandidateNeighbors_, sparseDelta_/4)) { OMPL_INFORM("No graph neighbors found for start within radius %f", sparseDelta_); return false; @@ -210,7 +210,7 @@ bool ompl::geometric::SPARSdb::getSimilarPaths(int /*nearestK*/, const base::Sta // Goal OMPL_INFORM("Looking for a node near the problem goal"); - if (!findGraphNeighbors(goal, goalVertexCandidateNeighbors_, sparseDelta_/2)) + if (!findGraphNeighbors(goal, goalVertexCandidateNeighbors_, sparseDelta_/4)) { OMPL_INFORM("No graph neighbors found for goal within radius %f", sparseDelta_); return false; From 3702b390d9039fb6750ab6ff454ac8bcb0f67910 Mon Sep 17 00:00:00 2001 From: yammineramy <88019967+yammineramy@users.noreply.github.com> Date: Fri, 20 Sep 2024 23:58:32 -0400 Subject: [PATCH 04/13] Update SPARSdb.cpp --- src/ompl/tools/thunder/src/SPARSdb.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/ompl/tools/thunder/src/SPARSdb.cpp b/src/ompl/tools/thunder/src/SPARSdb.cpp index 5598fdaeab..9b3d716889 100644 --- a/src/ompl/tools/thunder/src/SPARSdb.cpp +++ b/src/ompl/tools/thunder/src/SPARSdb.cpp @@ -1312,8 +1312,11 @@ void ompl::geometric::SPARSdb::findGraphNeighbors(base::State *st, std::vectornearestR(queryVertex_, *radius, graphNeighborhood); if (verbose_ && false) OMPL_INFORM("Finding nearest nodes in NN tree within radius %f", *radius); From 492380627841a92303d98e0896fd2a3766610f3a Mon Sep 17 00:00:00 2001 From: yammineramy <88019967+yammineramy@users.noreply.github.com> Date: Sat, 21 Sep 2024 00:26:14 -0400 Subject: [PATCH 05/13] Update SPARSdb.cpp --- src/ompl/tools/thunder/src/SPARSdb.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/ompl/tools/thunder/src/SPARSdb.cpp b/src/ompl/tools/thunder/src/SPARSdb.cpp index 9b3d716889..8c7055a6a7 100644 --- a/src/ompl/tools/thunder/src/SPARSdb.cpp +++ b/src/ompl/tools/thunder/src/SPARSdb.cpp @@ -1333,8 +1333,11 @@ bool ompl::geometric::SPARSdb::findGraphNeighbors(const base::State *state, std: base::State *stateCopy = si_->cloneState(state); if (!radius.has_value()) { + OMPL_INFORM("No radius provided, using sparseDelta_"); radius = sparseDelta_; } + OMPL_INFORM("radius: %f", *radius); + OMPL_INFORM("sparseDelta_: %f", sparseDelta_); // Don't check for visibility graphNeighborhood.clear(); stateProperty_[queryVertex_] = stateCopy; From 02f0eb470ee95e46a9362a7cc563286c1cdc1070 Mon Sep 17 00:00:00 2001 From: yammineramy <88019967+yammineramy@users.noreply.github.com> Date: Sat, 21 Sep 2024 00:36:01 -0400 Subject: [PATCH 06/13] Update SPARSdb.cpp --- src/ompl/tools/thunder/src/SPARSdb.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ompl/tools/thunder/src/SPARSdb.cpp b/src/ompl/tools/thunder/src/SPARSdb.cpp index 8c7055a6a7..511c87d54f 100644 --- a/src/ompl/tools/thunder/src/SPARSdb.cpp +++ b/src/ompl/tools/thunder/src/SPARSdb.cpp @@ -200,6 +200,7 @@ bool ompl::geometric::SPARSdb::getSimilarPaths(int /*nearestK*/, const base::Sta // Start OMPL_INFORM("Looking for a node near the problem start"); + OMPL_INFORM("jajaja"); if (!findGraphNeighbors(start, startVertexCandidateNeighbors_, sparseDelta_/4)) { OMPL_INFORM("No graph neighbors found for start within radius %f", sparseDelta_); From 40315a6bde5ff0b90f4eddb57f4fc3e8232281d1 Mon Sep 17 00:00:00 2001 From: yammineramy <88019967+yammineramy@users.noreply.github.com> Date: Sat, 21 Sep 2024 01:40:28 -0400 Subject: [PATCH 07/13] Update MinimaxObjective.cpp --- src/ompl/base/objectives/src/MinimaxObjective.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ompl/base/objectives/src/MinimaxObjective.cpp b/src/ompl/base/objectives/src/MinimaxObjective.cpp index 842e194b92..9908fd6773 100644 --- a/src/ompl/base/objectives/src/MinimaxObjective.cpp +++ b/src/ompl/base/objectives/src/MinimaxObjective.cpp @@ -69,7 +69,7 @@ ompl::base::Cost ompl::base::MinimaxObjective::motionCost(const State *s1, const if (this->isCostBetterThan(worstCost, lastCost)) worstCost = lastCost; - return worstCost; + return si_->getStateSpace()->distance(s1, s2) * worstCost; } ompl::base::Cost ompl::base::MinimaxObjective::combineCosts(Cost c1, Cost c2) const From 087dbacc715c697f1a6b7c9bbada11ab99a46503 Mon Sep 17 00:00:00 2001 From: yammineramy <88019967+yammineramy@users.noreply.github.com> Date: Sat, 21 Sep 2024 01:42:14 -0400 Subject: [PATCH 08/13] Update SPARSdb.cpp --- src/ompl/tools/thunder/src/SPARSdb.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/ompl/tools/thunder/src/SPARSdb.cpp b/src/ompl/tools/thunder/src/SPARSdb.cpp index 511c87d54f..cd207db3b8 100644 --- a/src/ompl/tools/thunder/src/SPARSdb.cpp +++ b/src/ompl/tools/thunder/src/SPARSdb.cpp @@ -1313,11 +1313,8 @@ void ompl::geometric::SPARSdb::findGraphNeighbors(base::State *st, std::vectornearestR(queryVertex_, *radius, graphNeighborhood); if (verbose_ && false) OMPL_INFORM("Finding nearest nodes in NN tree within radius %f", *radius); From 248b7e85fd9161b18f2c75fc8d4e7fe26fa3d91d Mon Sep 17 00:00:00 2001 From: yammineramy <88019967+yammineramy@users.noreply.github.com> Date: Sat, 21 Sep 2024 01:44:18 -0400 Subject: [PATCH 09/13] Update MinimaxObjective.cpp --- src/ompl/base/objectives/src/MinimaxObjective.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/ompl/base/objectives/src/MinimaxObjective.cpp b/src/ompl/base/objectives/src/MinimaxObjective.cpp index 9908fd6773..1e7b805b49 100644 --- a/src/ompl/base/objectives/src/MinimaxObjective.cpp +++ b/src/ompl/base/objectives/src/MinimaxObjective.cpp @@ -68,8 +68,7 @@ ompl::base::Cost ompl::base::MinimaxObjective::motionCost(const State *s1, const Cost lastCost = this->stateCost(s2); if (this->isCostBetterThan(worstCost, lastCost)) worstCost = lastCost; - - return si_->getStateSpace()->distance(s1, s2) * worstCost; + return Cost(si_->getStateSpace()->distance(s1, s2) * worstCost.value()); } ompl::base::Cost ompl::base::MinimaxObjective::combineCosts(Cost c1, Cost c2) const From 1980b930412568bd9fcc329fa32006ca6837e485 Mon Sep 17 00:00:00 2001 From: yammineramy <88019967+yammineramy@users.noreply.github.com> Date: Sat, 21 Sep 2024 01:53:43 -0400 Subject: [PATCH 10/13] Update SPARSdb.cpp --- src/ompl/tools/thunder/src/SPARSdb.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ompl/tools/thunder/src/SPARSdb.cpp b/src/ompl/tools/thunder/src/SPARSdb.cpp index cd207db3b8..d4eca5fac9 100644 --- a/src/ompl/tools/thunder/src/SPARSdb.cpp +++ b/src/ompl/tools/thunder/src/SPARSdb.cpp @@ -201,7 +201,7 @@ bool ompl::geometric::SPARSdb::getSimilarPaths(int /*nearestK*/, const base::Sta // Start OMPL_INFORM("Looking for a node near the problem start"); OMPL_INFORM("jajaja"); - if (!findGraphNeighbors(start, startVertexCandidateNeighbors_, sparseDelta_/4)) + if (!findGraphNeighbors(start, startVertexCandidateNeighbors_)) { OMPL_INFORM("No graph neighbors found for start within radius %f", sparseDelta_); return false; @@ -211,7 +211,7 @@ bool ompl::geometric::SPARSdb::getSimilarPaths(int /*nearestK*/, const base::Sta // Goal OMPL_INFORM("Looking for a node near the problem goal"); - if (!findGraphNeighbors(goal, goalVertexCandidateNeighbors_, sparseDelta_/4)) + if (!findGraphNeighbors(goal, goalVertexCandidateNeighbors_)) { OMPL_INFORM("No graph neighbors found for goal within radius %f", sparseDelta_); return false; From e932a3b7fb1ab21ae8f761b26768114c4e89a621 Mon Sep 17 00:00:00 2001 From: yammineramy <88019967+yammineramy@users.noreply.github.com> Date: Sat, 21 Sep 2024 02:26:11 -0400 Subject: [PATCH 11/13] Update ParallelPlan.cpp --- src/ompl/tools/multiplan/src/ParallelPlan.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/ompl/tools/multiplan/src/ParallelPlan.cpp b/src/ompl/tools/multiplan/src/ParallelPlan.cpp index 17a8ef1ea4..91e9eb9fe5 100644 --- a/src/ompl/tools/multiplan/src/ParallelPlan.cpp +++ b/src/ompl/tools/multiplan/src/ParallelPlan.cpp @@ -174,10 +174,10 @@ void ompl::tools::ParallelPlan::solveMore(base::Planner *planner, std::size_t mi if (nrSol >= maxSolCount) ptc->terminate(); - double found_path_length {pdef_->getSolutionPath()->length()}; - OMPL_DEBUG("ParallelPlan.solveMore: Solution of length: %lf found by %s in %lf seconds", found_path_length, planner->getName().c_str(), duration); - + double found_path_cost {pdef_->getSolutionPath()->cost(pdef_->getOptimizationObjective()).value()}; + OMPL_DEBUG("ParallelPlan.solveMore: Solution of length: %lf and cost: %lf found by %s in %lf seconds", found_path_length, found_path_cost, planner->getName().c_str(), duration); + // OMPL_DEBUG("ParallelPlan.solveMore: Solution of length: %lf found by %s in %lf seconds", found_path_length, planner->getName().c_str(), duration); const std::vector &paths = pdef_->getSolutions(); std::lock_guard slock(phlock_); From e495b0d0f58268a92ad86d66da32cdd9a781f2e2 Mon Sep 17 00:00:00 2001 From: yammineramy <88019967+yammineramy@users.noreply.github.com> Date: Sat, 21 Sep 2024 02:44:23 -0400 Subject: [PATCH 12/13] Update Thunder.cpp --- src/ompl/tools/thunder/src/Thunder.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/ompl/tools/thunder/src/Thunder.cpp b/src/ompl/tools/thunder/src/Thunder.cpp index 72be4055d2..6d5437f7a4 100644 --- a/src/ompl/tools/thunder/src/Thunder.cpp +++ b/src/ompl/tools/thunder/src/Thunder.cpp @@ -116,6 +116,10 @@ void ompl::tools::Thunder::setup() cforest_planner->setNumThreads(cforest_n_threads_); planner = cforest_planner; } + else if (counter % 2 == 0) + { + planner = std::make_shared(si_); + } else { planner = std::make_shared(si_); From 214689e01e46102162068bed005df891cd5317f9 Mon Sep 17 00:00:00 2001 From: yammineramy <88019967+yammineramy@users.noreply.github.com> Date: Sat, 21 Sep 2024 14:46:45 -0400 Subject: [PATCH 13/13] Update PlannerDataStorage.h --- src/ompl/base/PlannerDataStorage.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/ompl/base/PlannerDataStorage.h b/src/ompl/base/PlannerDataStorage.h index c594a10948..aea532d9bf 100644 --- a/src/ompl/base/PlannerDataStorage.h +++ b/src/ompl/base/PlannerDataStorage.h @@ -271,7 +271,6 @@ namespace ompl if (!weight_opt.has_value()) OMPL_ERROR("Unable to get edge weight"); Cost weight {weight_opt.value()}; - OMPL_DEBUG("Storing edge (%i to %i) with weight %f", fromVertex, toVertex, weight.value()); // Convert to new structure auto edgeData {PlannerDataEdgeData()}; edgeData.e_ = &pd.getEdge(fromVertex, toVertex);