From 0fb241cf7bad4915c2e5e3dbf7e41ce3ba887c4c Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Mon, 20 Feb 2023 15:45:03 +0100
Subject: [PATCH] A few updates
- In PRM, do not insert edges in the loop over the connected components,
- replace deprecated call in script tutorial_2.py .
---
include/hpp_tutorial/tutorial_2.hh | 2 +-
script/tutorial_2.py | 2 +-
src/tutorial_1.cc | 4 ++--
src/tutorial_2.cc | 29 +++++++++++++++++++----------
4 files changed, 23 insertions(+), 14 deletions(-)
diff --git a/include/hpp_tutorial/tutorial_2.hh b/include/hpp_tutorial/tutorial_2.hh
index 702b0cd..06cb487 100644
--- a/include/hpp_tutorial/tutorial_2.hh
+++ b/include/hpp_tutorial/tutorial_2.hh
@@ -21,7 +21,7 @@
///
/// <h1>Implementing a new path planning algorithm</h1>
///
-/// The code of this tutorial can be found in \c src/tutorial.cc.
+/// The code of this tutorial can be found in \c src/tutorial_2.cc.
/// The compilation and installation instructions can be found in
/// \c CMakeLists.txt.
///
diff --git a/script/tutorial_2.py b/script/tutorial_2.py
index 49c0b70..ee9d394 100644
--- a/script/tutorial_2.py
+++ b/script/tutorial_2.py
@@ -27,7 +27,7 @@ rank = robot.rankInConfiguration["r_elbow_flex_joint"]
q_goal[rank] = -0.5
vf(q_goal)
-vf.loadObstacleModel("iai_maps", "kitchen_area", "kitchen")
+vf.loadObstacleModel("package://hpp_tutorial/urdf/kitchen_area.urdf", "kitchen")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
diff --git a/src/tutorial_1.cc b/src/tutorial_1.cc
index f619338..103eea8 100644
--- a/src/tutorial_1.cc
+++ b/src/tutorial_1.cc
@@ -42,8 +42,8 @@ int main() {
// Add obstacle
DevicePtr_t obstacle = Device::create("kitchen");
- hpp::pinocchio::urdf::loadUrdfModel(obstacle, "anchor", "iai_maps",
- "kitchen_area");
+ hpp::pinocchio::urdf::loadModel(obstacle, 0, "", "anchor",
+ "package://hpp_tutorial/urdf/kitchen_area.urdf", "");
obstacle->controlComputation(JOINT_POSITION);
ps->addObstacle(obstacle, true, true);
diff --git a/src/tutorial_2.cc b/src/tutorial_2.cc
index f80f323..22f0148 100644
--- a/src/tutorial_2.cc
+++ b/src/tutorial_2.cc
@@ -58,6 +58,8 @@ class Planner : public core::PathPlanner {
///
/// We will see how to implement a basic PRM algorithm.
virtual void oneStep() {
+ using core::NodePtr_t;
+ using core::PathPtr_t;
// Retrieve the robot the problem has been defined for.
pinocchio::DevicePtr_t robot(problem()->robot());
// Retrieve the path validation algorithm associated to the problem
@@ -79,32 +81,39 @@ class Planner : public core::PathPlanner {
shooter_->shoot(qrand);
} while (!configValidations->validate(qrand, validationReport));
// Add qrand as a new node
- core::NodePtr_t newNode = r->addNode(qrand);
+ NodePtr_t newNode = r->addNode(qrand);
// try to connect the random configuration to each connected component
// of the roadmap.
- for (core::ConnectedComponents_t::const_iterator itcc =
- r->connectedComponents().begin();
- itcc != r->connectedComponents().end(); ++itcc) {
- core::ConnectedComponentPtr_t cc = *itcc;
+
+ // Modifying the connected components of the graph while making a loop
+ // over all of them is not correct. We therefore record the edges to
+ // to insert and add them after the loop.
+ typedef std::tuple<NodePtr_t, NodePtr_t, PathPtr_t> DelayedEdge_t;
+ typedef std::vector<DelayedEdge_t> DelayedEdges_t;
+ DelayedEdges_t delayedEdges;
+ for (auto cc : r->connectedComponents()) {
// except its own connected component of course
if (cc != newNode->connectedComponent()) {
value_type d;
// Get nearest node to qrand in connected component
- core::NodePtr_t nearest = r->nearestNode(qrand, cc, d);
+ NodePtr_t nearest = r->nearestNode(qrand, cc, d);
core::ConfigurationPtr_t qnear = nearest->configuration();
// Create local path between qnear and qrand
- core::PathPtr_t localPath = (*sm)(*qnear, qrand);
+ PathPtr_t localPath = (*sm)(*qnear, qrand);
// validate local path
- core::PathPtr_t validPart;
+ PathPtr_t validPart;
// report on path validation: unused here
core::PathValidationReportPtr_t report;
if (pathValidation->validate(localPath, false, validPart, report)) {
// Create node and edges with qrand and the local path
- r->addEdge(nearest, newNode, localPath);
- r->addEdge(newNode, nearest, localPath->reverse());
+ delayedEdges.push_back(DelayedEdge_t(nearest, newNode, localPath));
}
}
}
+ for(auto de : delayedEdges){
+ r->addEdge(std::get<0>(de), std::get<1>(de), std::get<2>(de));
+ r->addEdge(std::get<1>(de), std::get<0>(de), std::get<2>(de)->reverse());
+ }
}
protected:
--
GitLab