From 365361594c1de3d99460c47ff04f5bd879aad563 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 29 Jul 2014 17:19:37 +0200
Subject: [PATCH] ProblemSolver gives the Graph to the planner before solving
---
include/hpp/manipulation/problem-solver.hh | 37 +++++++++++++---
src/problem-solver.cc | 49 ++++++++++++++++++----
2 files changed, 73 insertions(+), 13 deletions(-)
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 932042a4..3e7869d4 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -21,8 +21,9 @@
# include <map>
# include <hpp/core/problem-solver.hh>
# include <hpp/model/device.hh>
-# include <hpp/manipulation/object.hh>
-# include <hpp/manipulation/robot.hh>
+# include "hpp/manipulation/object.hh"
+# include "hpp/manipulation/robot.hh"
+# include "hpp/manipulation/graph/fwd.hh"
namespace hpp {
namespace manipulation {
@@ -77,29 +78,52 @@ namespace hpp {
///
/// throw if no robot is registered with this name.
DevicePtr_t robot (const std::string& name) const;
-
+
/// Get object with given name
///
/// throw if no object is registered with this name.
ObjectPtr_t object (const std::string& name) const;
/// \}
+ /// \name Constraint graph
+ /// \{
+
+ /// Set the constraint graph
+ void constraintGraph (const graph::GraphPtr_t& graph);
+
+ /// Get the constraint graph
+ graph::GraphPtr_t constraintGraph () const;
+ /// \}
+
+ /// \name Solve problem
+ /// \{
+
+ /// Prepare the solver for a step by step planning.
+ /// and try to make direct connections (call PathPlanner::tryDirectPath)
+ /// \return the return value of PathPlanner::pathExists
+ virtual bool prepareSolveStepByStep ();
+
+ /// Set and solve the problem
+ virtual void solve ();
+
+ /// \}
+
/// Add grasp
void addGrasp( const DifferentiableFunctionPtr_t& constraint,
const model::GripperPtr_t& gripper,
- const HandlePtr_t& handle)
+ const HandlePtr_t& handle)
{
Grasp_t* ptr = new Grasp_t (gripper, handle);
GraspPtr_t shPtr (ptr);
graspsMap_[constraint] = shPtr;
}
-
+
/// get grapsMap
GraspsMap_t& grasps()
{
return graspsMap_;
}
-
+
/// get graps by name
///
/// return NULL if no grasp named graspName
@@ -139,6 +163,7 @@ namespace hpp {
private:
typedef std::map <const std::string, DevicePtr_t> RobotsandObjects_t;
RobotPtr_t robot_;
+ graph::GraphPtr_t constraintGraph_;
/// Map of single robots to store before building a composite robot.
RobotsandObjects_t robotsAndObjects_;
GraspsMap_t graspsMap_;
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 2e48ae62..b0f55286 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -17,11 +17,14 @@
#include <hpp/util/pointer.hh>
#include <hpp/util/debug.hh>
-#include <hpp/manipulation/object.hh>
-#include <hpp/manipulation/problem-solver.hh>
-#include <hpp/manipulation/robot.hh>
#include <hpp/model/gripper.hh>
+#include "hpp/manipulation/object.hh"
+#include "hpp/manipulation/robot.hh"
+#include "hpp/manipulation/graph/graph.hh"
+#include "hpp/manipulation/manipulation-planner.hh"
+
+#include "hpp/manipulation/problem-solver.hh"
namespace hpp {
namespace manipulation {
@@ -69,6 +72,16 @@ namespace hpp {
return object;
}
+ void ProblemSolver::constraintGraph (const graph::GraphPtr_t& graph)
+ {
+ constraintGraph_ = graph;
+ }
+
+ graph::GraphPtr_t ProblemSolver::constraintGraph () const
+ {
+ return constraintGraph_;
+ }
+
LockedDofPtr_t ProblemSolver::lockedDofConstraint (const std::string& name) const
{
LockedDofConstraintMap_t::const_iterator it =
@@ -94,10 +107,10 @@ namespace hpp {
void ProblemSolver::resetConstraints ()
{
if (robot_)
- constraints_ = ConstraintSet::create (robot_,
- "Default constraint set");
+ constraints_ = ConstraintSet::create (robot_,
+ "Default constraint set");
GraspsMap_t graspsMap = grasps();
- for (GraspsMap_t::const_iterator itGrasp = graspsMap.begin() ;
+ for (GraspsMap_t::const_iterator itGrasp = graspsMap.begin();
itGrasp != graspsMap.end() ; itGrasp++) {
GraspPtr_t grasp = itGrasp->second;
GripperPtr_t gripper = grasp->first;
@@ -111,7 +124,7 @@ namespace hpp {
}
}
}
-
+
void ProblemSolver::addConstraintToConfigProjector (
const std::string& constraintName,
const DifferentiableFunctionPtr_t& constraint)
@@ -132,5 +145,27 @@ namespace hpp {
}
}
}
+
+ bool ProblemSolver::prepareSolveStepByStep ()
+ {
+ ManipulationPlannerPtr_t manipPlanner =
+ HPP_DYNAMIC_PTR_CAST (ManipulationPlanner, pathPlanner ());
+ if (manipPlanner)
+ manipPlanner->constraintGraph (constraintGraph_);
+ else
+ hppDout (warning, "The planner is not a manipulation planner.");
+ return core::ProblemSolver::prepareSolveStepByStep ();
+ }
+
+ void ProblemSolver::solve ()
+ {
+ ManipulationPlannerPtr_t manipPlanner =
+ HPP_DYNAMIC_PTR_CAST (ManipulationPlanner, pathPlanner ());
+ if (manipPlanner)
+ manipPlanner->constraintGraph (constraintGraph_);
+ else
+ hppDout (warning, "The planner is not a manipulation planner.");
+ core::ProblemSolver::solve ();
+ }
} // namespace manipulation
} // namespace hpp
--
GitLab