From 3acff2d61ff7f11b5d43ae2a7fd073c879e8d645 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Mon, 14 Dec 2020 07:42:59 +0000
Subject: [PATCH] Update to modifications in hpp-core
- methods formerly taking a const reference to Problem as input now take
a shared pointer to const Problem.
---
include/hpp/manipulation/fwd.hh | 1 +
.../hpp/manipulation/graph-node-optimizer.hh | 5 +-
include/hpp/manipulation/graph-optimizer.hh | 8 ++-
.../hpp/manipulation/manipulation-planner.hh | 7 ++-
.../enforce-transition-semantic.hh | 8 +--
.../path-optimization/random-shortcut.hh | 5 +-
.../path-optimization/small-steps.hh | 4 +-
.../spline-gradient-based.hh | 6 +-
.../path-planner/end-effector-trajectory.hh | 11 ++--
.../cross-state-optimization.hh | 14 +++--
.../end-effector-trajectory.hh | 7 ++-
.../hpp/manipulation/steering-method/graph.hh | 15 ++---
src/graph-node-optimizer.cc | 4 +-
src/graph-optimizer.cc | 10 +--
src/manipulation-planner.cc | 61 ++++++++++---------
src/path-optimization/small-steps.cc | 9 +--
.../spline-gradient-based.cc | 21 ++++---
src/path-planner/end-effector-trajectory.cc | 35 ++++++-----
src/problem-solver.cc | 14 +++--
src/problem.cc | 2 +-
.../cross-state-optimization.cc | 29 ++++-----
.../end-effector-trajectory.cc | 7 ++-
src/steering-method/graph.cc | 24 +++-----
tests/test-constraintgraph.cc | 2 +-
24 files changed, 164 insertions(+), 145 deletions(-)
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index b315943..35e0ade 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -53,6 +53,7 @@ namespace hpp {
typedef ProblemSolver* ProblemSolverPtr_t;
HPP_PREDEF_CLASS (Problem);
typedef boost::shared_ptr <Problem> ProblemPtr_t;
+ typedef boost::shared_ptr <const Problem> ProblemConstPtr_t;
HPP_PREDEF_CLASS (Roadmap);
typedef boost::shared_ptr <Roadmap> RoadmapPtr_t;
HPP_PREDEF_CLASS (RoadmapNode);
diff --git a/include/hpp/manipulation/graph-node-optimizer.hh b/include/hpp/manipulation/graph-node-optimizer.hh
index cd810ee..62d2665 100644
--- a/include/hpp/manipulation/graph-node-optimizer.hh
+++ b/include/hpp/manipulation/graph-node-optimizer.hh
@@ -46,13 +46,14 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI GraphNodeOptimizer : public PathOptimizer
{
public:
- static GraphNodeOptimizerPtr_t create (const core::Problem& problem);
+ static GraphNodeOptimizerPtr_t create
+ (const core::ProblemConstPtr_t& problem);
virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
protected:
/// Constructor
- GraphNodeOptimizer (const core::Problem& problem) :
+ GraphNodeOptimizer (const core::ProblemConstPtr_t& problem) :
PathOptimizer (problem)
{}
diff --git a/include/hpp/manipulation/graph-optimizer.hh b/include/hpp/manipulation/graph-optimizer.hh
index 28dbc7c..fc783c5 100644
--- a/include/hpp/manipulation/graph-optimizer.hh
+++ b/include/hpp/manipulation/graph-optimizer.hh
@@ -46,7 +46,8 @@ namespace hpp {
typedef core::PathOptimizerBuilder_t PathOptimizerBuilder_t;
template <typename TraitsOrInnerType>
- static GraphOptimizerPtr_t create (const core::Problem& problem);
+ static GraphOptimizerPtr_t create
+ (const core::ProblemConstPtr_t& problem);
virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
@@ -58,7 +59,8 @@ namespace hpp {
protected:
/// Constructor
- GraphOptimizer (const core::Problem& problem, PathOptimizerBuilder_t factory) :
+ GraphOptimizer (const core::ProblemConstPtr_t& problem,
+ PathOptimizerBuilder_t factory) :
PathOptimizer (problem), factory_ (factory), pathOptimizer_ ()
{}
@@ -73,7 +75,7 @@ namespace hpp {
/// Member function definition
template <typename TraitsOrInnerType>
GraphOptimizerPtr_t GraphOptimizer::create
- (const core::Problem& problem)
+ (const core::ProblemConstPtr_t& problem)
{
return GraphOptimizerPtr_t (
new GraphOptimizer (problem, TraitsOrInnerType::create)
diff --git a/include/hpp/manipulation/manipulation-planner.hh b/include/hpp/manipulation/manipulation-planner.hh
index e510d2b..a14d01b 100644
--- a/include/hpp/manipulation/manipulation-planner.hh
+++ b/include/hpp/manipulation/manipulation-planner.hh
@@ -40,7 +40,8 @@ namespace hpp {
typedef std::list<std::size_t> ErrorFreqs_t;
/// Create an instance and return a shared pointer to the instance
- static ManipulationPlannerPtr_t create (const core::Problem& problem,
+ static ManipulationPlannerPtr_t create
+ (const core::ProblemConstPtr_t& problem,
const core::RoadmapPtr_t& roadmap);
/// One step of extension.
@@ -71,7 +72,7 @@ namespace hpp {
protected:
/// Protected constructor
- ManipulationPlanner (const Problem& problem,
+ ManipulationPlanner (const ProblemConstPtr_t& problem,
const RoadmapPtr_t& roadmap);
/// Store weak pointer to itself
@@ -88,7 +89,7 @@ namespace hpp {
/// Configuration shooter
ConfigurationShooterPtr_t shooter_;
/// Pointer to the problem
- const Problem& problem_;
+ ProblemConstPtr_t problem_;
/// Pointer to the roadmap
RoadmapPtr_t roadmap_;
/// weak pointer to itself
diff --git a/include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh b/include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh
index de21802..f50da1b 100644
--- a/include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh
+++ b/include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh
@@ -36,8 +36,8 @@ namespace hpp {
typedef hpp::core::PathVectorPtr_t PathVectorPtr_t;
typedef boost::shared_ptr<EnforceTransitionSemantic> Ptr_t;
- static Ptr_t create (const core::Problem& problem) {
- const Problem& p = dynamic_cast <const Problem&> (problem);
+ static Ptr_t create (const core::ProblemConstPtr_t& problem) {
+ ProblemConstPtr_t p (HPP_DYNAMIC_PTR_CAST(const Problem, problem));
return Ptr_t (new EnforceTransitionSemantic (p));
}
@@ -45,11 +45,11 @@ namespace hpp {
protected:
/// Constructor
- EnforceTransitionSemantic (const Problem& problem) :
+ EnforceTransitionSemantic (const ProblemConstPtr_t& problem) :
PathOptimizer (problem), problem_ (problem) {}
private:
- const Problem& problem_;
+ ProblemConstPtr_t problem_;
};
/// \}
diff --git a/include/hpp/manipulation/path-optimization/random-shortcut.hh b/include/hpp/manipulation/path-optimization/random-shortcut.hh
index 7b77789..b95767f 100644
--- a/include/hpp/manipulation/path-optimization/random-shortcut.hh
+++ b/include/hpp/manipulation/path-optimization/random-shortcut.hh
@@ -35,13 +35,14 @@ namespace hpp {
{
public:
/// Return shared pointer to new object.
- static RandomShortcutPtr_t create (const core::Problem& problem)
+ static RandomShortcutPtr_t create
+ (const core::ProblemConstPtr_t problem)
{
return RandomShortcutPtr_t (new RandomShortcut (problem));
}
protected:
- RandomShortcut (const core::Problem& problem)
+ RandomShortcut (const core::ProblemConstPtr_t& problem)
: core::pathOptimization::RandomShortcut (problem)
{}
diff --git a/include/hpp/manipulation/path-optimization/small-steps.hh b/include/hpp/manipulation/path-optimization/small-steps.hh
index 65950de..b134155 100644
--- a/include/hpp/manipulation/path-optimization/small-steps.hh
+++ b/include/hpp/manipulation/path-optimization/small-steps.hh
@@ -39,7 +39,7 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI SmallSteps : public PathOptimizer
{
public:
- static SmallStepsPtr_t create (const core::Problem& problem)
+ static SmallStepsPtr_t create (const core::ProblemConstPtr_t& problem)
{
SmallSteps* ptr (new SmallSteps (problem));
return SmallStepsPtr_t (ptr);
@@ -49,7 +49,7 @@ namespace hpp {
protected:
/// Constructor
- SmallSteps (const core::Problem& problem) :
+ SmallSteps (const core::ProblemConstPtr_t& problem) :
PathOptimizer (problem)
{}
};
diff --git a/include/hpp/manipulation/path-optimization/spline-gradient-based.hh b/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
index dea847e..6af8561 100644
--- a/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
+++ b/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
@@ -45,19 +45,19 @@ namespace hpp {
using typename Parent_t::Splines_t;
/// Return shared pointer to new object.
- static Ptr_t create (const Problem& problem);
+ static Ptr_t create (const ProblemConstPtr_t& problem);
/// This is only for compatibility purpose (with ProblemSolver).
/// problem is statically casted to an object of type
/// const manipulation::Problem& and method create(const Problem&)
/// is called.
- static Ptr_t createFromCore (const core::Problem& problem);
+ static Ptr_t createFromCore (const core::ProblemConstPtr_t& problem);
protected:
typedef typename hpp::core::pathOptimization::LinearConstraint LinearConstraint;
using typename Parent_t::SplineOptimizationDatas_t;
- SplineGradientBased (const Problem& problem);
+ SplineGradientBased(const ProblemConstPtr_t& problem);
/// Get path validation for each spline
///
diff --git a/include/hpp/manipulation/path-planner/end-effector-trajectory.hh b/include/hpp/manipulation/path-planner/end-effector-trajectory.hh
index e39dd0f..133601a 100644
--- a/include/hpp/manipulation/path-planner/end-effector-trajectory.hh
+++ b/include/hpp/manipulation/path-planner/end-effector-trajectory.hh
@@ -53,12 +53,14 @@ namespace hpp {
public:
/// Return shared pointer to new instance
/// \param problem the path planning problem
- static EndEffectorTrajectoryPtr_t create (const core::Problem& problem);
+ static EndEffectorTrajectoryPtr_t create
+ (const core::ProblemConstPtr_t& problem);
/// Return shared pointer to new instance
/// \param problem the path planning problem
/// \param roadmap previously built roadmap
static EndEffectorTrajectoryPtr_t createWithRoadmap
- (const core::Problem& problem, const core::RoadmapPtr_t& roadmap);
+ (const core::ProblemConstPtr_t& problem,
+ const core::RoadmapPtr_t& roadmap);
/// Initialize the problem resolution
/// \li call parent implementation
@@ -108,11 +110,12 @@ namespace hpp {
protected:
/// Protected constructor
/// \param problem the path planning problem
- EndEffectorTrajectory (const core::Problem& problem);
+ EndEffectorTrajectory (const core::ProblemConstPtr_t& problem);
/// Protected constructor
/// \param problem the path planning problem
/// \param roadmap previously built roadmap
- EndEffectorTrajectory (const core::Problem& problem, const core::RoadmapPtr_t& roadmap);
+ EndEffectorTrajectory (const core::ProblemConstPtr_t& problem,
+ const core::RoadmapPtr_t& roadmap);
/// Store weak pointer to itself
void init (const EndEffectorTrajectoryWkPtr_t& weak);
diff --git a/include/hpp/manipulation/steering-method/cross-state-optimization.hh b/include/hpp/manipulation/steering-method/cross-state-optimization.hh
index 1c188d5..376515b 100644
--- a/include/hpp/manipulation/steering-method/cross-state-optimization.hh
+++ b/include/hpp/manipulation/steering-method/cross-state-optimization.hh
@@ -89,20 +89,21 @@ namespace hpp {
public:
struct OptimizationData;
- static CrossStateOptimizationPtr_t create (const Problem& problem);
+ static CrossStateOptimizationPtr_t create
+ (const ProblemConstPtr_t& problem);
/// \warning core::Problem will be casted to Problem
static CrossStateOptimizationPtr_t create
- (const core::Problem& problem);
+ (const core::ProblemConstPtr_t& problem);
template <typename T>
static CrossStateOptimizationPtr_t create
- (const core::Problem& problem);
+ (const core::ProblemConstPtr_t& problem);
core::SteeringMethodPtr_t copy () const;
protected:
- CrossStateOptimization (const Problem& problem) :
+ CrossStateOptimization (const ProblemConstPtr_t& problem) :
SteeringMethod (problem),
sameRightHandSide_ ()
{
@@ -167,9 +168,10 @@ namespace hpp {
template <typename T>
CrossStateOptimizationPtr_t CrossStateOptimization::create
- (const core::Problem& problem)
+ (const core::ProblemConstPtr_t& problem)
{
- CrossStateOptimizationPtr_t gsm = CrossStateOptimization::create (problem);
+ CrossStateOptimizationPtr_t gsm = CrossStateOptimization::create
+ (problem);
gsm->innerSteeringMethod (T::create (problem));
return gsm;
}
diff --git a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
index dd04146..542b8f9 100644
--- a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
+++ b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
@@ -39,9 +39,10 @@ namespace hpp {
public:
typedef core::interval_t interval_t;
- static EndEffectorTrajectoryPtr_t create (const core::Problem& problem)
+ static EndEffectorTrajectoryPtr_t create
+ (const core::ProblemConstPtr_t& problem)
{
- EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory (problem));
+ EndEffectorTrajectoryPtr_t ptr(new EndEffectorTrajectory (problem));
ptr->init(ptr);
return ptr;
}
@@ -95,7 +96,7 @@ namespace hpp {
PathPtr_t projectedPath (vectorIn_t times, matrixIn_t configs) const;
protected:
- EndEffectorTrajectory (const core::Problem& problem)
+ EndEffectorTrajectory (const core::ProblemConstPtr_t& problem)
: core::SteeringMethod (problem)
{}
diff --git a/include/hpp/manipulation/steering-method/graph.hh b/include/hpp/manipulation/steering-method/graph.hh
index b3145b7..bcaba19 100644
--- a/include/hpp/manipulation/steering-method/graph.hh
+++ b/include/hpp/manipulation/steering-method/graph.hh
@@ -45,7 +45,7 @@ namespace hpp {
protected:
/// Constructor
- SteeringMethod (const Problem& problem);
+ SteeringMethod (const ProblemConstPtr_t& problem);
/// Copy constructor
SteeringMethod (const SteeringMethod& other);
@@ -56,7 +56,7 @@ namespace hpp {
}
/// A pointer to the manipulation problem
- const Problem& problem_;
+ ProblemConstPtr_t problem_;
/// The encapsulated steering method
core::SteeringMethodPtr_t steeringMethod_;
};
@@ -72,14 +72,11 @@ namespace hpp {
/// Create instance and return shared pointer
/// \warning core::Problem will be casted to Problem
static GraphPtr_t create
- (const core::Problem& problem);
+ (const core::ProblemConstPtr_t& problem);
template <typename T>
static GraphPtr_t create
- (const core::Problem& problem);
-
- /// Create instance and return shared pointer
- static GraphPtr_t create (const Problem& problem);
+ (const core::ProblemConstPtr_t& problem);
/// Create copy and return shared pointer
static GraphPtr_t createCopy
@@ -93,7 +90,7 @@ namespace hpp {
protected:
/// Constructor
- Graph (const Problem& problem);
+ Graph (const ProblemConstPtr_t& problem);
/// Copy constructor
Graph (const Graph&);
@@ -113,7 +110,7 @@ namespace hpp {
template <typename T>
GraphPtr_t Graph::create
- (const core::Problem& problem)
+ (const core::ProblemConstPtr_t& problem)
{
GraphPtr_t gsm = Graph::create (problem);
gsm->innerSteeringMethod (T::create (problem));
diff --git a/src/graph-node-optimizer.cc b/src/graph-node-optimizer.cc
index 4ddf44b..c476bf0 100644
--- a/src/graph-node-optimizer.cc
+++ b/src/graph-node-optimizer.cc
@@ -21,7 +21,7 @@
namespace hpp {
namespace manipulation {
GraphNodeOptimizerPtr_t GraphNodeOptimizer::create
- (const core::Problem& problem)
+ (const core::ProblemConstPtr_t& problem)
{
GraphNodeOptimizer* ptr = new GraphNodeOptimizer (problem);
return GraphNodeOptimizerPtr_t (ptr);
@@ -29,7 +29,7 @@ namespace hpp {
PathVectorPtr_t GraphNodeOptimizer::optimize (const PathVectorPtr_t& path)
{
- core::Problem& p = const_cast <core::Problem&> (this->problem ());
+ core::ProblemPtr_t p = const_cast <core::ProblemPtr_t> (this->problem ());
core::SteeringMethodPtr_t sm = p.steeringMethod ();
/// Start by flattening the path
diff --git a/src/graph-optimizer.cc b/src/graph-optimizer.cc
index c977884..62ca44c 100644
--- a/src/graph-optimizer.cc
+++ b/src/graph-optimizer.cc
@@ -35,7 +35,7 @@ namespace hpp {
(path->outputSize(), path->outputDerivativeSize()),
toConcat;
GraphPathValidationPtr_t gpv = HPP_DYNAMIC_PTR_CAST (GraphPathValidation,
- this->problem().pathValidation ());
+ this->problem()->pathValidation ());
path->flatten (expanded);
ConstraintSetPtr_t c;
@@ -65,18 +65,18 @@ namespace hpp {
if (isShort)
toConcat = toOpt;
else {
- core::ProblemPtr_t p = core::Problem::create (problem().robot());
- p->distance(problem().distance());
+ core::ProblemPtr_t p = core::Problem::create (problem()->robot());
+ p->distance(problem()->distance());
// It should be ok to use the path validation of each edge because it
// corresponds to the global path validation minus the collision pairs
// disabled using the edge constraint.
// p.pathValidation(gpv->innerValidation());
- p->pathProjector(problem().pathProjector());
+ p->pathProjector(problem()->pathProjector());
p->steeringMethod(edge->steeringMethod()->copy());
p->constraints(p->steeringMethod()->constraints());
p->constraints()->configProjector()->rightHandSideFromConfig(toOpt->initial());
p->pathValidation(edge->pathValidation());
- pathOptimizer_ = factory_ (*p);
+ pathOptimizer_ = factory_ (p);
toConcat = pathOptimizer_->optimize (toOpt);
}
i_s = i_e;
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 3e13a38..c50aa83 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -109,22 +109,22 @@ namespace hpp {
(SuccessBin::createReason ("--Path validation returned length 0")) // PATH_VALIDATION_ZERO = 6,
(SuccessBin::createReason ("--Path could not be projected at all")); // PATH_PROJECTION_ZERO = 7
- ManipulationPlannerPtr_t ManipulationPlanner::create (const core::Problem& problem,
- const core::RoadmapPtr_t& roadmap)
+ ManipulationPlannerPtr_t ManipulationPlanner::create
+ (const core::ProblemConstPtr_t& problem,
+ const core::RoadmapPtr_t& roadmap)
{
ManipulationPlanner* ptr;
core::RoadmapPtr_t r2 = roadmap;
- RoadmapPtr_t r;
- try {
- const Problem& p = dynamic_cast < const Problem& > (problem);
- RoadmapPtr_t r = HPP_DYNAMIC_PTR_CAST (Roadmap, r2);
- ptr = new ManipulationPlanner (p, r);
- } catch (std::exception&) {
- if (!r)
- throw std::invalid_argument ("The roadmap must be of type hpp::manipulation::Roadmap.");
- else
- throw std::invalid_argument ("The problem must be of type hpp::manipulation::Problem.");
- }
+ ProblemConstPtr_t p = HPP_DYNAMIC_PTR_CAST (const Problem, problem);
+ RoadmapPtr_t r = HPP_DYNAMIC_PTR_CAST (Roadmap, r2);
+ if (!r)
+ throw std::invalid_argument
+ ("The roadmap must be of type hpp::manipulation::Roadmap.");
+ if (!p)
+ throw std::invalid_argument
+ ("The problem must be of type hpp::manipulation::Problem.");
+
+ ptr = new ManipulationPlanner (p, r);
ManipulationPlannerPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
@@ -160,9 +160,9 @@ namespace hpp {
{
HPP_START_TIMECOUNTER(oneStep);
- DevicePtr_t robot = HPP_DYNAMIC_PTR_CAST(Device, problem ().robot ());
+ DevicePtr_t robot = HPP_DYNAMIC_PTR_CAST(Device, problem()->robot ());
HPP_ASSERT(robot);
- const graph::States_t& graphStates = problem_.constraintGraph ()
+ const graph::States_t& graphStates = problem_->constraintGraph ()
->stateSelector ()->getStates ();
graph::States_t::const_iterator itState;
core::Nodes_t newNodes;
@@ -252,9 +252,9 @@ namespace hpp {
const ConfigurationPtr_t& q_rand,
core::PathPtr_t& validPath)
{
- graph::GraphPtr_t graph = problem_.constraintGraph ();
- PathProjectorPtr_t pathProjector = problem_.pathProjector ();
- pinocchio::DevicePtr_t robot (problem_.robot ());
+ graph::GraphPtr_t graph = problem_->constraintGraph ();
+ PathProjectorPtr_t pathProjector = problem_->pathProjector ();
+ pinocchio::DevicePtr_t robot (problem_->robot ());
value_type eps (graph->errorThreshold ());
// Select next node in the constraint graph.
const ConfigurationPtr_t q_near = n_near->configuration ();
@@ -303,7 +303,7 @@ namespace hpp {
}
HPP_STOP_TIMECOUNTER (projectPath);
} else projPath = path;
- PathValidationPtr_t pathValidation (problem_.pathValidation ());
+ PathValidationPtr_t pathValidation (problem_->pathValidation ());
PathValidationReportPtr_t report;
core::PathPtr_t fullValidPath;
HPP_START_TIMECOUNTER (validatePath);
@@ -373,9 +373,9 @@ namespace hpp {
inline std::size_t ManipulationPlanner::tryConnectToRoadmap (const core::Nodes_t nodes)
{
- PathProjectorPtr_t pathProjector (problem().pathProjector ());
+ PathProjectorPtr_t pathProjector (problem()->pathProjector ());
core::PathPtr_t path;
- graph::GraphPtr_t graph = problem_.constraintGraph ();
+ graph::GraphPtr_t graph = problem_->constraintGraph ();
graph::Edges_t possibleEdges;
bool connectSucceed = false;
@@ -404,7 +404,8 @@ namespace hpp {
graph::StatePtr_t s2 = getState (graph, *itn2);
assert (q1 != q2);
- path = connect (q1, q2, s1, s2, graph, pathProjector, problem_.pathValidation());
+ path = connect (q1, q2, s1, s2, graph, pathProjector,
+ problem_->pathValidation());
if (path) {
nbConnection++;
@@ -427,9 +428,9 @@ namespace hpp {
inline std::size_t ManipulationPlanner::tryConnectNewNodes (const core::Nodes_t nodes)
{
- PathProjectorPtr_t pathProjector (problem().pathProjector ());
+ PathProjectorPtr_t pathProjector (problem()->pathProjector ());
core::PathPtr_t path;
- graph::GraphPtr_t graph = problem_.constraintGraph ();
+ graph::GraphPtr_t graph = problem_->constraintGraph ();
std::size_t nbConnection = 0;
for (core::Nodes_t::const_iterator itn1 = nodes.begin ();
itn1 != nodes.end (); ++itn1) {
@@ -447,7 +448,8 @@ namespace hpp {
graph::StatePtr_t s2 = getState (graph, *itn2);
assert (q1 != q2);
- path = connect (q1, q2, s1, s2, graph, pathProjector, problem_.pathValidation());
+ path = connect (q1, q2, s1, s2, graph, pathProjector,
+ problem_->pathValidation());
if (path) {
nbConnection++;
if (!_1to2) roadmap ()->addEdge (*itn1, *itn2, path);
@@ -463,13 +465,14 @@ namespace hpp {
return nbConnection;
}
- ManipulationPlanner::ManipulationPlanner (const Problem& problem,
+ ManipulationPlanner::ManipulationPlanner (const ProblemConstPtr_t& problem,
const RoadmapPtr_t& roadmap) :
core::PathPlanner (problem, roadmap),
- shooter_ (problem.configurationShooter()),
+ shooter_ (problem->configurationShooter()),
problem_ (problem), roadmap_ (roadmap),
- extendStep_ (problem.getParameter("ManipulationPlanner/extendStep").floatValue()),
- qProj_ (problem.robot ()->configSize ())
+ extendStep_ (problem->getParameter
+ ("ManipulationPlanner/extendStep").floatValue()),
+ qProj_ (problem->robot ()->configSize ())
{}
void ManipulationPlanner::init (const ManipulationPlannerWkPtr_t& weak)
diff --git a/src/path-optimization/small-steps.cc b/src/path-optimization/small-steps.cc
index 0ae7bae..9689c66 100644
--- a/src/path-optimization/small-steps.cc
+++ b/src/path-optimization/small-steps.cc
@@ -38,9 +38,10 @@ namespace hpp {
toConcat;
path->flatten (flat);
- GraphPathValidationPtr_t gpv = HPP_DYNAMIC_PTR_CAST (GraphPathValidation,
- this->problem().pathValidation ());
- const_cast <core::Problem&>(this->problem ()).pathValidation (gpv->innerValidation());
+ GraphPathValidationPtr_t gpv(HPP_DYNAMIC_PTR_CAST(GraphPathValidation,
+ this->problem()->pathValidation()));
+ const_cast<core::Problem&>(*this->problem()).pathValidation
+ (gpv->innerValidation());
wholebodyStep::SmallStepsPtr_t stepPtr
(wholebodyStep::SmallSteps::create(problem()));
@@ -69,7 +70,7 @@ namespace hpp {
opted->concatenate (toConcat);
}
- const_cast <core::Problem&>(this->problem ()).pathValidation (gpv);
+ const_cast<core::Problem&>(*this->problem ()).pathValidation (gpv);
return opted;
}
} // namespace pathOptimization
diff --git a/src/path-optimization/spline-gradient-based.cc b/src/path-optimization/spline-gradient-based.cc
index 0e6cf91..592d878 100644
--- a/src/path-optimization/spline-gradient-based.cc
+++ b/src/path-optimization/spline-gradient-based.cc
@@ -27,8 +27,8 @@ namespace hpp {
namespace pathOptimization {
template <int _PB, int _SO>
- SplineGradientBased<_PB, _SO>::SplineGradientBased (const Problem& problem)
- : Parent_t (problem)
+ SplineGradientBased<_PB, _SO>::SplineGradientBased
+ (const ProblemConstPtr_t& problem) : Parent_t (problem)
{
this->checkOptimum_ = true;
}
@@ -39,7 +39,7 @@ namespace hpp {
template <int _PB, int _SO>
typename SplineGradientBased<_PB, _SO>::Ptr_t SplineGradientBased<_PB, _SO>::create
- (const Problem& problem)
+ (const ProblemConstPtr_t& problem)
{
SplineGradientBased* ptr = new SplineGradientBased (problem);
Ptr_t shPtr (ptr);
@@ -48,10 +48,10 @@ namespace hpp {
template <int _PB, int _SO>
typename SplineGradientBased<_PB, _SO>::Ptr_t SplineGradientBased<_PB, _SO>::createFromCore
- (const core::Problem& problem)
+ (const core::ProblemConstPtr_t& problem)
{
- HPP_STATIC_CAST_REF_CHECK(const Problem, problem);
- return create (static_cast<const Problem&>(problem));
+ assert(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
+ return create (HPP_STATIC_PTR_CAST(const Problem, problem));
}
template <int _PB, int _SO>
@@ -64,7 +64,7 @@ namespace hpp {
if (set && set->edge())
this->validations_[i] = set->edge()->pathValidation();
else
- this->validations_[i] = this->problem().pathValidation();
+ this->validations_[i] = this->problem()->pathValidation();
}
}
@@ -74,7 +74,9 @@ namespace hpp {
{
assert (init->numberPaths() == splines.size() && sods.size() == splines.size());
- bool zeroDerivative = this->problem().getParameter ("SplineGradientBased/zeroDerivativesAtStateIntersection").boolValue();
+ bool zeroDerivative = this->problem()->getParameter
+ ("SplineGradientBased/zeroDerivativesAtStateIntersection").
+ boolValue();
const std::size_t last = splines.size() - 1;
graph::StatePtr_t stateOfStart;
@@ -179,7 +181,8 @@ namespace hpp {
// TODO Should we add zero velocity sometimes ?
ConstraintSetPtr_t set = state->configConstraint();
- value_type guessThreshold = this->problem().getParameter ("SplineGradientBased/guessThreshold").floatValue();
+ value_type guessThreshold = this->problem()->getParameter
+ ("SplineGradientBased/guessThreshold").floatValue();
Eigen::RowBlockIndices select =
this->computeActiveParameters (path, set->configProjector()->solver(), guessThreshold, true);
hppDout (info, "End of path " << idxSpline << ": do not change this dof " << select);
diff --git a/src/path-planner/end-effector-trajectory.cc b/src/path-planner/end-effector-trajectory.cc
index 13df4ba..a898f5f 100644
--- a/src/path-planner/end-effector-trajectory.cc
+++ b/src/path-planner/end-effector-trajectory.cc
@@ -41,7 +41,8 @@ namespace hpp {
typedef manipulation::steeringMethod::EndEffectorTrajectory SM_t;
typedef manipulation::steeringMethod::EndEffectorTrajectoryPtr_t SMPtr_t;
- EndEffectorTrajectoryPtr_t EndEffectorTrajectory::create (const core::Problem& problem)
+ EndEffectorTrajectoryPtr_t EndEffectorTrajectory::create
+ (const core::ProblemConstPtr_t& problem)
{
EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory(problem));
ptr->init(ptr);
@@ -49,9 +50,11 @@ namespace hpp {
}
EndEffectorTrajectoryPtr_t EndEffectorTrajectory::createWithRoadmap (
- const core::Problem& problem, const core::RoadmapPtr_t& roadmap)
+ const core::ProblemConstPtr_t& problem,
+ const core::RoadmapPtr_t& roadmap)
{
- EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory(problem, roadmap));
+ EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory(problem,
+ roadmap));
ptr->init(ptr);
return ptr;
}
@@ -63,13 +66,13 @@ namespace hpp {
{
//core::PathPlanner::startSolve();
//problem().checkProblem ();
- if (!problem().robot ()) {
+ if (!problem()->robot ()) {
std::string msg ("No device in problem.");
hppDout (error, msg);
throw std::runtime_error (msg);
}
- if (!problem().initConfig ()) {
+ if (!problem()->initConfig ()) {
std::string msg ("No init config in problem.");
hppDout (error, msg);
throw std::runtime_error (msg);
@@ -78,7 +81,7 @@ namespace hpp {
// Tag init and goal configurations in the roadmap
roadmap()->resetGoalNodes ();
- SMPtr_t sm (HPP_DYNAMIC_PTR_CAST (SM_t, problem().steeringMethod()));
+ SMPtr_t sm (HPP_DYNAMIC_PTR_CAST (SM_t, problem()->steeringMethod()));
if (!sm)
throw std::invalid_argument ("Steering method must be of type hpp::manipulation::steeringMethod::EndEffectorTrajectory");
@@ -119,7 +122,7 @@ namespace hpp {
void EndEffectorTrajectory::oneStep ()
{
- SMPtr_t sm (HPP_DYNAMIC_PTR_CAST (SM_t, problem().steeringMethod()));
+ SMPtr_t sm (HPP_DYNAMIC_PTR_CAST (SM_t, problem()->steeringMethod()));
if (!sm)
throw std::invalid_argument ("Steering method must be of type hpp::manipulation::steeringMethod::EndEffectorTrajectory");
if (!sm->trajectoryConstraint ())
@@ -131,14 +134,14 @@ namespace hpp {
throw std::invalid_argument ("Steering method constraint has no ConfigProjector.");
core::ConfigProjectorPtr_t constraints (sm->constraints()->configProjector());
- core::ConfigValidationPtr_t cfgValidation (problem().configValidations());
- core:: PathValidationPtr_t pathValidation (problem().pathValidation());
+ core::ConfigValidationPtr_t cfgValidation (problem()->configValidations());
+ core:: PathValidationPtr_t pathValidation (problem()->pathValidation());
core:: ValidationReportPtr_t cfgReport;
core::PathValidationReportPtr_t pathReport;
core::interval_t timeRange (sm->timeRange());
- std::vector<core::Configuration_t> qs (configurations(*problem().initConfig()));
+ std::vector<core::Configuration_t> qs (configurations(*problem()->initConfig()));
if (qs.empty()) {
hppDout (info, "Failed to generate initial configs.");
return;
@@ -150,7 +153,7 @@ namespace hpp {
std::size_t i;
vector_t times (nDiscreteSteps_+1);
- matrix_t steps (problem().robot()->configSize(), nDiscreteSteps_+1);
+ matrix_t steps (problem()->robot()->configSize(), nDiscreteSteps_+1);
times[0] = timeRange.first;
for (int j = 1; j < nDiscreteSteps_; ++j)
@@ -221,7 +224,7 @@ namespace hpp {
std::vector<core::Configuration_t> configs(nRandomConfig_ + 1);
configs[0] = q_init;
for (int i = 1; i < nRandomConfig_ + 1; ++i)
- problem().configurationShooter()->shoot(configs[i]);
+ problem()->configurationShooter()->shoot(configs[i]);
return configs;
}
@@ -231,11 +234,13 @@ namespace hpp {
throw std::runtime_error ("Using an IkSolverInitialization is not implemented yet");
}
- EndEffectorTrajectory::EndEffectorTrajectory (const core::Problem& problem)
- : core::PathPlanner (problem)
+ EndEffectorTrajectory::EndEffectorTrajectory
+ (const core::ProblemConstPtr_t& problem) : core::PathPlanner (problem)
{}
- EndEffectorTrajectory::EndEffectorTrajectory (const core::Problem& problem, const core::RoadmapPtr_t& roadmap)
+ EndEffectorTrajectory::EndEffectorTrajectory
+ (const core::ProblemConstPtr_t& problem,
+ const core::RoadmapPtr_t& roadmap)
: core::PathPlanner (problem, roadmap)
{}
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 33b6f3c..fadf04c 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -91,7 +91,7 @@ namespace hpp {
template <typename ParentSM_t, typename ChildSM_t>
core::SteeringMethodPtr_t createSMWithGuess
- (const core::Problem& problem)
+ (const core::ProblemConstPtr_t& problem)
{
boost::shared_ptr<ParentSM_t> sm = ParentSM_t::create (problem);
sm->innerSteeringMethod (ChildSM_t::createWithGuess (problem));
@@ -100,13 +100,15 @@ namespace hpp {
template <typename PathProjectorType>
core::PathProjectorPtr_t createPathProjector
- (const core::Problem& problem, const value_type& step)
+ (const core::ProblemConstPtr_t& problem, const value_type& step)
{
steeringMethod::GraphPtr_t gsm =
- HPP_DYNAMIC_PTR_CAST (steeringMethod::Graph, problem.steeringMethod());
- if (!gsm) throw std::logic_error ("The steering method should be of type"
- " steeringMethod::Graph");
- return PathProjectorType::create (problem.distance(),
+ HPP_DYNAMIC_PTR_CAST
+ (steeringMethod::Graph, problem->steeringMethod());
+ if (!gsm) throw std::logic_error
+ ("The steering method should be of type"
+ " steeringMethod::Graph");
+ return PathProjectorType::create (problem->distance(),
gsm->innerSteeringMethod(), step);
}
}
diff --git a/src/problem.cc b/src/problem.cc
index d22119a..1a3cf39 100644
--- a/src/problem.cc
+++ b/src/problem.cc
@@ -42,7 +42,7 @@ namespace hpp {
Parent::init (wkPtr);
wkPtr_ = wkPtr;
- Parent::steeringMethod (steeringMethod::Graph::create (*this));
+ Parent::steeringMethod (steeringMethod::Graph::create (wkPtr_.lock()));
distance (WeighedDistance::create (HPP_DYNAMIC_PTR_CAST(Device, robot()), graph_));
setPathValidationFactory(core::pathValidation::createDiscretizedCollisionChecking, 0.05);
}
diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc
index 6e3afcf..e6b3d2e 100644
--- a/src/steering-method/cross-state-optimization.cc
+++ b/src/steering-method/cross-state-optimization.cc
@@ -56,18 +56,18 @@ namespace hpp {
using graph::segments_t;
CrossStateOptimizationPtr_t CrossStateOptimization::create (
- const Problem& problem)
+ const ProblemConstPtr_t& problem)
{
- CrossStateOptimizationPtr_t shPtr (new CrossStateOptimization (problem));
+ CrossStateOptimizationPtr_t shPtr(new CrossStateOptimization (problem));
shPtr->init(shPtr);
return shPtr;
}
CrossStateOptimizationPtr_t CrossStateOptimization::create (
- const core::Problem& problem)
+ const core::ProblemConstPtr_t& problem)
{
- HPP_STATIC_CAST_REF_CHECK (const Problem, problem);
- const Problem& p = static_cast <const Problem&> (problem);
+ assert(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
+ ProblemConstPtr_t p(HPP_STATIC_PTR_CAST(const Problem, problem));
return create (p);
}
@@ -150,7 +150,7 @@ namespace hpp {
std::map <ImplicitPtr_t, ImplicitPtr_t> constraintCopy, constraintOrig;
ImplicitPtr_t copy;
- GraphPtr_t cg (problem_.constraintGraph ());
+ GraphPtr_t cg (problem_->constraintGraph ());
const ConstraintsAndComplements_t& cac
(cg->constraintsAndComplements ());
for (std::size_t i = 0; i < cg->nbComponents (); ++i) {
@@ -329,7 +329,7 @@ namespace hpp {
c->rightHandSideFromConfig (d.q2);
vector_t rhsGoal (c->rightHandSide ());
// Check that right hand sides are close to each other
- value_type eps (problem_.constraintGraph ()->errorThreshold ());
+ value_type eps (problem_->constraintGraph ()->errorThreshold ());
value_type eps2 (eps * eps);
if ((rhsGoal - rhsInit).squaredNorm () > eps2) {
return false;
@@ -472,7 +472,7 @@ namespace hpp {
++index;
} // for (NumericalConstraints_t::const_iterator it
displayStatusMatrix (d.M_status, constraints_);
- graph::GraphPtr_t cg (problem_.constraintGraph ());
+ graph::GraphPtr_t cg (problem_->constraintGraph ());
// Fill solvers with graph, node and edge constraints
for (std::size_t j = 0; j < d.N; ++j) {
graph::StatePtr_t state (transitions [(std::size_t)j]->stateTo ());
@@ -569,7 +569,7 @@ namespace hpp {
using core::PathVector;
using core::PathVectorPtr_t;
- const core::DevicePtr_t& robot = problem().robot();
+ const core::DevicePtr_t& robot = problem()->robot();
PathVectorPtr_t pv = PathVector::create (
robot->configSize(), robot->numberDof());
core::PathPtr_t path;
@@ -607,12 +607,13 @@ namespace hpp {
core::PathPtr_t CrossStateOptimization::impl_compute (
ConfigurationIn_t q1, ConfigurationIn_t q2) const
{
- const graph::Graph& graph = *problem_.constraintGraph ();
+ const graph::GraphPtr_t& graph(problem_->constraintGraph ());
GraphSearchData d;
- d.s1 = graph.getState (q1);
- d.s2 = graph.getState (q2);
+ d.s1 = graph->getState (q1);
+ d.s2 = graph->getState (q2);
// d.maxDepth = 2;
- d.maxDepth = problem_.getParameter ("CrossStateOptimization/maxDepth").intValue();
+ d.maxDepth = problem_->getParameter
+ ("CrossStateOptimization/maxDepth").intValue();
// Find
d.queue1.push (d.addInitState());
@@ -630,7 +631,7 @@ namespace hpp {
hppDout (info, ss.str());
#endif // HPP_DEBUG
- OptimizationData optData (problem().robot(), q1, q2, transitions);
+ OptimizationData optData (problem()->robot(), q1, q2, transitions);
if (buildOptimizationProblem (optData, transitions)) {
if (solveOptimizationProblem (optData)) {
core::PathPtr_t path = buildPath (optData, transitions);
diff --git a/src/steering-method/end-effector-trajectory.cc b/src/steering-method/end-effector-trajectory.cc
index f452c39..5f39f2a 100644
--- a/src/steering-method/end-effector-trajectory.cc
+++ b/src/steering-method/end-effector-trajectory.cc
@@ -144,7 +144,8 @@ namespace hpp {
try {
core::ConstraintSetPtr_t c (getUpdatedConstraints());
- return core::StraightPath::create (problem().robot(), q1, q2, timeRange_, c);
+ return core::StraightPath::create
+ (problem()->robot(), q1, q2, timeRange_, c);
} catch (const std::exception& e) {
std::cout << timeRange_.first << ", " << timeRange_.second << '\n';
if (eeTraj_)
@@ -171,8 +172,8 @@ namespace hpp {
using core::InterpolatedPath;
using core::InterpolatedPathPtr_t;
- InterpolatedPathPtr_t path = InterpolatedPath::create(
- problem().robot(), configs.col(0), configs.col(N-1), timeRange_, c);
+ InterpolatedPathPtr_t path = InterpolatedPath::create
+ (problem()->robot(), configs.col(0), configs.col(N-1), timeRange_, c);
for (size_type i = 1; i < configs.cols()-1; ++i)
path->insert(times[i], configs.col(i));
diff --git a/src/steering-method/graph.cc b/src/steering-method/graph.cc
index cd07999..0e47f49 100644
--- a/src/steering-method/graph.cc
+++ b/src/steering-method/graph.cc
@@ -27,7 +27,7 @@
namespace hpp {
namespace manipulation {
- SteeringMethod::SteeringMethod (const Problem& problem) :
+ SteeringMethod::SteeringMethod (const ProblemConstPtr_t& problem) :
core::SteeringMethod (problem), problem_ (problem),
steeringMethod_ (core::SteeringMethodStraight::create (problem))
{
@@ -42,17 +42,11 @@ namespace hpp {
namespace steeringMethod {
GraphPtr_t Graph::create
- (const core::Problem& problem)
+ (const core::ProblemConstPtr_t& problem)
{
- HPP_STATIC_CAST_REF_CHECK (const Problem, problem);
- const Problem& p = static_cast <const Problem&> (problem);
- return create (p);
- }
-
- GraphPtr_t Graph::create
- (const Problem& problem)
- {
- Graph* ptr = new Graph (problem);
+ assert(HPP_DYNAMIC_PTR_CASE (const Problem, problem));
+ ProblemConstPtr_t p = HPP_STATIC_PTR_CAST(const Problem, problem);
+ Graph* ptr = new Graph (p);
GraphPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
@@ -67,7 +61,7 @@ namespace hpp {
return shPtr;
}
- Graph::Graph (const Problem& problem) :
+ Graph::Graph (const ProblemConstPtr_t& problem) :
SteeringMethod (problem), weak_ ()
{
}
@@ -84,12 +78,12 @@ namespace hpp {
// them
if (q1 == q2) {
core::SteeringMethodPtr_t sm
- (problem_.manipulationSteeringMethod()->innerSteeringMethod());
+ (problem_->manipulationSteeringMethod()->innerSteeringMethod());
return (*sm) (q1, q2);
}
- if (!problem_.constraintGraph())
+ if (!problem_->constraintGraph())
throw std::invalid_argument ("The constraint graph should be set to use the steeringMethod::Graph");
- const graph::Graph& graph = *problem_.constraintGraph ();
+ const graph::Graph& graph = *(problem_->constraintGraph ());
try {
possibleEdges = graph.getEdges
(graph.getState (q1), graph.getState (q2));
diff --git a/tests/test-constraintgraph.cc b/tests/test-constraintgraph.cc
index 8fd07b7..680a3bc 100644
--- a/tests/test-constraintgraph.cc
+++ b/tests/test-constraintgraph.cc
@@ -82,7 +82,7 @@ namespace hpp_test {
"ur5_joint_limited_robot.srdf");
}
SteeringMethodPtr_t sm
- (hpp::manipulation::steeringMethod::Graph::create (*problem));
+ (hpp::manipulation::steeringMethod::Graph::create (problem));
hpp::core::ProblemPtr_t pb (problem);
pb->steeringMethod (sm);
--
GitLab