diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index ad46e668292317eaa730ef3a8b1b112ee7c76760..59829be006f44220b0970a9e5ee6babf4aa3b671 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -96,23 +96,23 @@ typedef core::vectorOut_t vectorOut_t;
HPP_PREDEF_CLASS(ManipulationPlanner);
typedef shared_ptr<ManipulationPlanner> ManipulationPlannerPtr_t;
namespace pathPlanner {
- HPP_PREDEF_CLASS(EndEffectorTrajectory);
- typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
- HPP_PREDEF_CLASS (StatesPathFinder);
- typedef shared_ptr < StatesPathFinder > StatesPathFinderPtr_t;
- HPP_PREDEF_CLASS (InStatePath);
- typedef shared_ptr < InStatePath > InStatePathPtr_t;
- HPP_PREDEF_CLASS (StateShooter);
- typedef shared_ptr < StateShooter > StateShooterPtr_t;
-} // namespace pathPlanner
+HPP_PREDEF_CLASS(EndEffectorTrajectory);
+typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
+HPP_PREDEF_CLASS(StatesPathFinder);
+typedef shared_ptr<StatesPathFinder> StatesPathFinderPtr_t;
+HPP_PREDEF_CLASS(InStatePath);
+typedef shared_ptr<InStatePath> InStatePathPtr_t;
+HPP_PREDEF_CLASS(StateShooter);
+typedef shared_ptr<StateShooter> StateShooterPtr_t;
+} // namespace pathPlanner
HPP_PREDEF_CLASS(GraphPathValidation);
typedef shared_ptr<GraphPathValidation> GraphPathValidationPtr_t;
HPP_PREDEF_CLASS(SteeringMethod);
typedef shared_ptr<SteeringMethod> SteeringMethodPtr_t;
-namespace steeringMethod{
- HPP_PREDEF_CLASS(EndEffectorTrajectory);
- typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
-}
+namespace steeringMethod {
+HPP_PREDEF_CLASS(EndEffectorTrajectory);
+typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
+} // namespace steeringMethod
typedef core::PathOptimizer PathOptimizer;
typedef core::PathOptimizerPtr_t PathOptimizerPtr_t;
HPP_PREDEF_CLASS(GraphOptimizer);
diff --git a/include/hpp/manipulation/path-planner/in-state-path.hh b/include/hpp/manipulation/path-planner/in-state-path.hh
index 4395ed6daff7b459eda72c9a29ffb70e472ac946..8e53d2ba87b3f40287bc791b5cd0fcf5ec24fa91 100644
--- a/include/hpp/manipulation/path-planner/in-state-path.hh
+++ b/include/hpp/manipulation/path-planner/in-state-path.hh
@@ -17,119 +17,112 @@
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
#ifndef HPP_MANIPULATION_PATH_PLANNER_IN_STATE_PATH_HH
-# define HPP_MANIPULATION_PATH_PLANNER_IN_STATE_PATH_HH
+#define HPP_MANIPULATION_PATH_PLANNER_IN_STATE_PATH_HH
-# include <hpp/core/path-planner.hh>
-# include <hpp/core/config-projector.hh>
-# include <hpp/core/collision-validation.hh>
-# include <hpp/core/joint-bound-validation.hh>
-
-# include <hpp/manipulation/config.hh>
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/problem.hh>
-# include <hpp/manipulation/steering-method/graph.hh>
+#include <hpp/core/collision-validation.hh>
+#include <hpp/core/config-projector.hh>
+#include <hpp/core/joint-bound-validation.hh>
+#include <hpp/core/path-planner.hh>
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/problem.hh>
+#include <hpp/manipulation/steering-method/graph.hh>
namespace hpp {
- namespace manipulation {
- namespace pathPlanner {
- /// \addtogroup path_planner
- /// \{
-
- ///
- /// Path planner designed to build a path between two configurations
- /// on the same leaf of a given state graph edge
- class HPP_MANIPULATION_DLLAPI InStatePath : public core::PathPlanner
- {
- public:
- virtual ~InStatePath()
- {}
-
- static InStatePathPtr_t create (
- const core::ProblemConstPtr_t& problem);
-
- static InStatePathPtr_t createWithRoadmap (
- const core::ProblemConstPtr_t& problem,
- const core::RoadmapPtr_t& roadmap);
-
- InStatePathPtr_t copy () const;
-
- void maxIterPlanner(const unsigned long& maxiter);
- void timeOutPlanner(const double& timeout);
- void resetRoadmap(const bool& resetroadmap);
- void plannerType(const std::string& plannertype);
- void addOptimizerType(const std::string& opttype);
- void resetOptimizerTypes();
-
- /// Set the edge along which q_init and q_goal will be linked.
- /// Use setEdge before setInit and setGoal.
- void setEdge (const graph::EdgePtr_t& edge);
- void setInit (const ConfigurationPtr_t& q);
- void setGoal (const ConfigurationPtr_t& q);
-
- virtual void oneStep()
- {}
- virtual core::PathVectorPtr_t solve();
-
- const core::RoadmapPtr_t& leafRoadmap() const;
- void mergeLeafRoadmapWith(const core::RoadmapPtr_t& roadmap) const;
-
- protected:
- InStatePath (const core::ProblemConstPtr_t& problem,
- const core::RoadmapPtr_t& roadmap) :
- PathPlanner(problem),
- problem_ (HPP_STATIC_PTR_CAST(const manipulation::Problem, problem)),
- leafRoadmap_(roadmap),
- constraints_(), weak_()
- {
- const core::DevicePtr_t& robot = problem_->robot();
- leafProblem_ = core::Problem::create(robot);
- leafProblem_->setParameter
- ("kPRM*/numberOfNodes", core::Parameter((size_type) 2000));
- leafProblem_->clearConfigValidations();
- leafProblem_->addConfigValidation(core::CollisionValidation::create(robot));
- leafProblem_->addConfigValidation(core::JointBoundValidation::create(robot));
- for (const core::CollisionObjectPtr_t & obs: problem_->collisionObstacles()) {
- leafProblem_->addObstacle(obs);
- }
- leafProblem_->pathProjector(problem->pathProjector());
- }
-
- InStatePath (const InStatePath& other) :
- PathPlanner(other.problem_),
- problem_(other.problem_), leafProblem_(other.leafProblem_),
- leafRoadmap_ (other.leafRoadmap_),
- constraints_(other.constraints_),
- weak_ ()
- {}
-
- void init (InStatePathWkPtr_t weak)
- {
- weak_ = weak;
- }
-
- private:
-
- // a pointer to the problem used to create the InStatePath instance
- ProblemConstPtr_t problem_;
- // a new problem created for this InStatePath instance
- core::ProblemPtr_t leafProblem_;
- core::RoadmapPtr_t leafRoadmap_;
- ConstraintSetPtr_t constraints_;
-
- double timeOutPathPlanning_ = 0;
- unsigned long maxIterPathPlanning_ = 0;
- bool resetRoadmap_ = true;
- std::string plannerType_ = "BiRRT*";
- std::vector<std::string> optimizerTypes_;
-
- /// Weak pointer to itself
- InStatePathWkPtr_t weak_;
-
- }; // class InStatePath
- /// \}
-
- } // namespace pathPlanner
- } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH
+namespace manipulation {
+namespace pathPlanner {
+/// \addtogroup path_planner
+/// \{
+
+///
+/// Path planner designed to build a path between two configurations
+/// on the same leaf of a given state graph edge
+class HPP_MANIPULATION_DLLAPI InStatePath : public core::PathPlanner {
+ public:
+ virtual ~InStatePath() {}
+
+ static InStatePathPtr_t create(const core::ProblemConstPtr_t& problem);
+
+ static InStatePathPtr_t createWithRoadmap(
+ const core::ProblemConstPtr_t& problem,
+ const core::RoadmapPtr_t& roadmap);
+
+ InStatePathPtr_t copy() const;
+
+ void maxIterPlanner(const unsigned long& maxiter);
+ void timeOutPlanner(const double& timeout);
+ void resetRoadmap(const bool& resetroadmap);
+ void plannerType(const std::string& plannertype);
+ void addOptimizerType(const std::string& opttype);
+ void resetOptimizerTypes();
+
+ /// Set the edge along which q_init and q_goal will be linked.
+ /// Use setEdge before setInit and setGoal.
+ void setEdge(const graph::EdgePtr_t& edge);
+ void setInit(const ConfigurationPtr_t& q);
+ void setGoal(const ConfigurationPtr_t& q);
+
+ virtual void oneStep() {}
+ virtual core::PathVectorPtr_t solve();
+
+ const core::RoadmapPtr_t& leafRoadmap() const;
+ void mergeLeafRoadmapWith(const core::RoadmapPtr_t& roadmap) const;
+
+ protected:
+ InStatePath(const core::ProblemConstPtr_t& problem,
+ const core::RoadmapPtr_t& roadmap)
+ : PathPlanner(problem),
+ problem_(HPP_STATIC_PTR_CAST(const manipulation::Problem, problem)),
+ leafRoadmap_(roadmap),
+ constraints_(),
+ weak_() {
+ const core::DevicePtr_t& robot = problem_->robot();
+ leafProblem_ = core::Problem::create(robot);
+ leafProblem_->setParameter("kPRM*/numberOfNodes",
+ core::Parameter((size_type)2000));
+ leafProblem_->clearConfigValidations();
+ leafProblem_->addConfigValidation(core::CollisionValidation::create(robot));
+ leafProblem_->addConfigValidation(
+ core::JointBoundValidation::create(robot));
+ for (const core::CollisionObjectPtr_t& obs :
+ problem_->collisionObstacles()) {
+ leafProblem_->addObstacle(obs);
+ }
+ leafProblem_->pathProjector(problem->pathProjector());
+ }
+
+ InStatePath(const InStatePath& other)
+ : PathPlanner(other.problem_),
+ problem_(other.problem_),
+ leafProblem_(other.leafProblem_),
+ leafRoadmap_(other.leafRoadmap_),
+ constraints_(other.constraints_),
+ weak_() {}
+
+ void init(InStatePathWkPtr_t weak) { weak_ = weak; }
+
+ private:
+ // a pointer to the problem used to create the InStatePath instance
+ ProblemConstPtr_t problem_;
+ // a new problem created for this InStatePath instance
+ core::ProblemPtr_t leafProblem_;
+ core::RoadmapPtr_t leafRoadmap_;
+ ConstraintSetPtr_t constraints_;
+
+ double timeOutPathPlanning_ = 0;
+ unsigned long maxIterPathPlanning_ = 0;
+ bool resetRoadmap_ = true;
+ std::string plannerType_ = "BiRRT*";
+ std::vector<std::string> optimizerTypes_;
+
+ /// Weak pointer to itself
+ InStatePathWkPtr_t weak_;
+
+}; // class InStatePath
+/// \}
+
+} // namespace pathPlanner
+} // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH
diff --git a/include/hpp/manipulation/path-planner/states-path-finder.hh b/include/hpp/manipulation/path-planner/states-path-finder.hh
index a0140eeb89b7111c6b7770ed0b25f8f3dac1ee0a..44c442c51459363d930feac4a78dd4622e57c8ac 100644
--- a/include/hpp/manipulation/path-planner/states-path-finder.hh
+++ b/include/hpp/manipulation/path-planner/states-path-finder.hh
@@ -18,255 +18,248 @@
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
#ifndef HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
-# define HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
-
-# include <hpp/core/fwd.hh>
-# include <hpp/core/path.hh>
-
-# include <hpp/core/projection-error.hh>
-# include <hpp/core/config-projector.hh>
-
-# include <hpp/core/validation-report.hh>
-# include <hpp/core/config-validations.hh>
-
-# include <hpp/core/path-planner.hh>
-
-# include <hpp/manipulation/config.hh>
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/problem.hh>
+#define HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
+
+#include <hpp/core/config-projector.hh>
+#include <hpp/core/config-validations.hh>
+#include <hpp/core/fwd.hh>
+#include <hpp/core/path-planner.hh>
+#include <hpp/core/path.hh>
+#include <hpp/core/projection-error.hh>
+#include <hpp/core/validation-report.hh>
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/problem.hh>
namespace hpp {
- namespace manipulation {
- namespace pathPlanner {
-
- /// \addtogroup path_planner
- /// \{
-
- /// Optimization-based path planning method.
- ///
- /// #### Sketch of the method
- ///
- /// Given two configurations \f$ (q_1,q_2) \f$, this class formulates and
- /// solves the problem as follows.
- /// - Compute the corresponding states \f$ (s_1, s_2) \f$.
- /// - For each path \f$ (e_0, ... e_n) \f$ of length not more than
- /// parameter "StatesPathFinder/maxDepth" between
- /// \f$ (s_1, s_2)\f$ in the constraint graph, do:
- /// - define \f$ n-1 \f$ intermediate configuration \f$ p_i \f$,
- /// - initialize the optimization problem, as explained below,
- /// - solve the optimization problem, which gives \f$ p^*_i \f$,
- /// - in case of failure, continue the loop.
- ///
- /// #### Problem formulation
- /// Find \f$ (p_i) \f$ such that:
- /// - \f$ p_0 = q_1 \f$,
- /// - \f$ p_{n+1} = q_2 \f$,
- /// - \f$ p_i \f$ is in state between \f$ (e_{i-1}, e_i) \f$, (\ref StateFunction)
- /// - \f$ (p_i, p_{i+1}) \f$ are reachable with transition \f$ e_i \f$ (\ref EdgeFunction).
- ///
- /// #### Problem resolution
- ///
- /// One solver (hpp::constraints::solver::BySubstitution) is created
- /// for each waypoint \f$p_i\f$.
- /// - method buildOptimizationProblem builds a matrix the rows of which
- /// are the parameterizable numerical constraints present in the
- /// graph, and the columns of which are the waypoints. Each value in the
- /// matrix defines the status of each constraint right hand side for
- /// this waypoint, among {absent from the solver,
- /// equal to value for previous waypoint,
- /// equal to value for start configuration,
- /// equal to value for end configuration}.
- /// - method analyseOptimizationProblem loops over the waypoint solvers,
- /// tests what happens when solving each waypoint after initializing
- /// only the right hand sides that are equal to the initial or goal
- /// configuration, and detects if a collision is certain to block any attempts
- /// to solve the problem in the solveOptimizationProblem step.
- /// - method solveOptimizationProblem tries to solve for each waypoint after
- /// initializing the right hand sides with the proper values, backtracking
- /// to the previous waypoint if the solving failed or a collision is
- /// detected a number of times set from the parameter
- /// "StatesPathFinder/nTriesUntilBacktrack". If too much backtracking
- /// occurs, the method can eventually return false.
- /// - eventually method buildPath build paths between waypoints with
- /// the constraints of the transition in which the path lies.
- ///
- /// #### Current status
- ///
- /// The method has been successfully tested with romeo holding a placard
- /// and the construction set benchmarks. The result is satisfactory
- /// except between pregrasp and grasp waypoints that may be far
- /// away from each other if the transition between those state does
- /// not contain the grasp complement constraint. The same holds
- /// between placement and pre-placement.
- class HPP_MANIPULATION_DLLAPI StatesPathFinder : public core::PathPlanner
- {
-
- public:
- struct OptimizationData;
-
- virtual ~StatesPathFinder () {};
-
- static StatesPathFinderPtr_t create (
- const core::ProblemConstPtr_t& problem);
-
- static StatesPathFinderPtr_t createWithRoadmap (
- const core::ProblemConstPtr_t& problem,
- const core::RoadmapPtr_t& roadmap);
-
- StatesPathFinderPtr_t copy () const;
-
- /// create a vector of configurations between two configurations
- /// \param q1 initial configuration
- /// \param q2 pointer to final configuration, NULL if goal is
- /// defined as a set of constraints
- /// \return a Configurations_t from q1 to q2 if found. An empty
- /// vector if a path could not be built.
- core::Configurations_t computeConfigList (ConfigurationIn_t q1,
- ConfigurationPtr_t q2);
-
- // access functions for Python interface
- std::vector<std::string> constraintNamesFromSolverAtWaypoint
- (std::size_t wp);
- std::vector<std::string> lastBuiltTransitions() const;
- std::string displayConfigsSolved () const;
- bool buildOptimizationProblemFromNames(std::vector<std::string> names);
-
- // Substeps of method solveOptimizationProblem
- void initWPRandom(std::size_t wp);
- void initWPNear(std::size_t wp);
- void initWP(std::size_t wp, ConfigurationIn_t q);
- /// Status of the step to solve for a particular waypoint
- enum SolveStepStatus {
- /// Valid solution (no collision)
- VALID_SOLUTION,
- /// Bad solve status, no solution from the solver
- NO_SOLUTION,
- /// Solution has collision in edge leading to the waypoint
- COLLISION_BEFORE,
- /// Solution has collision in edge going from the waypoint
- COLLISION_AFTER,
- };
-
- SolveStepStatus solveStep(std::size_t wp);
-
- /// deletes from memory the latest working states list, which is used to
- /// resume finding solutions from that list in case of failure at a
- /// later step.
- void reset();
-
- virtual void startSolve();
- virtual void oneStep();
- /// when both initial state is one of potential goal states,
- /// try connecting them directly
- virtual void tryConnectInitAndGoals ();
-
- protected:
- StatesPathFinder (const core::ProblemConstPtr_t& problem,
- const core::RoadmapPtr_t&);
- StatesPathFinder (const StatesPathFinder& other);
-
- void init (StatesPathFinderWkPtr_t weak)
- {
- weak_ = weak;
- }
-
- private:
- typedef constraints::solver::BySubstitution Solver_t;
- struct GraphSearchData;
-
- /// Gather constraints of all edges
- void gatherGraphConstraints ();
-
- /// Step 1 of the algorithm
- /// \return whether the max depth was reached.
- bool findTransitions (GraphSearchData& data) const;
-
- /// Step 2 of the algorithm
- graph::Edges_t getTransitionList (const GraphSearchData& data, const std::size_t& i) const;
-
- /// Step 3 of the algorithm
-
- // check that the solver either contains exactly same constraint
- // or a constraint with similar parametrizable form
- // constraint/both and constraint/complement
- bool contains (const Solver_t& solver, const ImplicitPtr_t& c) const;
-
- // check that the solver either contains exactly same constraint
- // or a stricter version of the constraint
- // constraint/both stricter than constraint and stricter than constraint/complement
- bool containsStricter (const Solver_t& solver, const ImplicitPtr_t& c) const;
- bool checkConstantRightHandSide (size_type index);
- bool buildOptimizationProblem (const graph::Edges_t& transitions);
-
- /// Step 4 of the algorithm
- void preInitializeRHS(std::size_t j, Configuration_t& q);
- bool analyseOptimizationProblem (const graph::Edges_t& transitions);
- bool analyseOptimizationProblem2 (const graph::Edges_t& transitions,
- core::ProblemConstPtr_t _problem);
-
- /// Step 5 of the algorithm
- void initializeRHS (std::size_t j);
- bool solveOptimizationProblem ();
-
- /// Step 6 of the algorithm
- core::Configurations_t getConfigList () const;
-
- /// Functions used in assert statements
- bool checkWaypointRightHandSide (std::size_t ictr, std::size_t jslv) const;
- bool checkSolverRightHandSide (std::size_t ictr, std::size_t jslv) const;
- bool checkWaypointRightHandSide (std::size_t jslv) const;
- bool checkSolverRightHandSide (std::size_t jslv) const;
-
- void displayRhsMatrix ();
- void displayStatusMatrix (const graph::Edges_t& transitions);
-
- /// A pointer to the manipulation problem
- ProblemConstPtr_t problem_;
- /// Path planning problem in each leaf.
- core::ProblemPtr_t inStateProblem_;
-
- /// Vector of parameterizable edge numerical constraints
- NumericalConstraints_t constraints_;
- /// Map of indices in constraints_
- std::map < std::string, std::size_t > index_;
-
- /// associative map that stores pairs of constraints of the form
- /// (constraint/complement, constraint/hold)
- std::map <ImplicitPtr_t, ImplicitPtr_t> sameRightHandSide_;
-
- /// associative map that stores pairs of constraints of either form
- /// (constraint, constraint/hold)
- /// or (constraint/complement, constraint/hold)
- std::map <ImplicitPtr_t, ImplicitPtr_t> stricterConstraints_;
-
- mutable OptimizationData* optData_;
- mutable std::shared_ptr <GraphSearchData> graphData_;
- graph::Edges_t lastBuiltTransitions_;
-
- /// Constraints defining the goal
- /// For now:
- /// - comparison type Equality is initialized to zero
- /// - if goal constraint is not already present in any graph state,
- /// it should not require propagating a complement.
- /// invalid eg: specify the full pose of an object placement or
- /// object grasp
- NumericalConstraints_t goalConstraints_;
- bool goalDefinedByConstraints_;
- // Variables used across several calls to oneStep
- ConfigurationPtr_t q1_, q2_;
- core::Configurations_t configList_;
- std::size_t idxConfigList_;
- size_type nTryConfigList_;
- bool solved_, interrupt_;
-
- /// Weak pointer to itself
- StatesPathFinderWkPtr_t weak_;
-
- }; // class StatesPathFinder
- /// \}
-
- } // namespace pathPlanner
- } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
+namespace manipulation {
+namespace pathPlanner {
+
+/// \addtogroup path_planner
+/// \{
+
+/// Optimization-based path planning method.
+///
+/// #### Sketch of the method
+///
+/// Given two configurations \f$ (q_1,q_2) \f$, this class formulates and
+/// solves the problem as follows.
+/// - Compute the corresponding states \f$ (s_1, s_2) \f$.
+/// - For each path \f$ (e_0, ... e_n) \f$ of length not more than
+/// parameter "StatesPathFinder/maxDepth" between
+/// \f$ (s_1, s_2)\f$ in the constraint graph, do:
+/// - define \f$ n-1 \f$ intermediate configuration \f$ p_i \f$,
+/// - initialize the optimization problem, as explained below,
+/// - solve the optimization problem, which gives \f$ p^*_i \f$,
+/// - in case of failure, continue the loop.
+///
+/// #### Problem formulation
+/// Find \f$ (p_i) \f$ such that:
+/// - \f$ p_0 = q_1 \f$,
+/// - \f$ p_{n+1} = q_2 \f$,
+/// - \f$ p_i \f$ is in state between \f$ (e_{i-1}, e_i) \f$, (\ref
+/// StateFunction)
+/// - \f$ (p_i, p_{i+1}) \f$ are reachable with transition \f$ e_i \f$ (\ref
+/// EdgeFunction).
+///
+/// #### Problem resolution
+///
+/// One solver (hpp::constraints::solver::BySubstitution) is created
+/// for each waypoint \f$p_i\f$.
+/// - method buildOptimizationProblem builds a matrix the rows of which
+/// are the parameterizable numerical constraints present in the
+/// graph, and the columns of which are the waypoints. Each value in the
+/// matrix defines the status of each constraint right hand side for
+/// this waypoint, among {absent from the solver,
+/// equal to value for previous waypoint,
+/// equal to value for start configuration,
+/// equal to value for end configuration}.
+/// - method analyseOptimizationProblem loops over the waypoint solvers,
+/// tests what happens when solving each waypoint after initializing
+/// only the right hand sides that are equal to the initial or goal
+/// configuration, and detects if a collision is certain to block any attempts
+/// to solve the problem in the solveOptimizationProblem step.
+/// - method solveOptimizationProblem tries to solve for each waypoint after
+/// initializing the right hand sides with the proper values, backtracking
+/// to the previous waypoint if the solving failed or a collision is
+/// detected a number of times set from the parameter
+/// "StatesPathFinder/nTriesUntilBacktrack". If too much backtracking
+/// occurs, the method can eventually return false.
+/// - eventually method buildPath build paths between waypoints with
+/// the constraints of the transition in which the path lies.
+///
+/// #### Current status
+///
+/// The method has been successfully tested with romeo holding a placard
+/// and the construction set benchmarks. The result is satisfactory
+/// except between pregrasp and grasp waypoints that may be far
+/// away from each other if the transition between those state does
+/// not contain the grasp complement constraint. The same holds
+/// between placement and pre-placement.
+class HPP_MANIPULATION_DLLAPI StatesPathFinder : public core::PathPlanner {
+ public:
+ struct OptimizationData;
+
+ virtual ~StatesPathFinder(){};
+
+ static StatesPathFinderPtr_t create(const core::ProblemConstPtr_t& problem);
+
+ static StatesPathFinderPtr_t createWithRoadmap(
+ const core::ProblemConstPtr_t& problem,
+ const core::RoadmapPtr_t& roadmap);
+
+ StatesPathFinderPtr_t copy() const;
+
+ /// create a vector of configurations between two configurations
+ /// \param q1 initial configuration
+ /// \param q2 pointer to final configuration, NULL if goal is
+ /// defined as a set of constraints
+ /// \return a Configurations_t from q1 to q2 if found. An empty
+ /// vector if a path could not be built.
+ core::Configurations_t computeConfigList(ConfigurationIn_t q1,
+ ConfigurationPtr_t q2);
+
+ // access functions for Python interface
+ std::vector<std::string> constraintNamesFromSolverAtWaypoint(std::size_t wp);
+ std::vector<std::string> lastBuiltTransitions() const;
+ std::string displayConfigsSolved() const;
+ bool buildOptimizationProblemFromNames(std::vector<std::string> names);
+
+ // Substeps of method solveOptimizationProblem
+ void initWPRandom(std::size_t wp);
+ void initWPNear(std::size_t wp);
+ void initWP(std::size_t wp, ConfigurationIn_t q);
+ /// Status of the step to solve for a particular waypoint
+ enum SolveStepStatus {
+ /// Valid solution (no collision)
+ VALID_SOLUTION,
+ /// Bad solve status, no solution from the solver
+ NO_SOLUTION,
+ /// Solution has collision in edge leading to the waypoint
+ COLLISION_BEFORE,
+ /// Solution has collision in edge going from the waypoint
+ COLLISION_AFTER,
+ };
+
+ SolveStepStatus solveStep(std::size_t wp);
+
+ /// deletes from memory the latest working states list, which is used to
+ /// resume finding solutions from that list in case of failure at a
+ /// later step.
+ void reset();
+
+ virtual void startSolve();
+ virtual void oneStep();
+ /// when both initial state is one of potential goal states,
+ /// try connecting them directly
+ virtual void tryConnectInitAndGoals();
+
+ protected:
+ StatesPathFinder(const core::ProblemConstPtr_t& problem,
+ const core::RoadmapPtr_t&);
+ StatesPathFinder(const StatesPathFinder& other);
+
+ void init(StatesPathFinderWkPtr_t weak) { weak_ = weak; }
+
+ private:
+ typedef constraints::solver::BySubstitution Solver_t;
+ struct GraphSearchData;
+
+ /// Gather constraints of all edges
+ void gatherGraphConstraints();
+
+ /// Step 1 of the algorithm
+ /// \return whether the max depth was reached.
+ bool findTransitions(GraphSearchData& data) const;
+
+ /// Step 2 of the algorithm
+ graph::Edges_t getTransitionList(const GraphSearchData& data,
+ const std::size_t& i) const;
+
+ /// Step 3 of the algorithm
+
+ // check that the solver either contains exactly same constraint
+ // or a constraint with similar parametrizable form
+ // constraint/both and constraint/complement
+ bool contains(const Solver_t& solver, const ImplicitPtr_t& c) const;
+
+ // check that the solver either contains exactly same constraint
+ // or a stricter version of the constraint
+ // constraint/both stricter than constraint and stricter than
+ // constraint/complement
+ bool containsStricter(const Solver_t& solver, const ImplicitPtr_t& c) const;
+ bool checkConstantRightHandSide(size_type index);
+ bool buildOptimizationProblem(const graph::Edges_t& transitions);
+
+ /// Step 4 of the algorithm
+ void preInitializeRHS(std::size_t j, Configuration_t& q);
+ bool analyseOptimizationProblem(const graph::Edges_t& transitions);
+ bool analyseOptimizationProblem2(const graph::Edges_t& transitions,
+ core::ProblemConstPtr_t _problem);
+
+ /// Step 5 of the algorithm
+ void initializeRHS(std::size_t j);
+ bool solveOptimizationProblem();
+
+ /// Step 6 of the algorithm
+ core::Configurations_t getConfigList() const;
+
+ /// Functions used in assert statements
+ bool checkWaypointRightHandSide(std::size_t ictr, std::size_t jslv) const;
+ bool checkSolverRightHandSide(std::size_t ictr, std::size_t jslv) const;
+ bool checkWaypointRightHandSide(std::size_t jslv) const;
+ bool checkSolverRightHandSide(std::size_t jslv) const;
+
+ void displayRhsMatrix();
+ void displayStatusMatrix(const graph::Edges_t& transitions);
+
+ /// A pointer to the manipulation problem
+ ProblemConstPtr_t problem_;
+ /// Path planning problem in each leaf.
+ core::ProblemPtr_t inStateProblem_;
+
+ /// Vector of parameterizable edge numerical constraints
+ NumericalConstraints_t constraints_;
+ /// Map of indices in constraints_
+ std::map<std::string, std::size_t> index_;
+
+ /// associative map that stores pairs of constraints of the form
+ /// (constraint/complement, constraint/hold)
+ std::map<ImplicitPtr_t, ImplicitPtr_t> sameRightHandSide_;
+
+ /// associative map that stores pairs of constraints of either form
+ /// (constraint, constraint/hold)
+ /// or (constraint/complement, constraint/hold)
+ std::map<ImplicitPtr_t, ImplicitPtr_t> stricterConstraints_;
+
+ mutable OptimizationData* optData_;
+ mutable std::shared_ptr<GraphSearchData> graphData_;
+ graph::Edges_t lastBuiltTransitions_;
+
+ /// Constraints defining the goal
+ /// For now:
+ /// - comparison type Equality is initialized to zero
+ /// - if goal constraint is not already present in any graph state,
+ /// it should not require propagating a complement.
+ /// invalid eg: specify the full pose of an object placement or
+ /// object grasp
+ NumericalConstraints_t goalConstraints_;
+ bool goalDefinedByConstraints_;
+ // Variables used across several calls to oneStep
+ ConfigurationPtr_t q1_, q2_;
+ core::Configurations_t configList_;
+ std::size_t idxConfigList_;
+ size_type nTryConfigList_;
+ bool solved_, interrupt_;
+
+ /// Weak pointer to itself
+ StatesPathFinderWkPtr_t weak_;
+
+}; // class StatesPathFinder
+/// \}
+
+} // namespace pathPlanner
+} // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
diff --git a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
index e99c498915d5c41ee243cc9c01d3ec52eb404449..5117641a261fb9bb4106c2155c891c14f75228db 100644
--- a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
+++ b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
@@ -77,9 +77,8 @@ class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory
\f]
\f{eqnarray*}{
- f(t) = \mathbf{p}_i \oplus \frac{t-t_i}{t_{i+1}-t_i} (\mathbf{p}_{i+1}-\mathbf{p}_i) &&
- \mbox{ for } t \in [t_i,t_{i+1}]
- \f}
+ f(t) = \mathbf{p}_i \oplus \frac{t-t_i}{t_{i+1}-t_i}
+ (\mathbf{p}_{i+1}-\mathbf{p}_i) && \mbox{ for } t \in [t_i,t_{i+1}] \f}
where \f$t_0 = 0\f$ and
\f{eqnarray*}{
diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
index a6ffc26701b34030fae3108addb342b505829a49..b91c9c28e414c31ca09eb3ba995133e27aa5179f 100644
--- a/src/path-planner/states-path-finder.cc
+++ b/src/path-planner/states-path-finder.cc
@@ -17,1613 +17,1622 @@
// received a copy of the GNU Lesser General Public License along with
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
-#include <pinocchio/fwd.hpp>
-#include <hpp/manipulation/path-planner/states-path-finder.hh>
-
-#include <map>
-#include <queue>
-#include <vector>
-
-#include <hpp/util/debug.hh>
-#include <hpp/util/exception-factory.hh>
-#include <hpp/util/timer.hh>
-
-#include <pinocchio/multibody/model.hpp>
-
-#include <hpp/pinocchio/configuration.hh>
-#include <hpp/pinocchio/joint-collection.hh>
-
#include <hpp/constraints/affine-function.hh>
+#include <hpp/constraints/explicit.hh>
#include <hpp/constraints/locked-joint.hh>
#include <hpp/constraints/solver/by-substitution.hh>
-#include <hpp/constraints/explicit.hh>
-
+#include <hpp/core/collision-validation-report.hh>
+#include <hpp/core/collision-validation.hh>
+#include <hpp/core/configuration-shooter.hh>
#include <hpp/core/diffusing-planner.hh>
-#include <hpp/core/path-vector.hh>
+#include <hpp/core/path-optimization/random-shortcut.hh>
#include <hpp/core/path-planning-failed.hh>
#include <hpp/core/path-projector/progressive.hh>
-#include <hpp/core/configuration-shooter.hh>
-#include <hpp/core/collision-validation.hh>
-#include <hpp/core/collision-validation-report.hh>
-#include <hpp/core/problem-target/task-target.hh>
-#include <hpp/core/problem-target/goal-configurations.hh>
-#include <hpp/core/path-optimization/random-shortcut.hh>
#include <hpp/core/path-validation.hh>
-
+#include <hpp/core/path-vector.hh>
+#include <hpp/core/problem-target/goal-configurations.hh>
+#include <hpp/core/problem-target/task-target.hh>
#include <hpp/manipulation/constraint-set.hh>
-
#include <hpp/manipulation/graph/edge.hh>
-#include <hpp/manipulation/graph/state.hh>
-#include <hpp/manipulation/roadmap.hh>
#include <hpp/manipulation/graph/state-selector.hh>
-
+#include <hpp/manipulation/graph/state.hh>
#include <hpp/manipulation/path-planner/in-state-path.hh>
+#include <hpp/manipulation/path-planner/states-path-finder.hh>
+#include <hpp/manipulation/roadmap.hh>
+#include <hpp/pinocchio/configuration.hh>
+#include <hpp/pinocchio/joint-collection.hh>
+#include <hpp/util/debug.hh>
+#include <hpp/util/exception-factory.hh>
+#include <hpp/util/timer.hh>
+#include <map>
+#include <pinocchio/fwd.hpp>
+#include <pinocchio/multibody/model.hpp>
+#include <queue>
+#include <vector>
namespace hpp {
- namespace manipulation {
- namespace pathPlanner {
-
- using Eigen::RowBlockIndices;
- using Eigen::ColBlockIndices;
-
- using graph::StatePtr_t;
- using graph::States_t;
- using graph::EdgePtr_t;
- using graph::Edges_t;
- using graph::Neighbors_t;
- using graph::NumericalConstraints_t;
- using graph::LockedJoints_t;
- using graph::segments_t;
-
- using core::ProblemTargetPtr_t;
- using core::problemTarget::GoalConfigurations;
- using core::problemTarget::GoalConfigurationsPtr_t;
- using core::problemTarget::TaskTarget;
- using core::problemTarget::TaskTargetPtr_t;
+namespace manipulation {
+namespace pathPlanner {
+
+using Eigen::ColBlockIndices;
+using Eigen::RowBlockIndices;
+
+using graph::EdgePtr_t;
+using graph::Edges_t;
+using graph::LockedJoints_t;
+using graph::Neighbors_t;
+using graph::NumericalConstraints_t;
+using graph::segments_t;
+using graph::StatePtr_t;
+using graph::States_t;
+
+using core::ProblemTargetPtr_t;
+using core::problemTarget::GoalConfigurations;
+using core::problemTarget::GoalConfigurationsPtr_t;
+using core::problemTarget::TaskTarget;
+using core::problemTarget::TaskTargetPtr_t;
#ifdef HPP_DEBUG
- static void displayRoadmap(const core::RoadmapPtr_t&
- roadmap
- )
- {
- unsigned i=0;
- for (auto cc : roadmap->connectedComponents()){
- hppDout(info, " CC " << i++);
- for (auto n : cc->nodes()) {
- hppDout(info, pinocchio::displayConfig(*(n->configuration())));
- }
- }
- }
+static void displayRoadmap(const core::RoadmapPtr_t& roadmap) {
+ unsigned i = 0;
+ for (auto cc : roadmap->connectedComponents()) {
+ hppDout(info, " CC " << i++);
+ for (auto n : cc->nodes()) {
+ hppDout(info, pinocchio::displayConfig(*(n->configuration())));
+ }
+ }
+}
#endif
- StatesPathFinder::StatesPathFinder(const core::ProblemConstPtr_t& problem,
- const core::RoadmapPtr_t& roadmap) :
- PathPlanner(problem, roadmap),
- problem_ (HPP_STATIC_PTR_CAST(const manipulation::Problem, problem)),
- constraints_(), index_(), sameRightHandSide_(),
- stricterConstraints_(), optData_(0x0), graphData_(0x0),
- lastBuiltTransitions_(),
- goalConstraints_(), goalDefinedByConstraints_(false),
- q1_(0x0), q2_(0x0), configList_(), idxConfigList_(0),
- nTryConfigList_(0), solved_(false), interrupt_(false),
- weak_()
- {
- gatherGraphConstraints ();
- inStateProblem_ = core::Problem::create(problem_->robot());
- }
-
- StatesPathFinder::StatesPathFinder (const StatesPathFinder& other) :
- PathPlanner(other.problem_),
- problem_ (other.problem_), constraints_ (), index_ (other.index_),
- sameRightHandSide_ (other.sameRightHandSide_), weak_ ()
- {}
-
- StatesPathFinderPtr_t StatesPathFinder::create (
- const core::ProblemConstPtr_t& problem)
- {
- StatesPathFinder* ptr;
- RoadmapPtr_t r = Roadmap::create(problem->distance(), problem->robot());
- try {
- ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
- ptr = new StatesPathFinder (p, r);
- } catch (std::exception&) {
- throw std::invalid_argument
- ("The problem must be of type hpp::manipulation::Problem.");
- }
- StatesPathFinderPtr_t shPtr (ptr);
- ptr->init (shPtr);
- return shPtr;
- }
-
- StatesPathFinderPtr_t StatesPathFinder::createWithRoadmap (
- const core::ProblemConstPtr_t& problem,
- const core::RoadmapPtr_t& roadmap)
- {
- StatesPathFinder* ptr;
- ProblemConstPtr_t p = HPP_DYNAMIC_PTR_CAST (const Problem, problem);
- RoadmapPtr_t r = HPP_DYNAMIC_PTR_CAST (Roadmap, roadmap);
- 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 StatesPathFinder (p, r);
- StatesPathFinderPtr_t shPtr (ptr);
- ptr->init (shPtr);
- return shPtr;
- }
-
- StatesPathFinderPtr_t StatesPathFinder::copy () const
- {
- StatesPathFinder* ptr = new StatesPathFinder (*this);
- StatesPathFinderPtr_t shPtr (ptr);
- ptr->init (shPtr);
- return shPtr;
- }
-
- struct StatesPathFinder::GraphSearchData
- {
- StatePtr_t s1;
- // list of potential goal states
- // can be multiple if goal is defined as a set of constraints
- graph::States_t s2;
-
- // index of the transition list
- size_t idxSol;
-
- // Datas for findNextTransitions
- struct state_with_depth {
- StatePtr_t s;
- EdgePtr_t e;
- std::size_t l; // depth to root
- std::size_t i; // index in parent state_with_depths
- // constructor used for root node
- inline state_with_depth () : s(), e(), l(0), i (0) {}
- // constructor used for non-root node
- inline state_with_depth (EdgePtr_t _e, std::size_t _l, std::size_t _i)
- : s(_e->stateFrom()), e(_e), l(_l), i (_i) {}
- };
- typedef std::vector<state_with_depth> state_with_depths;
- typedef std::map<StatePtr_t,state_with_depths> StateMap_t;
- /// std::size_t is the index in state_with_depths at StateMap_t::iterator
- struct state_with_depth_ptr_t {
- StateMap_t::iterator state;
- std::size_t parentIdx;
- state_with_depth_ptr_t (const StateMap_t::iterator& it, std::size_t idx)
- : state (it), parentIdx (idx) {}
- };
- typedef std::deque<state_with_depth_ptr_t> Deque_t;
- // vector of pointers to state with depth
- typedef std::vector<state_with_depth_ptr_t> state_with_depths_t;
- // transition lists exceeding this depth in the constraint graph will not be considered
- std::size_t maxDepth;
- // map each state X to a list of preceding states in transition lists that visit X
- // state_with_depth struct gives info to trace the entire transition lists
- StateMap_t parent1;
- // store a vector of pointers to the end state of each transition list
- state_with_depths_t solutions;
- // the frontier of the graph search
- // consists states that have not been expanded on
- Deque_t queue1;
-
- // track index of first transition list that has not been checked out
- // only needed when goal is defined as set of constraints
- size_t queueIt;
-
- const state_with_depth& getParent(const state_with_depth_ptr_t& _p) const
- {
- const state_with_depths& parents = _p.state->second;
- return parents[_p.parentIdx];
- }
-
- // add initial state (root node) to the map parent1
- state_with_depth_ptr_t addInitState()
- {
- StateMap_t::iterator next =
- parent1.insert(StateMap_t::value_type(s1, state_with_depths(1))).first;
- return state_with_depth_ptr_t (next, 0);
- }
-
- // store a transition to the map parent1
- state_with_depth_ptr_t addParent(
- const state_with_depth_ptr_t& _p,
- const EdgePtr_t& transition)
- {
- const state_with_depths& parents = _p.state->second;
- const state_with_depth& from = parents[_p.parentIdx];
-
- // Insert state to if necessary
- StateMap_t::iterator next = parent1.insert (
- StateMap_t::value_type(
- transition->stateTo(), state_with_depths ()
- )).first;
-
- next->second.push_back (
- state_with_depth(transition, from.l + 1, _p.parentIdx));
-
- return state_with_depth_ptr_t (next, next->second.size()-1);
- }
- };
-
- static bool containsLevelSet(const graph::EdgePtr_t& e) {
- graph::WaypointEdgePtr_t we =
- HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, e);
- if (!we)
- return false;
- for (std::size_t i = 0; i <= we->nbWaypoints(); i++) {
- graph::LevelSetEdgePtr_t lse =
- HPP_DYNAMIC_PTR_CAST(graph::LevelSetEdge, we->waypoint(i));
- if (lse)
- return true;
- }
- return false;
- }
-
- static bool isLoopTransition (const graph::EdgePtr_t& transition) {
- return transition->stateTo() == transition->stateFrom();
- }
-
- void StatesPathFinder::gatherGraphConstraints ()
- {
- typedef graph::Edge Edge;
- typedef graph::EdgePtr_t EdgePtr_t;
- typedef graph::GraphPtr_t GraphPtr_t;
- typedef constraints::solver::BySubstitution Solver_t;
-
- GraphPtr_t cg (problem_->constraintGraph ());
- const ConstraintsAndComplements_t& cac
- (cg->constraintsAndComplements ());
- for (std::size_t i = 0; i < cg->nbComponents (); ++i) {
- EdgePtr_t edge (HPP_DYNAMIC_PTR_CAST (Edge, cg->get (i).lock ()));
-
- // only gather the edge constraints
- if (!edge) continue;
-
- // Don't even consider level set edges
- if (containsLevelSet(edge)) continue;
-
- const Solver_t& solver (edge->pathConstraint ()->
- configProjector ()->solver ());
- const NumericalConstraints_t& constraints
- (solver.numericalConstraints ());
- for (NumericalConstraints_t::const_iterator it
- (constraints.begin ()); it != constraints.end (); ++it) {
- // only look at parameterized constraint
- if ((*it)->parameterSize () == 0) continue;
-
- const std::string& name ((*it)->function ().name ());
- // if constraint is in map, no need to add it
- if (index_.find (name) != index_.end ()) continue;
-
- index_ [name] = constraints_.size ();
- // Check whether constraint is equivalent to a previous one
- for (NumericalConstraints_t::const_iterator it1
- (constraints_.begin ()); it1 != constraints_.end ();
- ++it1) {
- for (ConstraintsAndComplements_t::const_iterator it2
- (cac.begin ()); it2 != cac.end (); ++it2) {
- if (((**it1 == *(it2->complement)) &&
- (**it == *(it2->both))) ||
- ((**it1 == *(it2->both)) &&
- (**it == *(it2->complement)))) {
- assert (sameRightHandSide_.count (*it1) == 0);
- assert (sameRightHandSide_.count (*it) == 0);
- sameRightHandSide_ [*it1] = *it;
- sameRightHandSide_ [*it] = *it1;
- }
- }
- }
- constraints_.push_back (*it);
- hppDout (info, "Adding constraint \"" << name << "\"");
- hppDout (info, "Edge \"" << edge->name () << "\"");
- hppDout (info, "parameter size: " << (*it)->parameterSize ());
-
+StatesPathFinder::StatesPathFinder(const core::ProblemConstPtr_t& problem,
+ const core::RoadmapPtr_t& roadmap)
+ : PathPlanner(problem, roadmap),
+ problem_(HPP_STATIC_PTR_CAST(const manipulation::Problem, problem)),
+ constraints_(),
+ index_(),
+ sameRightHandSide_(),
+ stricterConstraints_(),
+ optData_(0x0),
+ graphData_(0x0),
+ lastBuiltTransitions_(),
+ goalConstraints_(),
+ goalDefinedByConstraints_(false),
+ q1_(0x0),
+ q2_(0x0),
+ configList_(),
+ idxConfigList_(0),
+ nTryConfigList_(0),
+ solved_(false),
+ interrupt_(false),
+ weak_() {
+ gatherGraphConstraints();
+ inStateProblem_ = core::Problem::create(problem_->robot());
+}
+
+StatesPathFinder::StatesPathFinder(const StatesPathFinder& other)
+ : PathPlanner(other.problem_),
+ problem_(other.problem_),
+ constraints_(),
+ index_(other.index_),
+ sameRightHandSide_(other.sameRightHandSide_),
+ weak_() {}
+
+StatesPathFinderPtr_t StatesPathFinder::create(
+ const core::ProblemConstPtr_t& problem) {
+ StatesPathFinder* ptr;
+ RoadmapPtr_t r = Roadmap::create(problem->distance(), problem->robot());
+ try {
+ ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
+ ptr = new StatesPathFinder(p, r);
+ } catch (std::exception&) {
+ throw std::invalid_argument(
+ "The problem must be of type hpp::manipulation::Problem.");
+ }
+ StatesPathFinderPtr_t shPtr(ptr);
+ ptr->init(shPtr);
+ return shPtr;
+}
+
+StatesPathFinderPtr_t StatesPathFinder::createWithRoadmap(
+ const core::ProblemConstPtr_t& problem, const core::RoadmapPtr_t& roadmap) {
+ StatesPathFinder* ptr;
+ ProblemConstPtr_t p = HPP_DYNAMIC_PTR_CAST(const Problem, problem);
+ RoadmapPtr_t r = HPP_DYNAMIC_PTR_CAST(Roadmap, roadmap);
+ 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 StatesPathFinder(p, r);
+ StatesPathFinderPtr_t shPtr(ptr);
+ ptr->init(shPtr);
+ return shPtr;
+}
+
+StatesPathFinderPtr_t StatesPathFinder::copy() const {
+ StatesPathFinder* ptr = new StatesPathFinder(*this);
+ StatesPathFinderPtr_t shPtr(ptr);
+ ptr->init(shPtr);
+ return shPtr;
+}
+
+struct StatesPathFinder::GraphSearchData {
+ StatePtr_t s1;
+ // list of potential goal states
+ // can be multiple if goal is defined as a set of constraints
+ graph::States_t s2;
+
+ // index of the transition list
+ size_t idxSol;
+
+ // Datas for findNextTransitions
+ struct state_with_depth {
+ StatePtr_t s;
+ EdgePtr_t e;
+ std::size_t l; // depth to root
+ std::size_t i; // index in parent state_with_depths
+ // constructor used for root node
+ inline state_with_depth() : s(), e(), l(0), i(0) {}
+ // constructor used for non-root node
+ inline state_with_depth(EdgePtr_t _e, std::size_t _l, std::size_t _i)
+ : s(_e->stateFrom()), e(_e), l(_l), i(_i) {}
+ };
+ typedef std::vector<state_with_depth> state_with_depths;
+ typedef std::map<StatePtr_t, state_with_depths> StateMap_t;
+ /// std::size_t is the index in state_with_depths at StateMap_t::iterator
+ struct state_with_depth_ptr_t {
+ StateMap_t::iterator state;
+ std::size_t parentIdx;
+ state_with_depth_ptr_t(const StateMap_t::iterator& it, std::size_t idx)
+ : state(it), parentIdx(idx) {}
+ };
+ typedef std::deque<state_with_depth_ptr_t> Deque_t;
+ // vector of pointers to state with depth
+ typedef std::vector<state_with_depth_ptr_t> state_with_depths_t;
+ // transition lists exceeding this depth in the constraint graph will not be
+ // considered
+ std::size_t maxDepth;
+ // map each state X to a list of preceding states in transition lists that
+ // visit X state_with_depth struct gives info to trace the entire transition
+ // lists
+ StateMap_t parent1;
+ // store a vector of pointers to the end state of each transition list
+ state_with_depths_t solutions;
+ // the frontier of the graph search
+ // consists states that have not been expanded on
+ Deque_t queue1;
+
+ // track index of first transition list that has not been checked out
+ // only needed when goal is defined as set of constraints
+ size_t queueIt;
+
+ const state_with_depth& getParent(const state_with_depth_ptr_t& _p) const {
+ const state_with_depths& parents = _p.state->second;
+ return parents[_p.parentIdx];
+ }
+
+ // add initial state (root node) to the map parent1
+ state_with_depth_ptr_t addInitState() {
+ StateMap_t::iterator next =
+ parent1.insert(StateMap_t::value_type(s1, state_with_depths(1))).first;
+ return state_with_depth_ptr_t(next, 0);
+ }
+
+ // store a transition to the map parent1
+ state_with_depth_ptr_t addParent(const state_with_depth_ptr_t& _p,
+ const EdgePtr_t& transition) {
+ const state_with_depths& parents = _p.state->second;
+ const state_with_depth& from = parents[_p.parentIdx];
+
+ // Insert state to if necessary
+ StateMap_t::iterator next =
+ parent1
+ .insert(StateMap_t::value_type(transition->stateTo(),
+ state_with_depths()))
+ .first;
+
+ next->second.push_back(
+ state_with_depth(transition, from.l + 1, _p.parentIdx));
+
+ return state_with_depth_ptr_t(next, next->second.size() - 1);
+ }
+};
+
+static bool containsLevelSet(const graph::EdgePtr_t& e) {
+ graph::WaypointEdgePtr_t we = HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, e);
+ if (!we) return false;
+ for (std::size_t i = 0; i <= we->nbWaypoints(); i++) {
+ graph::LevelSetEdgePtr_t lse =
+ HPP_DYNAMIC_PTR_CAST(graph::LevelSetEdge, we->waypoint(i));
+ if (lse) return true;
+ }
+ return false;
+}
+
+static bool isLoopTransition(const graph::EdgePtr_t& transition) {
+ return transition->stateTo() == transition->stateFrom();
+}
+
+void StatesPathFinder::gatherGraphConstraints() {
+ typedef graph::Edge Edge;
+ typedef graph::EdgePtr_t EdgePtr_t;
+ typedef graph::GraphPtr_t GraphPtr_t;
+ typedef constraints::solver::BySubstitution Solver_t;
+
+ GraphPtr_t cg(problem_->constraintGraph());
+ const ConstraintsAndComplements_t& cac(cg->constraintsAndComplements());
+ for (std::size_t i = 0; i < cg->nbComponents(); ++i) {
+ EdgePtr_t edge(HPP_DYNAMIC_PTR_CAST(Edge, cg->get(i).lock()));
+
+ // only gather the edge constraints
+ if (!edge) continue;
+
+ // Don't even consider level set edges
+ if (containsLevelSet(edge)) continue;
+
+ const Solver_t& solver(edge->pathConstraint()->configProjector()->solver());
+ const NumericalConstraints_t& constraints(solver.numericalConstraints());
+ for (NumericalConstraints_t::const_iterator it(constraints.begin());
+ it != constraints.end(); ++it) {
+ // only look at parameterized constraint
+ if ((*it)->parameterSize() == 0) continue;
+
+ const std::string& name((*it)->function().name());
+ // if constraint is in map, no need to add it
+ if (index_.find(name) != index_.end()) continue;
+
+ index_[name] = constraints_.size();
+ // Check whether constraint is equivalent to a previous one
+ for (NumericalConstraints_t::const_iterator it1(constraints_.begin());
+ it1 != constraints_.end(); ++it1) {
+ for (ConstraintsAndComplements_t::const_iterator it2(cac.begin());
+ it2 != cac.end(); ++it2) {
+ if (((**it1 == *(it2->complement)) && (**it == *(it2->both))) ||
+ ((**it1 == *(it2->both)) && (**it == *(it2->complement)))) {
+ assert(sameRightHandSide_.count(*it1) == 0);
+ assert(sameRightHandSide_.count(*it) == 0);
+ sameRightHandSide_[*it1] = *it;
+ sameRightHandSide_[*it] = *it1;
}
}
- // constraint/both is the intersection of both constraint and constraint/complement
- for (ConstraintsAndComplements_t::const_iterator it(cac.begin ());
- it != cac.end (); ++it) {
- stricterConstraints_ [it->constraint] = it->both;
- stricterConstraints_ [it->complement] = it->both;
- }
}
-
- bool StatesPathFinder::findTransitions (GraphSearchData& d) const
- {
- // all potential solutions should be attempted before finding more
- if (d.idxSol < d.solutions.size()) return false;
- bool done = false;
- while (! d.queue1.empty() && !done)
- {
- GraphSearchData::state_with_depth_ptr_t _state = d.queue1.front();
-
- const GraphSearchData::state_with_depth& parent = d.getParent(_state);
- if (parent.l >= d.maxDepth) return true;
- d.queue1.pop_front();
-
- const Neighbors_t& neighbors = _state.state->first->neighbors();
- for (Neighbors_t::const_iterator _n = neighbors.begin();
- _n != neighbors.end(); ++_n) {
- EdgePtr_t transition = _n->second;
-
- // Don't even consider level set edges
- if (containsLevelSet(transition)) continue;
-
- // Avoid identical consecutive transition
- if (transition == parent.e) continue;
-
- // Avoid loop transitions
- if (isLoopTransition(transition)) continue;
-
- // Insert the end state of the new path to parent
- GraphSearchData::state_with_depth_ptr_t endState = d.addParent (_state, transition);
- d.queue1.push_back (endState);
-
- // done if last state is one of potential goal states
- if (std::find(
- d.s2.begin(), d.s2.end(), transition->stateTo()) != d.s2.end()) {
- done = true;
- d.solutions.push_back(endState);
- }
- }
- }
- // return true if search is exhausted and goal state not found
- if (!done) return true;
- return false;
+ constraints_.push_back(*it);
+ hppDout(info, "Adding constraint \"" << name << "\"");
+ hppDout(info, "Edge \"" << edge->name() << "\"");
+ hppDout(info, "parameter size: " << (*it)->parameterSize());
+ }
+ }
+ // constraint/both is the intersection of both constraint and
+ // constraint/complement
+ for (ConstraintsAndComplements_t::const_iterator it(cac.begin());
+ it != cac.end(); ++it) {
+ stricterConstraints_[it->constraint] = it->both;
+ stricterConstraints_[it->complement] = it->both;
+ }
+}
+
+bool StatesPathFinder::findTransitions(GraphSearchData& d) const {
+ // all potential solutions should be attempted before finding more
+ if (d.idxSol < d.solutions.size()) return false;
+ bool done = false;
+ while (!d.queue1.empty() && !done) {
+ GraphSearchData::state_with_depth_ptr_t _state = d.queue1.front();
+
+ const GraphSearchData::state_with_depth& parent = d.getParent(_state);
+ if (parent.l >= d.maxDepth) return true;
+ d.queue1.pop_front();
+
+ const Neighbors_t& neighbors = _state.state->first->neighbors();
+ for (Neighbors_t::const_iterator _n = neighbors.begin();
+ _n != neighbors.end(); ++_n) {
+ EdgePtr_t transition = _n->second;
+
+ // Don't even consider level set edges
+ if (containsLevelSet(transition)) continue;
+
+ // Avoid identical consecutive transition
+ if (transition == parent.e) continue;
+
+ // Avoid loop transitions
+ if (isLoopTransition(transition)) continue;
+
+ // Insert the end state of the new path to parent
+ GraphSearchData::state_with_depth_ptr_t endState =
+ d.addParent(_state, transition);
+ d.queue1.push_back(endState);
+
+ // done if last state is one of potential goal states
+ if (std::find(d.s2.begin(), d.s2.end(), transition->stateTo()) !=
+ d.s2.end()) {
+ done = true;
+ d.solutions.push_back(endState);
}
-
- Edges_t StatesPathFinder::getTransitionList (
- const GraphSearchData& d, const std::size_t& idxSol) const
- {
- Edges_t transitions;
- if (idxSol >= d.solutions.size()) return transitions;
-
- const GraphSearchData::state_with_depth_ptr_t endState = d.solutions[idxSol];
- const GraphSearchData::state_with_depth* current = &d.getParent(endState);
- transitions.reserve (current->l);
- graph::WaypointEdgePtr_t we;
- while (current->e) {
- assert (current->l > 0);
- we = HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, current->e);
- if (we) {
- for (int i = (int)we->nbWaypoints(); i >= 0; --i)
- transitions.push_back(we->waypoint(i));
- } else {
- transitions.push_back(current->e);
- }
- current = &d.parent1.at(current->s)[current->i];
- }
- std::reverse (transitions.begin(), transitions.end());
- return transitions;
+ }
+ }
+ // return true if search is exhausted and goal state not found
+ if (!done) return true;
+ return false;
+}
+
+Edges_t StatesPathFinder::getTransitionList(const GraphSearchData& d,
+ const std::size_t& idxSol) const {
+ Edges_t transitions;
+ if (idxSol >= d.solutions.size()) return transitions;
+
+ const GraphSearchData::state_with_depth_ptr_t endState = d.solutions[idxSol];
+ const GraphSearchData::state_with_depth* current = &d.getParent(endState);
+ transitions.reserve(current->l);
+ graph::WaypointEdgePtr_t we;
+ while (current->e) {
+ assert(current->l > 0);
+ we = HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, current->e);
+ if (we) {
+ for (int i = (int)we->nbWaypoints(); i >= 0; --i)
+ transitions.push_back(we->waypoint(i));
+ } else {
+ transitions.push_back(current->e);
+ }
+ current = &d.parent1.at(current->s)[current->i];
+ }
+ std::reverse(transitions.begin(), transitions.end());
+ return transitions;
+}
+
+struct StatesPathFinder::OptimizationData {
+ typedef constraints::solver::HierarchicalIterative::Saturation_t Saturation_t;
+ enum RightHandSideStatus_t {
+ // Constraint is not in solver for this waypoint
+ ABSENT,
+ // right hand side of constraint for this waypoint is equal to
+ // right hand side for previous waypoint
+ EQUAL_TO_PREVIOUS,
+ // right hand side of constraint for this waypoint is equal to
+ // right hand side for initial configuration
+ EQUAL_TO_INIT,
+ // right hand side of constraint for this waypoint is equal to
+ // right hand side for goal configuration
+ EQUAL_TO_GOAL
+ }; // enum RightHandSideStatus_t
+ const std::size_t N, nq, nv;
+ std::vector<Solver_t> solvers;
+ std::vector<bool> isTargetWaypoint;
+ // Waypoints lying in each intermediate state
+ matrix_t waypoint;
+ const Configuration_t q1;
+ Configuration_t q2;
+ core::DevicePtr_t robot;
+ // Matrix specifying for each constraint and each waypoint how
+ // the right hand side is initialized in the solver.
+ Eigen::Matrix<LiegroupElement, Eigen::Dynamic, Eigen::Dynamic> M_rhs;
+ Eigen::Matrix<RightHandSideStatus_t, Eigen::Dynamic, Eigen::Dynamic> M_status;
+ // Number of trials to generate each waypoint configuration
+ // _solveGoalConfig: whether we need to solve for goal configuration
+ OptimizationData(const core::ProblemConstPtr_t _problem,
+ const Configuration_t& _q1, const ConfigurationPtr_t& _q2,
+ const Edges_t& transitions, const bool _solveGoalConfig)
+ : N(_solveGoalConfig ? transitions.size() : transitions.size() - 1),
+ nq(_problem->robot()->configSize()),
+ nv(_problem->robot()->numberDof()),
+ solvers(N, _problem->robot()->configSpace()),
+ waypoint(nq, N),
+ q1(_q1),
+ robot(_problem->robot()),
+ M_rhs(),
+ M_status() {
+ if (!_solveGoalConfig) {
+ assert(_q2);
+ q2 = *_q2;
+ }
+ waypoint.setZero();
+ for (auto solver : solvers) {
+ // Set maximal number of iterations for each solver
+ solver.maxIterations(
+ _problem->getParameter("StatesPathFinder/maxIteration").intValue());
+ // Set error threshold for each solver
+ solver.errorThreshold(
+ _problem->getParameter("StatesPathFinder/errorThreshold")
+ .floatValue());
+ }
+ assert(transitions.size() > 0);
+ isTargetWaypoint.assign(transitions.size(), false);
+ for (std::size_t i = 0; i < transitions.size(); i++)
+ isTargetWaypoint[i] = transitions[i]->stateTo()->isWaypoint();
+ }
+};
+
+bool StatesPathFinder::checkConstantRightHandSide(size_type index) {
+ // a goal configuration is required to check if constraint is satisfied
+ // between initial and final configurations
+ assert(!goalDefinedByConstraints_);
+ OptimizationData& d = *optData_;
+ const ImplicitPtr_t c(constraints_[index]);
+ LiegroupElement rhsInit(c->function().outputSpace());
+ c->rightHandSideFromConfig(d.q1, rhsInit);
+ LiegroupElement rhsGoal(c->function().outputSpace());
+ c->rightHandSideFromConfig(d.q2, rhsGoal);
+ // Check that right hand sides are close to each other
+ value_type eps(problem_->constraintGraph()->errorThreshold());
+ value_type eps2(eps * eps);
+ if ((rhsGoal - rhsInit).squaredNorm() > eps2) {
+ return false;
+ }
+ // Matrix of solver right hand sides
+ for (size_type j = 0; j < d.M_rhs.cols(); ++j) {
+ d.M_rhs(index, j) = rhsInit;
+ }
+ return true;
+}
+
+bool StatesPathFinder::checkWaypointRightHandSide(std::size_t ictr,
+ std::size_t jslv) const {
+ const OptimizationData& d = *optData_;
+ ImplicitPtr_t c = constraints_[ictr]->copy();
+ LiegroupElement rhsNow(c->function().outputSpace());
+ assert(rhsNow.size() == c->rightHandSideSize());
+ c->rightHandSideFromConfig(d.waypoint.col(jslv), rhsNow);
+ c = constraints_[ictr]->copy();
+ LiegroupElement rhsOther(c->function().outputSpace());
+ switch (d.M_status(ictr, jslv)) {
+ case OptimizationData::EQUAL_TO_INIT:
+ c->rightHandSideFromConfig(d.q1, rhsOther);
+ break;
+ case OptimizationData::EQUAL_TO_GOAL:
+ assert(!goalDefinedByConstraints_);
+ c->rightHandSideFromConfig(d.q2, rhsOther);
+ break;
+ case OptimizationData::EQUAL_TO_PREVIOUS:
+ c->rightHandSideFromConfig(d.waypoint.col(jslv - 1), rhsOther);
+ break;
+ case OptimizationData::ABSENT:
+ default:
+ return true;
+ }
+ hpp::pinocchio::vector_t diff = rhsOther - rhsNow;
+ hpp::pinocchio::vector_t diffmask =
+ hpp::pinocchio::vector_t::Zero(diff.size());
+ for (auto k : c->activeRows()) // filter with constraint mask
+ for (size_type kk = k.first; kk < k.first + k.second; kk++)
+ diffmask[kk] = diff[kk];
+ value_type eps(problem_->constraintGraph()->errorThreshold());
+ value_type eps2(eps * eps);
+ return diffmask.squaredNorm() < eps2;
+}
+
+bool StatesPathFinder::checkWaypointRightHandSide(std::size_t jslv) const {
+ for (std::size_t ictr = 0; ictr < constraints_.size(); ictr++)
+ if (!checkWaypointRightHandSide(ictr, jslv)) return false;
+ return true;
+}
+
+void StatesPathFinder::displayRhsMatrix() {
+ OptimizationData& d = *optData_;
+ Eigen::Matrix<LiegroupElement, Eigen::Dynamic, Eigen::Dynamic>& m = d.M_rhs;
+
+ for (std::size_t i = 0; i < constraints_.size(); i++) {
+ const ImplicitPtr_t& constraint = constraints_[i];
+ for (std::size_t j = 0; j < d.solvers.size(); j++) {
+ const vectorIn_t& config = d.waypoint.col(j);
+ LiegroupElement le(constraint->function().outputSpace());
+ constraint->rightHandSideFromConfig(d.waypoint.col(j), le);
+ m(i, j) = le;
+ }
+ }
+
+ std::ostringstream oss;
+ oss.precision(2);
+ oss << "\\documentclass[12pt,landscape]{article}" << std::endl;
+ oss << "\\usepackage[a3paper]{geometry}" << std::endl;
+ oss << "\\begin {document}" << std::endl;
+
+ for (size_type ii = 0; ii < (m.cols() + 7) / 8; ii++) {
+ size_type j0 = ii * 8;
+ size_type j1 = std::min(ii * 8 + 8, m.cols());
+ size_type dj = j1 - j0;
+ oss << "\\begin {tabular}{";
+ for (size_type j = 0; j < dj + 2; ++j) oss << "c";
+ oss << "}" << std::endl;
+ oss << "Constraint & mask";
+ for (size_type j = j0; j < j1; ++j) oss << " & WP" << j;
+ oss << "\\\\" << std::endl;
+ for (size_type i = 0; i < m.rows(); ++i) {
+ std::vector<int> mask(constraints_[i]->parameterSize());
+ for (auto k : constraints_[i]->activeRows())
+ for (size_type kk = k.first; kk < k.first + k.second; kk++)
+ mask[kk] = 1;
+ std::ostringstream oss1;
+ oss1.precision(2);
+ std::ostringstream oss2;
+ oss2.precision(2);
+ oss1 << constraints_[i]->function().name() << " & ";
+
+ oss1 << "$\\left(\\begin{array}{c} ";
+ for (std::size_t k = 0; k < mask.size(); ++k) {
+ oss1 << mask[k] << "\\\\ ";
}
-
- struct StatesPathFinder::OptimizationData
- {
- typedef constraints::solver::HierarchicalIterative::Saturation_t
- Saturation_t;
- enum RightHandSideStatus_t {
- // Constraint is not in solver for this waypoint
- ABSENT,
- // right hand side of constraint for this waypoint is equal to
- // right hand side for previous waypoint
- EQUAL_TO_PREVIOUS,
- // right hand side of constraint for this waypoint is equal to
- // right hand side for initial configuration
- EQUAL_TO_INIT,
- // right hand side of constraint for this waypoint is equal to
- // right hand side for goal configuration
- EQUAL_TO_GOAL
- }; // enum RightHandSideStatus_t
- const std::size_t N, nq, nv;
- std::vector <Solver_t> solvers;
- std::vector <bool> isTargetWaypoint;
- // Waypoints lying in each intermediate state
- matrix_t waypoint;
- const Configuration_t q1;
- Configuration_t q2;
- core::DevicePtr_t robot;
- // Matrix specifying for each constraint and each waypoint how
- // the right hand side is initialized in the solver.
- Eigen::Matrix < LiegroupElement, Eigen::Dynamic, Eigen::Dynamic > M_rhs;
- Eigen::Matrix < RightHandSideStatus_t, Eigen::Dynamic, Eigen::Dynamic >
- M_status;
- // Number of trials to generate each waypoint configuration
- // _solveGoalConfig: whether we need to solve for goal configuration
- OptimizationData (const core::ProblemConstPtr_t _problem,
- const Configuration_t& _q1,
- const ConfigurationPtr_t& _q2,
- const Edges_t& transitions,
- const bool _solveGoalConfig
- ) :
- N (_solveGoalConfig? transitions.size (): transitions.size () - 1),
- nq (_problem->robot()->configSize()),
- nv (_problem->robot()->numberDof()),
- solvers (N, _problem->robot()->configSpace ()),
- waypoint (nq, N), q1 (_q1),
- robot (_problem->robot()),
- M_rhs (), M_status ()
- {
- if (!_solveGoalConfig) {
- assert (_q2);
- q2 = *_q2;
+ oss1 << "\\end{array}\\right)$" << std::endl;
+ oss1 << " & " << std::endl;
+
+ for (size_type j = j0; j < j1; ++j) {
+ if (d.M_status(i, j) != OptimizationData::ABSENT ||
+ (j < m.cols() - 1 &&
+ d.M_status(i, j + 1) == OptimizationData::EQUAL_TO_PREVIOUS)) {
+ oss2 << "$\\left(\\begin{array}{c} ";
+ for (size_type k = 0; k < m(i, j).size(); ++k) {
+ oss2 << ((abs(m(i, j).vector()[k]) < 1e-6) ? 0
+ : m(i, j).vector()[k])
+ << "\\\\ ";
}
- waypoint.setZero();
- for (auto solver: solvers){
- // Set maximal number of iterations for each solver
- solver.maxIterations(_problem->getParameter
- ("StatesPathFinder/maxIteration").intValue());
- // Set error threshold for each solver
- solver.errorThreshold(_problem->getParameter
- ("StatesPathFinder/errorThreshold").floatValue());
- }
- assert (transitions.size () > 0);
- isTargetWaypoint.assign(transitions.size(), false);
- for (std::size_t i = 0; i < transitions.size(); i++)
- isTargetWaypoint[i] = transitions[i]->stateTo()->isWaypoint();
- }
- };
-
- bool StatesPathFinder::checkConstantRightHandSide (size_type index)
- {
- // a goal configuration is required to check if constraint is satisfied
- // between initial and final configurations
- assert (!goalDefinedByConstraints_);
- OptimizationData& d = *optData_;
- const ImplicitPtr_t c (constraints_ [index]);
- LiegroupElement rhsInit(c->function().outputSpace());
- c->rightHandSideFromConfig (d.q1, rhsInit);
- LiegroupElement rhsGoal(c->function().outputSpace());
- c->rightHandSideFromConfig (d.q2, rhsGoal);
- // Check that right hand sides are close to each other
- value_type eps (problem_->constraintGraph ()->errorThreshold ());
- value_type eps2 (eps * eps);
- if ((rhsGoal - rhsInit).squaredNorm () > eps2) {
- return false;
+ oss2 << "\\end{array}\\right)$" << std::endl;
}
- // Matrix of solver right hand sides
- for (size_type j=0; j<d.M_rhs.cols (); ++j) {
- d.M_rhs (index, j) = rhsInit;
+ if (j < j1 - 1) {
+ oss2 << " & " << std::endl;
}
- return true;
}
-
- bool StatesPathFinder::checkWaypointRightHandSide
- (std::size_t ictr, std::size_t jslv) const
- {
- const OptimizationData& d = *optData_;
- ImplicitPtr_t c = constraints_ [ictr]->copy();
- LiegroupElement rhsNow(c->function().outputSpace());
- assert(rhsNow.size() == c->rightHandSideSize());
- c->rightHandSideFromConfig (d.waypoint.col (jslv), rhsNow);
- c = constraints_ [ictr]->copy();
- LiegroupElement rhsOther(c->function().outputSpace());
- switch (d.M_status(ictr, jslv)) {
- case OptimizationData::EQUAL_TO_INIT:
- c->rightHandSideFromConfig (d.q1, rhsOther);
- break;
- case OptimizationData::EQUAL_TO_GOAL:
- assert (!goalDefinedByConstraints_);
- c->rightHandSideFromConfig (d.q2, rhsOther);
- break;
- case OptimizationData::EQUAL_TO_PREVIOUS:
- c->rightHandSideFromConfig (d.waypoint.col (jslv-1), rhsOther);
- break;
- case OptimizationData::ABSENT:
- default:
- return true;
- }
- hpp::pinocchio::vector_t diff = rhsOther - rhsNow;
- hpp::pinocchio::vector_t diffmask= hpp::pinocchio::vector_t::Zero(diff.size());
- for (auto k: c->activeRows()) // filter with constraint mask
- for (size_type kk = k.first; kk < k.first + k.second; kk++)
- diffmask[kk] = diff[kk];
- value_type eps (problem_->constraintGraph ()->errorThreshold ());
- value_type eps2 (eps * eps);
- return diffmask.squaredNorm () < eps2;
+ std::string str2 = oss2.str();
+ if (str2.size() > 50) { // don't display constraints used nowhere
+ oss << oss1.str() << str2 << "\\\\" << std::endl;
}
-
- bool StatesPathFinder::checkWaypointRightHandSide
- (std::size_t jslv) const
- {
- for (std::size_t ictr = 0; ictr < constraints_.size(); ictr++)
- if (!checkWaypointRightHandSide(ictr, jslv))
- return false;
- return true;
+ }
+ oss << "\\end{tabular}" << std::endl << std::endl;
+ }
+ oss << "\\end{document}" << std::endl;
+
+ std::string s = oss.str();
+ std::string s2 = "";
+ for (std::size_t i = 0; i < s.size(); i++) {
+ if (s[i] == '_')
+ s2 += "\\_";
+ else
+ s2.push_back(s[i]);
+ }
+ hppDout(info, s2);
+}
+
+void StatesPathFinder::displayStatusMatrix(const graph::Edges_t& transitions) {
+ const Eigen::Matrix<OptimizationData::RightHandSideStatus_t, Eigen::Dynamic,
+ Eigen::Dynamic>& m = optData_->M_status;
+ std::ostringstream oss;
+ oss.precision(5);
+ oss << "\\documentclass[12pt,landscape]{article}" << std::endl;
+ oss << "\\usepackage[a3paper]{geometry}" << std::endl;
+ oss << "\\begin {document}" << std::endl;
+ oss << "\\paragraph{Edges}" << std::endl;
+ oss << "\\begin{enumerate}" << std::endl;
+ for (auto edge : transitions) {
+ oss << "\\item \\texttt{" << edge->name() << "}" << std::endl;
+ }
+ oss << "\\end{enumerate}" << std::endl;
+ oss << "\\begin {tabular}{l|";
+ for (size_type j = 0; j < m.cols(); ++j)
+ if (transitions[j]->stateTo()->isWaypoint())
+ oss << "c";
+ else
+ oss << "|c|";
+ oss << "|}" << std::endl;
+ oss << "Constraint";
+ for (size_type j = 0; j < m.cols(); ++j) oss << " & " << j + 1;
+ oss << "\\\\" << std::endl;
+ for (size_type i = 0; i < m.rows(); ++i) {
+ oss << "\\texttt{" << constraints_[i]->function().name() << "} & "
+ << std::endl;
+ for (size_type j = 0; j < m.cols(); ++j) {
+ oss << m(i, j);
+ if (j < m.cols() - 1) oss << " & ";
+ }
+ oss << "\\\\" << std::endl;
+ }
+ oss << "\\end{tabular}" << std::endl;
+ oss << "\\end{document}" << std::endl;
+
+ std::string s = oss.str();
+ std::string s2 = "";
+ for (std::size_t i = 0; i < s.size(); i++) {
+ if (s[i] == '_')
+ s2 += "\\_";
+ else
+ s2.push_back(s[i]);
+ }
+ hppDout(info, s2);
+}
+
+bool StatesPathFinder::contains(const Solver_t& solver,
+ const ImplicitPtr_t& c) const {
+ if (solver.contains(c)) return true;
+ std::map<ImplicitPtr_t, ImplicitPtr_t>::const_iterator it(
+ sameRightHandSide_.find(c));
+ if (it != sameRightHandSide_.end() && solver.contains(it->second))
+ return true;
+ return false;
+}
+
+bool StatesPathFinder::containsStricter(const Solver_t& solver,
+ const ImplicitPtr_t& c) const {
+ if (solver.contains(c)) return true;
+ std::map<ImplicitPtr_t, ImplicitPtr_t>::const_iterator it(
+ stricterConstraints_.find(c));
+ if (it != stricterConstraints_.end() && solver.contains(it->second))
+ return true;
+ return false;
+}
+
+bool StatesPathFinder::buildOptimizationProblem(
+ const graph::Edges_t& transitions) {
+ OptimizationData& d = *optData_;
+ // if no waypoint, check init and goal has same RHS
+ if (d.N == 0) {
+ assert(transitions.size() == 1);
+ assert(!goalDefinedByConstraints_);
+ size_type index = 0;
+ for (NumericalConstraints_t::const_iterator it(constraints_.begin());
+ it != constraints_.end(); ++it, ++index) {
+ const ImplicitPtr_t& c(*it);
+ // Get transition solver
+ const Solver_t& trSolver(
+ transitions[0]->pathConstraint()->configProjector()->solver());
+ if (!contains(trSolver, c)) continue;
+ if (!checkConstantRightHandSide(index)) return false;
+ }
+ return true;
+ }
+ d.M_status.resize(constraints_.size(), d.N);
+ d.M_status.fill(OptimizationData::ABSENT);
+ d.M_rhs.resize(constraints_.size(), d.N);
+ d.M_rhs.fill(LiegroupElement());
+ size_type index = 0;
+ // Loop over constraints
+ for (NumericalConstraints_t::const_iterator it(constraints_.begin());
+ it != constraints_.end(); ++it, ++index) {
+ const ImplicitPtr_t& c(*it);
+ // Loop forward over waypoints to determine right hand sides equal
+ // to initial configuration or previous configuration
+ for (std::size_t j = 0; j < d.N; ++j) {
+ // Get transition solver
+ const Solver_t& trSolver(
+ transitions[j]->pathConstraint()->configProjector()->solver());
+ if (!contains(trSolver, c)) continue;
+
+ if ((j == 0) ||
+ d.M_status(index, j - 1) == OptimizationData::EQUAL_TO_INIT) {
+ d.M_status(index, j) = OptimizationData::EQUAL_TO_INIT;
+ } else {
+ d.M_status(index, j) = OptimizationData::EQUAL_TO_PREVIOUS;
}
-
- void StatesPathFinder::displayRhsMatrix ()
- {
- OptimizationData& d = *optData_;
- Eigen::Matrix < LiegroupElement, Eigen::Dynamic, Eigen::Dynamic >& m
- = d.M_rhs;
-
- for (std::size_t i = 0; i < constraints_.size(); i++) {
- const ImplicitPtr_t& constraint = constraints_[i];
- for (std::size_t j = 0; j < d.solvers.size(); j++) {
- const vectorIn_t& config = d.waypoint.col(j);
- LiegroupElement le(constraint->function().outputSpace());
- constraint->rightHandSideFromConfig(d.waypoint.col(j), le);
- m(i,j) = le;
- }
- }
-
- std::ostringstream oss; oss.precision (2);
- oss << "\\documentclass[12pt,landscape]{article}" << std::endl;
- oss << "\\usepackage[a3paper]{geometry}" << std::endl;
- oss << "\\begin {document}" << std::endl;
-
- for (size_type ii = 0; ii < (m.cols()+7)/8; ii++) {
- size_type j0 = ii*8;
- size_type j1 = std::min(ii*8+8, m.cols());
- size_type dj = j1-j0;
- oss << "\\begin {tabular}{";
- for (size_type j=0; j<dj + 2; ++j)
- oss << "c";
- oss << "}" << std::endl;
- oss << "Constraint & mask";
- for (size_type j=j0; j<j1; ++j)
- oss << " & WP" << j;
- oss << "\\\\" << std::endl;
- for (size_type i=0; i<m.rows (); ++i) {
- std::vector<int> mask(constraints_[i]->parameterSize());
- for (auto k: constraints_[i]->activeRows())
- for (size_type kk = k.first; kk < k.first + k.second; kk++)
- mask[kk] = 1;
- std::ostringstream oss1; oss1.precision (2);
- std::ostringstream oss2; oss2.precision (2);
- oss1 << constraints_ [i]->function ().name () << " & ";
-
- oss1 << "$\\left(\\begin{array}{c} ";
- for (std::size_t k=0; k<mask.size (); ++k) {
- oss1 << mask[k] << "\\\\ ";
- }
- oss1 << "\\end{array}\\right)$" << std::endl;
- oss1 << " & " << std::endl;
-
- for (size_type j=j0; j<j1; ++j) {
- if (d.M_status(i,j) != OptimizationData::ABSENT || (j < m.cols () - 1 &&
- d.M_status(i,j+1) == OptimizationData::EQUAL_TO_PREVIOUS)) {
- oss2 << "$\\left(\\begin{array}{c} ";
- for (size_type k=0; k<m (i,j).size (); ++k) {
- oss2 << ((abs(m (i,j).vector()[k]) < 1e-6) ? 0 : m (i,j).vector()[k]) << "\\\\ ";
- }
- oss2 << "\\end{array}\\right)$" << std::endl;
- }
- if (j < j1 - 1) {
- oss2 << " & " << std::endl;
- }
- }
- std::string str2 = oss2.str();
- if (str2.size() > 50) { // don't display constraints used nowhere
- oss << oss1.str() << str2 << "\\\\" << std::endl;
- }
- }
- oss << "\\end{tabular}" << std::endl << std::endl;
- }
- oss << "\\end{document}" << std::endl;
-
- std::string s = oss.str ();
- std::string s2 = "";
- for (std::size_t i=0; i < s.size(); i++) {
- if (s[i] == '_') s2 += "\\_";
- else s2.push_back(s[i]);
- }
- hppDout (info, s2);
- }
-
- void StatesPathFinder::displayStatusMatrix (const graph::Edges_t& transitions)
- {
- const Eigen::Matrix < OptimizationData::RightHandSideStatus_t, Eigen::Dynamic, Eigen::Dynamic >&
- m = optData_->M_status;
- std::ostringstream oss; oss.precision (5);
- oss << "\\documentclass[12pt,landscape]{article}" << std::endl;
- oss << "\\usepackage[a3paper]{geometry}" << std::endl;
- oss << "\\begin {document}" << std::endl;
- oss << "\\paragraph{Edges}" << std::endl;
- oss << "\\begin{enumerate}" << std::endl;
- for (auto edge : transitions) {
- oss << "\\item \\texttt{" << edge->name() << "}" << std::endl;
- }
- oss << "\\end{enumerate}" << std::endl;
- oss << "\\begin {tabular}{l|";
- for (size_type j=0; j<m.cols (); ++j)
- if (transitions[j]->stateTo()->isWaypoint()) oss << "c";
- else oss << "|c|";
- oss << "|}" << std::endl;
- oss << "Constraint";
- for (size_type j=0; j<m.cols (); ++j)
- oss << " & " << j+1;
- oss << "\\\\" << std::endl;
- for (size_type i=0; i<m.rows (); ++i) {
- oss << "\\texttt{" << constraints_ [i]->function ().name () << "} & " << std::endl;
- for (size_type j=0; j<m.cols (); ++j) {
- oss << m (i,j);
- if (j < m.cols () - 1)
- oss << " & ";
+ }
+ // If the goal configuration is not given
+ // no need to determine if RHS equal to goal configuration
+ if (goalDefinedByConstraints_) {
+ continue;
+ }
+ // Loop backward over waypoints to determine right hand sides equal
+ // to final configuration
+ for (size_type j = d.N - 1; j > 0; --j) {
+ // Get transition solver
+ const Solver_t& trSolver(transitions[(std::size_t)j + 1]
+ ->pathConstraint()
+ ->configProjector()
+ ->solver());
+ if (!contains(trSolver, c)) break;
+
+ if ((j == (size_type)d.N - 1) ||
+ d.M_status(index, j + 1) == OptimizationData::EQUAL_TO_GOAL) {
+ // if constraint right hand side is already equal to
+ // initial config, check that right hand side is equal
+ // for init and goal configs.
+ if (d.M_status(index, j) == OptimizationData::EQUAL_TO_INIT) {
+ if (checkConstantRightHandSide(index)) {
+ // stop for this constraint
+ break;
+ } else {
+ // Right hand side of constraint should be equal along the
+ // whole path but is different at init and at goal configs.
+ return false;
}
- oss << "\\\\" << std::endl;
- }
- oss << "\\end{tabular}" << std::endl;
- oss << "\\end{document}" << std::endl;
-
- std::string s = oss.str ();
- std::string s2 = "";
- for (std::size_t i=0; i < s.size(); i++) {
- if (s[i] == '_') s2 += "\\_";
- else s2.push_back(s[i]);
+ } else {
+ d.M_status(index, j) = OptimizationData::EQUAL_TO_GOAL;
}
- hppDout (info, s2);
}
-
- bool StatesPathFinder::contains
- (const Solver_t& solver, const ImplicitPtr_t& c) const
- {
- if (solver.contains (c)) return true;
- std::map <ImplicitPtr_t, ImplicitPtr_t>::const_iterator it
- (sameRightHandSide_.find (c));
- if (it != sameRightHandSide_.end () && solver.contains (it->second))
- return true;
- return false;
- }
-
- bool StatesPathFinder::containsStricter
- (const Solver_t& solver, const ImplicitPtr_t& c) const
- {
- if (solver.contains (c)) return true;
- std::map <ImplicitPtr_t, ImplicitPtr_t>::const_iterator it
- (stricterConstraints_.find (c));
- if (it != stricterConstraints_.end() && solver.contains (it->second))
- return true;
- return false;
- }
-
- bool StatesPathFinder::buildOptimizationProblem
- (const graph::Edges_t& transitions)
- {
- OptimizationData& d = *optData_;
- // if no waypoint, check init and goal has same RHS
- if (d.N == 0) {
- assert (transitions.size() == 1);
- assert (!goalDefinedByConstraints_);
- size_type index = 0;
- for (NumericalConstraints_t::const_iterator it (constraints_.begin ());
- it != constraints_.end (); ++it, ++index) {
- const ImplicitPtr_t& c (*it);
- // Get transition solver
- const Solver_t& trSolver
- (transitions [0]->pathConstraint ()->configProjector ()->solver ());
- if (!contains (trSolver, c)) continue;
- if (!checkConstantRightHandSide (index)) return false;
- }
- return true;
- }
- d.M_status.resize (constraints_.size (), d.N);
- d.M_status.fill (OptimizationData::ABSENT);
- d.M_rhs.resize (constraints_.size (), d.N);
- d.M_rhs.fill (LiegroupElement ());
- size_type index = 0;
- // Loop over constraints
- for (NumericalConstraints_t::const_iterator it (constraints_.begin ());
- it != constraints_.end (); ++it, ++index) {
- const ImplicitPtr_t& c (*it);
- // Loop forward over waypoints to determine right hand sides equal
- // to initial configuration or previous configuration
- for (std::size_t j = 0; j < d.N; ++j) {
- // Get transition solver
- const Solver_t& trSolver
- (transitions [j]->pathConstraint ()->configProjector ()->solver ());
- if (!contains (trSolver, c)) continue;
-
- if ((j==0) || d.M_status (index, j-1) ==
- OptimizationData::EQUAL_TO_INIT) {
- d.M_status (index, j) = OptimizationData::EQUAL_TO_INIT;
- } else {
- d.M_status (index, j) = OptimizationData::EQUAL_TO_PREVIOUS;
- }
- }
- // If the goal configuration is not given
- // no need to determine if RHS equal to goal configuration
- if (goalDefinedByConstraints_) {
- continue;
- }
- // Loop backward over waypoints to determine right hand sides equal
- // to final configuration
- for (size_type j = d.N-1; j > 0; --j) {
- // Get transition solver
- const Solver_t& trSolver
- (transitions [(std::size_t)j+1]->pathConstraint ()->
- configProjector ()->solver ());
- if (!contains (trSolver, c)) break;
-
- if ((j==(size_type) d.N-1) || d.M_status (index, j+1) ==
- OptimizationData::EQUAL_TO_GOAL) {
- // if constraint right hand side is already equal to
- // initial config, check that right hand side is equal
- // for init and goal configs.
- if (d.M_status (index, j) ==
- OptimizationData::EQUAL_TO_INIT) {
- if (checkConstantRightHandSide (index)) {
- // stop for this constraint
- break;
- } else {
- // Right hand side of constraint should be equal along the
- // whole path but is different at init and at goal configs.
- return false;
- }
- } else {
- d.M_status (index, j) = OptimizationData::EQUAL_TO_GOAL;
- }
- }
- }
- } // for (NumericalConstraints_t::const_iterator it
+ }
+ } // for (NumericalConstraints_t::const_iterator it
#ifdef HPP_DEBUG
- displayStatusMatrix (transitions);
+ displayStatusMatrix(transitions);
#endif
- // Fill solvers with target constraints of transition
- for (std::size_t j = 0; j < d.N; ++j) {
- d.solvers [j] = transitions [j]->
- targetConstraint ()->configProjector ()->solver ();
- if (goalDefinedByConstraints_) {
- continue;
- }
- // when goal configuration is given, and if something
- // (eg relative pose of two objects grasping) is fixed until goal,
- // we need to propagate the constraint to an earlier solver,
- // otherwise the chance it solves for the correct config is very low
- if (j > 0 && j < d.N-1) {
- const Solver_t& otherSolver = transitions [j+1]->
- pathConstraint ()->configProjector ()->solver ();
- for (std::size_t i = 0; i < constraints_.size (); i++) {
- // transition from j-1 to j does not contain this constraint
- // transition from j to j+1 (all the way to goal) has constraint
- // constraint must be added to solve for waypoint at j (WP_j+1)
- if (d.M_status(i, j-1) == OptimizationData::ABSENT &&
- d.M_status(i, j) == OptimizationData::EQUAL_TO_GOAL &&
- !contains(d.solvers[j], constraints_[i]) &&
- otherSolver.contains(constraints_[i])) {
- d.solvers[j].add(constraints_[i]);
- hppDout(info, "Adding missing constraint " << constraints_[i]->function().name()
- << " to solver for waypoint" << j+1);
- }
- }
- }
- }
-
- // if goal is defined by constraints, some goal constraints may be
- // missing from the end state. we should add these constraints to solver
- // for the config in the final state
- if (goalDefinedByConstraints_) {
- for (auto goalConstraint: goalConstraints_) {
- if (!containsStricter(d.solvers [d.N-1], goalConstraint)) {
- d.solvers [d.N-1].add(goalConstraint);
- hppDout(info, "Adding goal constraint " << goalConstraint->function().name()
- << " to solver for waypoint" << d.N);
- }
- }
+ // Fill solvers with target constraints of transition
+ for (std::size_t j = 0; j < d.N; ++j) {
+ d.solvers[j] =
+ transitions[j]->targetConstraint()->configProjector()->solver();
+ if (goalDefinedByConstraints_) {
+ continue;
+ }
+ // when goal configuration is given, and if something
+ // (eg relative pose of two objects grasping) is fixed until goal,
+ // we need to propagate the constraint to an earlier solver,
+ // otherwise the chance it solves for the correct config is very low
+ if (j > 0 && j < d.N - 1) {
+ const Solver_t& otherSolver =
+ transitions[j + 1]->pathConstraint()->configProjector()->solver();
+ for (std::size_t i = 0; i < constraints_.size(); i++) {
+ // transition from j-1 to j does not contain this constraint
+ // transition from j to j+1 (all the way to goal) has constraint
+ // constraint must be added to solve for waypoint at j (WP_j+1)
+ if (d.M_status(i, j - 1) == OptimizationData::ABSENT &&
+ d.M_status(i, j) == OptimizationData::EQUAL_TO_GOAL &&
+ !contains(d.solvers[j], constraints_[i]) &&
+ otherSolver.contains(constraints_[i])) {
+ d.solvers[j].add(constraints_[i]);
+ hppDout(info, "Adding missing constraint "
+ << constraints_[i]->function().name()
+ << " to solver for waypoint" << j + 1);
}
-
- return true;
}
-
- bool StatesPathFinder::checkSolverRightHandSide
- (std::size_t ictr, std::size_t jslv) const
- {
- const OptimizationData& d = *optData_;
- ImplicitPtr_t c = constraints_ [ictr]->copy();
- const Solver_t& solver = d.solvers[jslv];
- vector_t rhs(c->rightHandSideSize());
- solver.getRightHandSide(c, rhs);
- LiegroupElement rhsNow(c->function().outputSpace());
- assert(rhsNow.size() == rhs.size());
- rhsNow.vector() = rhs;
- LiegroupElement rhsOther(c->function().outputSpace());
- switch (d.M_status(ictr, jslv)) {
- case OptimizationData::EQUAL_TO_INIT:
- c->rightHandSideFromConfig (d.q1, rhsOther);
- break;
- case OptimizationData::EQUAL_TO_GOAL:
- assert (!goalDefinedByConstraints_);
- c->rightHandSideFromConfig (d.q2, rhsOther);
- break;
- case OptimizationData::EQUAL_TO_PREVIOUS:
- c->rightHandSideFromConfig (d.waypoint.col (jslv-1), rhsOther);
- break;
- case OptimizationData::ABSENT:
- default:
- return true;
- }
- hpp::pinocchio::vector_t diff = rhsOther - rhsNow;
- hpp::pinocchio::vector_t diffmask= hpp::pinocchio::vector_t::Zero(diff.size());
- for (auto k: c->activeRows()) // filter with constraint mask
- for (size_type kk = k.first; kk < k.first + k.second; kk++)
- diffmask[kk] = diff[kk];
- value_type eps (problem_->constraintGraph ()->errorThreshold ());
- value_type eps2 (eps * eps);
- if (diffmask.squaredNorm () > eps2) hppDout(warning, diffmask.squaredNorm () << " vs " << eps2);
- return diffmask.squaredNorm () < eps2;
+ }
+ }
+
+ // if goal is defined by constraints, some goal constraints may be
+ // missing from the end state. we should add these constraints to solver
+ // for the config in the final state
+ if (goalDefinedByConstraints_) {
+ for (auto goalConstraint : goalConstraints_) {
+ if (!containsStricter(d.solvers[d.N - 1], goalConstraint)) {
+ d.solvers[d.N - 1].add(goalConstraint);
+ hppDout(info, "Adding goal constraint "
+ << goalConstraint->function().name()
+ << " to solver for waypoint" << d.N);
}
-
- bool StatesPathFinder::checkSolverRightHandSide
- (std::size_t jslv) const
- {
- for (std::size_t ictr = 0; ictr < constraints_.size(); ictr++)
- if (!checkSolverRightHandSide(ictr, jslv))
- return false;
- return true;
+ }
+ }
+
+ return true;
+}
+
+bool StatesPathFinder::checkSolverRightHandSide(std::size_t ictr,
+ std::size_t jslv) const {
+ const OptimizationData& d = *optData_;
+ ImplicitPtr_t c = constraints_[ictr]->copy();
+ const Solver_t& solver = d.solvers[jslv];
+ vector_t rhs(c->rightHandSideSize());
+ solver.getRightHandSide(c, rhs);
+ LiegroupElement rhsNow(c->function().outputSpace());
+ assert(rhsNow.size() == rhs.size());
+ rhsNow.vector() = rhs;
+ LiegroupElement rhsOther(c->function().outputSpace());
+ switch (d.M_status(ictr, jslv)) {
+ case OptimizationData::EQUAL_TO_INIT:
+ c->rightHandSideFromConfig(d.q1, rhsOther);
+ break;
+ case OptimizationData::EQUAL_TO_GOAL:
+ assert(!goalDefinedByConstraints_);
+ c->rightHandSideFromConfig(d.q2, rhsOther);
+ break;
+ case OptimizationData::EQUAL_TO_PREVIOUS:
+ c->rightHandSideFromConfig(d.waypoint.col(jslv - 1), rhsOther);
+ break;
+ case OptimizationData::ABSENT:
+ default:
+ return true;
+ }
+ hpp::pinocchio::vector_t diff = rhsOther - rhsNow;
+ hpp::pinocchio::vector_t diffmask =
+ hpp::pinocchio::vector_t::Zero(diff.size());
+ for (auto k : c->activeRows()) // filter with constraint mask
+ for (size_type kk = k.first; kk < k.first + k.second; kk++)
+ diffmask[kk] = diff[kk];
+ value_type eps(problem_->constraintGraph()->errorThreshold());
+ value_type eps2(eps * eps);
+ if (diffmask.squaredNorm() > eps2)
+ hppDout(warning, diffmask.squaredNorm() << " vs " << eps2);
+ return diffmask.squaredNorm() < eps2;
+}
+
+bool StatesPathFinder::checkSolverRightHandSide(std::size_t jslv) const {
+ for (std::size_t ictr = 0; ictr < constraints_.size(); ictr++)
+ if (!checkSolverRightHandSide(ictr, jslv)) return false;
+ return true;
+}
+
+bool StatesPathFinder::buildOptimizationProblemFromNames(
+ std::vector<std::string> names) {
+ graph::Edges_t transitions;
+ graph::GraphPtr_t cg(problem_->constraintGraph());
+ for (const std::string& name : names) {
+ for (std::size_t i = 0; i < cg->nbComponents(); ++i) {
+ graph::EdgePtr_t edge(
+ HPP_DYNAMIC_PTR_CAST(graph::Edge, cg->get(i).lock()));
+ if (edge && edge->name() == name) transitions.push_back(edge);
+ }
+ }
+ return buildOptimizationProblem(transitions);
+}
+
+void StatesPathFinder::preInitializeRHS(std::size_t j, Configuration_t& q) {
+ OptimizationData& d = *optData_;
+ Solver_t& solver(d.solvers[j]);
+ for (std::size_t i = 0; i < constraints_.size(); ++i) {
+ const ImplicitPtr_t& c(constraints_[i]);
+ bool ok = true;
+ switch (d.M_status((size_type)i, (size_type)j)) {
+ case OptimizationData::EQUAL_TO_INIT:
+ ok = solver.rightHandSideFromConfig(c, d.q1);
+ break;
+ case OptimizationData::EQUAL_TO_GOAL:
+ assert(!goalDefinedByConstraints_);
+ ok = solver.rightHandSideFromConfig(c, d.q2);
+ break;
+ case OptimizationData::EQUAL_TO_PREVIOUS:
+ ok = solver.rightHandSideFromConfig(c, q);
+ break;
+ case OptimizationData::ABSENT:
+ default:;
+ }
+ ok |= contains(solver, constraints_[i]);
+ if (!ok) {
+ std::ostringstream err_msg;
+ err_msg << "\nConstraint " << i << " missing for waypoint " << j + 1
+ << " (" << c->function().name() << ")\n"
+ << "The constraints in this solver are:\n";
+ for (const std::string& name : constraintNamesFromSolverAtWaypoint(j + 1))
+ err_msg << name << "\n";
+ hppDout(warning, err_msg.str());
+ }
+ assert(ok);
+ }
+}
+
+bool StatesPathFinder::analyseOptimizationProblem(
+ const graph::Edges_t& transitions) {
+ OptimizationData& d = *optData_;
+ size_type nTriesMax =
+ problem_->getParameter("StatesPathFinder/maxTriesCollisionAnalysis")
+ .intValue();
+ if (nTriesMax == 0) return true;
+ for (std::size_t wp = 1; wp <= d.solvers.size(); wp++) {
+ std::size_t j = wp - 1;
+ const Solver_t& solver = d.solvers[j];
+ using namespace core;
+ Solver_t::Status status;
+ size_type tries = 0;
+ Configuration_t q;
+ do {
+ q = *(problem()->configurationShooter()->shoot());
+ preInitializeRHS(j, q);
+ status = solver.solve(q, constraints::solver::lineSearch::Backtracking());
+ } while ((status != Solver_t::SUCCESS) && (++tries <= nTriesMax));
+ if (tries > nTriesMax) {
+ hppDout(info, "Collision analysis stopped at WP "
+ << wp << " because of too many bad solve statuses");
+ return false;
+ }
+ CollisionValidationPtr_t collisionValidations =
+ CollisionValidation::create(problem_->robot());
+ collisionValidations->checkParameterized(true);
+ collisionValidations->computeAllContacts(true);
+ ValidationReportPtr_t validationReport;
+ bool ok = true;
+ if (!collisionValidations->validate(q, validationReport)) {
+ AllCollisionsValidationReportPtr_t allReports =
+ HPP_DYNAMIC_PTR_CAST(AllCollisionsValidationReport, validationReport);
+ assert(allReports);
+ std::size_t nbReports = allReports->collisionReports.size();
+ hppDout(info, wp << " nbReports: " << nbReports);
+ for (std::size_t i = 0; i < nbReports; i++) {
+ CollisionValidationReportPtr_t& report =
+ allReports->collisionReports[i];
+ JointConstPtr_t j1 = report->object1->joint();
+ JointConstPtr_t j2 = report->object2->joint();
+ if (!j1 || !j2) continue;
+ const EdgePtr_t& edge = transitions[wp];
+ RelativeMotion::matrix_type m = edge->relativeMotion();
+ RelativeMotion::RelativeMotionType rmt =
+ m(RelativeMotion::idx(j1), RelativeMotion::idx(j2));
+ hppDout(info, "report " << i << " joints names \n"
+ << j1->name() << "\n"
+ << j2->name() << "\n"
+ << rmt);
+ if (rmt == RelativeMotion::RelativeMotionType::Unconstrained) continue;
+ ok = false;
+ break;
}
-
- bool StatesPathFinder::buildOptimizationProblemFromNames(std::vector<std::string> names)
- {
- graph::Edges_t transitions;
- graph::GraphPtr_t cg (problem_->constraintGraph ());
- for (const std::string& name: names) {
- for (std::size_t i = 0; i < cg->nbComponents (); ++i) {
- graph::EdgePtr_t edge (HPP_DYNAMIC_PTR_CAST (graph::Edge, cg->get (i).lock ()));
- if (edge && edge->name() == name)
- transitions.push_back(edge);
- }
- }
- return buildOptimizationProblem(transitions);
+ }
+ if (!ok) {
+ hppDout(info, "Analysis found a collision at WP " << wp);
+ return false;
+ }
+ hppDout(info, "Analysis at WP " << wp << " passed after " << tries
+ << " solve tries");
+ }
+ return true;
+}
+
+bool StatesPathFinder::analyseOptimizationProblem2(
+ const graph::Edges_t& transitions, core::ProblemConstPtr_t _problem) {
+ typedef constraints::JointConstPtr_t JointConstPtr_t;
+ typedef core::RelativeMotion RelativeMotion;
+
+ OptimizationData& d = *optData_;
+ // map from pair of joint indices to vectors of constraints
+ typedef std::map<std::pair<size_type, size_type>, NumericalConstraints_t>
+ JointConstraintMap;
+ JointConstraintMap jcmap;
+
+ // iterate over all the transitions, propagate only constrained pairs
+ for (std::size_t i = 0; i <= transitions.size() - 1; ++i) {
+ // get index of the transition
+ std::size_t transIdx = transitions.size() - 1 - i;
+ const EdgePtr_t& edge = transitions[transIdx];
+ RelativeMotion::matrix_type m = edge->relativeMotion();
+
+ // check through the pairs already existing in jcmap
+ JointConstraintMap::iterator it = jcmap.begin();
+ while (it != jcmap.end()) {
+ RelativeMotion::RelativeMotionType rmt =
+ m(it->first.first, it->first.second);
+ if (rmt == RelativeMotion::RelativeMotionType::Unconstrained) {
+ JointConstraintMap::iterator toErase = it;
+ ++it;
+ jcmap.erase(toErase);
+ } else {
+ ++it;
}
-
- void StatesPathFinder::preInitializeRHS(std::size_t j, Configuration_t& q) {
- OptimizationData& d = *optData_;
- Solver_t& solver (d.solvers [j]);
- for (std::size_t i=0; i<constraints_.size (); ++i) {
- const ImplicitPtr_t& c (constraints_ [i]);
- bool ok = true;
- switch (d.M_status ((size_type)i, (size_type)j)) {
- case OptimizationData::EQUAL_TO_INIT:
- ok = solver.rightHandSideFromConfig (c, d.q1);
- break;
- case OptimizationData::EQUAL_TO_GOAL:
- assert (!goalDefinedByConstraints_);
- ok = solver.rightHandSideFromConfig (c, d.q2);
- break;
- case OptimizationData::EQUAL_TO_PREVIOUS:
- ok = solver.rightHandSideFromConfig (c, q);
- break;
- case OptimizationData::ABSENT:
- default:
- ;
- }
- ok |= contains(solver, constraints_[i]);
- if(!ok) {
- std::ostringstream err_msg;
- err_msg << "\nConstraint " << i << " missing for waypoint " << j+1
- << " (" << c->function().name() << ")\n"
- << "The constraints in this solver are:\n";
- for (const std::string& name: constraintNamesFromSolverAtWaypoint(j+1))
- err_msg << name << "\n";
- hppDout(warning, err_msg.str());
- }
- assert(ok);
- }
- }
-
- bool StatesPathFinder::analyseOptimizationProblem
- (const graph::Edges_t& transitions)
- {
- OptimizationData& d = *optData_;
- size_type nTriesMax = problem_->getParameter
- ("StatesPathFinder/maxTriesCollisionAnalysis").intValue();
- if (nTriesMax == 0) return true;
- for (std::size_t wp = 1; wp <= d.solvers.size(); wp++) {
- std::size_t j = wp-1;
- const Solver_t& solver = d.solvers[j];
- using namespace core;
- Solver_t::Status status;
- size_type tries = 0;
- Configuration_t q;
- do {
- q = *(problem()->configurationShooter()->shoot());
- preInitializeRHS(j, q);
- status = solver.solve (q,
- constraints::solver::lineSearch::Backtracking ());
- } while ((status != Solver_t::SUCCESS) && (++tries <= nTriesMax));
- if (tries > nTriesMax) {
- hppDout(info, "Collision analysis stopped at WP " << wp
- << " because of too many bad solve statuses");
- return false;
- }
- CollisionValidationPtr_t collisionValidations = CollisionValidation::create(problem_->robot());
- collisionValidations->checkParameterized(true);
- collisionValidations->computeAllContacts(true);
- ValidationReportPtr_t validationReport;
- bool ok = true;
- if (!collisionValidations->validate (q, validationReport)) {
- AllCollisionsValidationReportPtr_t allReports = HPP_DYNAMIC_PTR_CAST(
- AllCollisionsValidationReport, validationReport);
- assert(allReports);
- std::size_t nbReports = allReports->collisionReports.size();
- hppDout(info, wp << " nbReports: " << nbReports);
- for (std::size_t i = 0; i < nbReports; i++) {
- CollisionValidationReportPtr_t& report = allReports->collisionReports[i];
- JointConstPtr_t j1 = report->object1->joint();
- JointConstPtr_t j2 = report->object2->joint();
- if (!j1 || !j2) continue;
- const EdgePtr_t& edge = transitions[wp];
- RelativeMotion::matrix_type m = edge->relativeMotion();
- RelativeMotion::RelativeMotionType rmt =
- m(RelativeMotion::idx(j1), RelativeMotion::idx(j2));
- hppDout(info, "report " << i << " joints names \n" <<
- j1->name() << "\n" << j2->name() << "\n" << rmt);
- if (rmt == RelativeMotion::RelativeMotionType::Unconstrained)
- continue;
- ok = false;
- break;
- }
- }
- if (!ok) {
- hppDout(info, "Analysis found a collision at WP " << wp);
- return false;
- }
- hppDout(info, "Analysis at WP " << wp << " passed after " << tries << " solve tries");
- }
- return true;
+ }
+ NumericalConstraints_t currentConstraints;
+ if (transIdx == transitions.size() - 1 && !goalDefinedByConstraints_) {
+ // get the constraints from the goal state
+ currentConstraints = transitions[transIdx]
+ ->targetConstraint()
+ ->configProjector()
+ ->solver()
+ .constraints();
+ } else {
+ currentConstraints = d.solvers[transIdx].constraints();
+ }
+ // loop through all constraints in the target node of the transition
+ for (auto constraint : currentConstraints) {
+ std::pair<JointConstPtr_t, JointConstPtr_t> jointPair =
+ constraint->functionPtr()->dependsOnRelPoseBetween(_problem->robot());
+ JointConstPtr_t joint1 = jointPair.first;
+ size_type index1 = RelativeMotion::idx(joint1);
+ JointConstPtr_t joint2 = jointPair.second;
+ size_type index2 = RelativeMotion::idx(joint2);
+
+ // ignore constraint if it involves the same joint
+ if (index1 == index2) continue;
+
+ // check that the two joints are constrained in the transition
+ RelativeMotion::RelativeMotionType rmt = m(index1, index2);
+ if (rmt == RelativeMotion::RelativeMotionType::Unconstrained) continue;
+
+ // insert if necessary
+ JointConstraintMap::iterator next =
+ jcmap
+ .insert(JointConstraintMap::value_type(
+ std::make_pair(index1, index2), NumericalConstraints_t()))
+ .first;
+ // if constraint is not in map, insert it
+ if (find_if(next->second.begin(), next->second.end(),
+ [&constraint](const ImplicitPtr_t& arg) {
+ return *arg == *constraint;
+ }) == next->second.end()) {
+ next->second.push_back(constraint);
}
-
- bool StatesPathFinder::analyseOptimizationProblem2
- (const graph::Edges_t& transitions, core::ProblemConstPtr_t _problem)
- {
- typedef constraints::JointConstPtr_t JointConstPtr_t;
- typedef core::RelativeMotion RelativeMotion;
-
- OptimizationData& d = *optData_;
- // map from pair of joint indices to vectors of constraints
- typedef std::map<std::pair<size_type, size_type>, NumericalConstraints_t> JointConstraintMap;
- JointConstraintMap jcmap;
-
- // iterate over all the transitions, propagate only constrained pairs
- for (std::size_t i = 0; i <= transitions.size() - 1; ++i) {
- // get index of the transition
- std::size_t transIdx = transitions.size() - 1 - i;
- const EdgePtr_t& edge = transitions[transIdx];
- RelativeMotion::matrix_type m = edge->relativeMotion();
-
- // check through the pairs already existing in jcmap
- JointConstraintMap::iterator it = jcmap.begin();
- while (it != jcmap.end()) {
- RelativeMotion::RelativeMotionType rmt =
- m(it->first.first, it->first.second);
- if (rmt == RelativeMotion::RelativeMotionType::Unconstrained) {
- JointConstraintMap::iterator toErase = it;
- ++it;
- jcmap.erase(toErase);
- } else {
- ++it;
- }
- }
- NumericalConstraints_t currentConstraints;
- if (transIdx == transitions.size() - 1 && !goalDefinedByConstraints_) {
- // get the constraints from the goal state
- currentConstraints = transitions [transIdx]->
- targetConstraint ()->configProjector ()->solver ().constraints();
- } else {
- currentConstraints = d.solvers [transIdx].constraints();
- }
- // loop through all constraints in the target node of the transition
- for (auto constraint: currentConstraints) {
- std::pair<JointConstPtr_t, JointConstPtr_t> jointPair =
- constraint->functionPtr()->dependsOnRelPoseBetween(_problem->robot());
- JointConstPtr_t joint1 = jointPair.first;
- size_type index1 = RelativeMotion::idx(joint1);
- JointConstPtr_t joint2 = jointPair.second;
- size_type index2 = RelativeMotion::idx(joint2);
-
- // ignore constraint if it involves the same joint
- if (index1 == index2) continue;
-
- // check that the two joints are constrained in the transition
- RelativeMotion::RelativeMotionType rmt = m(index1, index2);
- if (rmt == RelativeMotion::RelativeMotionType::Unconstrained)
- continue;
-
- // insert if necessary
- JointConstraintMap::iterator next = jcmap.insert(
- JointConstraintMap::value_type(
- std::make_pair(index1, index2), NumericalConstraints_t ()
- )
- ).first;
- // if constraint is not in map, insert it
- if (find_if(next->second.begin(), next->second.end(),
- [&constraint](const ImplicitPtr_t& arg)
- { return *arg == *constraint; }) == next->second.end()) {
- next->second.push_back(constraint);
- }
- }
- }
- // at this point jcmap contains all the constraints that
- // - depend on relative pose between 2 joints j1 and j2,
- // - are present in the solver of any waypoint wp_i
- // - j1 and j2 are constrained by all the transitions from q_init to wp_i
- //
- // in the following we test that q_init satisfies the above constraints
- // otherwise the list of transitions is discarded
- if (jcmap.size() == 0) {
- return true;
- }
-
- Solver_t analyseSolver (_problem->robot()->configSpace ());
- analyseSolver.errorThreshold(_problem->getParameter
- ("StatesPathFinder/errorThreshold").floatValue());
- // iterate through all the pairs that are left,
- // and check that the initial config satisfies all the constraints
- for (JointConstraintMap::iterator it (jcmap.begin());
- it != jcmap.end(); it++) {
- NumericalConstraints_t constraintList = it->second;
- hppDout(info, "Constraints involving joints " << it->first.first
- << " and " << it->first.second << " should be satisfied at q init");
- for (NumericalConstraints_t::iterator ctrIt (constraintList.begin());
- ctrIt != constraintList.end(); ++ctrIt) {
- analyseSolver.add((*ctrIt)->copy());
- }
- }
- // initialize the right hand side with the initial config
- analyseSolver.rightHandSideFromConfig(*q1_);
- if (analyseSolver.isSatisfied(*q1_)) {
- return true;
- }
- hppDout(info, "Analysis found initial configuration does not satisfy constraint");
+ }
+ }
+ // at this point jcmap contains all the constraints that
+ // - depend on relative pose between 2 joints j1 and j2,
+ // - are present in the solver of any waypoint wp_i
+ // - j1 and j2 are constrained by all the transitions from q_init to wp_i
+ //
+ // in the following we test that q_init satisfies the above constraints
+ // otherwise the list of transitions is discarded
+ if (jcmap.size() == 0) {
+ return true;
+ }
+
+ Solver_t analyseSolver(_problem->robot()->configSpace());
+ analyseSolver.errorThreshold(
+ _problem->getParameter("StatesPathFinder/errorThreshold").floatValue());
+ // iterate through all the pairs that are left,
+ // and check that the initial config satisfies all the constraints
+ for (JointConstraintMap::iterator it(jcmap.begin()); it != jcmap.end();
+ it++) {
+ NumericalConstraints_t constraintList = it->second;
+ hppDout(info, "Constraints involving joints "
+ << it->first.first << " and " << it->first.second
+ << " should be satisfied at q init");
+ for (NumericalConstraints_t::iterator ctrIt(constraintList.begin());
+ ctrIt != constraintList.end(); ++ctrIt) {
+ analyseSolver.add((*ctrIt)->copy());
+ }
+ }
+ // initialize the right hand side with the initial config
+ analyseSolver.rightHandSideFromConfig(*q1_);
+ if (analyseSolver.isSatisfied(*q1_)) {
+ return true;
+ }
+ hppDout(info,
+ "Analysis found initial configuration does not satisfy constraint");
+ return false;
+}
+
+void StatesPathFinder::initializeRHS(std::size_t j) {
+ OptimizationData& d = *optData_;
+ Solver_t& solver(d.solvers[j]);
+ for (std::size_t i = 0; i < constraints_.size(); ++i) {
+ const ImplicitPtr_t& c(constraints_[i]);
+ bool ok = true;
+ switch (d.M_status((size_type)i, (size_type)j)) {
+ case OptimizationData::EQUAL_TO_PREVIOUS:
+ assert(j != 0);
+ ok = solver.rightHandSideFromConfig(c, d.waypoint.col(j - 1));
+ break;
+ case OptimizationData::EQUAL_TO_INIT:
+ ok = solver.rightHandSideFromConfig(c, d.q1);
+ break;
+ case OptimizationData::EQUAL_TO_GOAL:
+ assert(!goalDefinedByConstraints_);
+ ok = solver.rightHandSideFromConfig(c, d.q2);
+ break;
+ case OptimizationData::ABSENT:
+ default:;
+ }
+ ok |= contains(solver, constraints_[i]);
+ if (!ok) {
+ std::ostringstream err_msg;
+ err_msg << "\nConstraint " << i << " missing for waypoint " << j + 1
+ << " (" << c->function().name() << ")\n"
+ << "The constraints in this solver are:\n";
+ for (const std::string& name : constraintNamesFromSolverAtWaypoint(j + 1))
+ err_msg << name << "\n";
+ hppDout(warning, err_msg.str());
+ }
+ assert(ok);
+ }
+}
+
+void StatesPathFinder::initWPRandom(std::size_t wp) {
+ assert(wp >= 1 && wp <= (std::size_t)optData_->waypoint.cols());
+ initializeRHS(wp - 1);
+ optData_->waypoint.col(wp - 1) =
+ *(problem()->configurationShooter()->shoot());
+}
+void StatesPathFinder::initWPNear(std::size_t wp) {
+ assert(wp >= 1 && wp <= (std::size_t)optData_->waypoint.cols());
+ initializeRHS(wp - 1);
+ if (wp == 1)
+ optData_->waypoint.col(wp - 1) = optData_->q1;
+ else
+ optData_->waypoint.col(wp - 1) = optData_->waypoint.col(wp - 2);
+}
+void StatesPathFinder::initWP(std::size_t wp, ConfigurationIn_t q) {
+ assert(wp >= 1 && wp <= (std::size_t)optData_->waypoint.cols());
+ initializeRHS(wp - 1);
+ optData_->waypoint.col(wp - 1) = q;
+}
+
+StatesPathFinder::SolveStepStatus StatesPathFinder::solveStep(std::size_t wp) {
+ assert(wp >= 1 && wp <= (std::size_t)optData_->waypoint.cols());
+ std::size_t j = wp - 1;
+ Solver_t& solver(optData_->solvers[j]);
+ Solver_t::Status status =
+ solver.solve(optData_->waypoint.col(j),
+ constraints::solver::lineSearch::Backtracking());
+ if (status == Solver_t::SUCCESS) {
+ assert(checkWaypointRightHandSide(j));
+ const graph::Edges_t& edges = lastBuiltTransitions_;
+ core::ValidationReportPtr_t report;
+ // check collision based on preceeding edge to the waypoint
+ if (!edges[j]->pathValidation()->validate(optData_->waypoint.col(j),
+ report))
+ return SolveStepStatus::COLLISION_BEFORE;
+ // if wp is not the goal node, check collision based on following edge
+ if (j < edges.size() - 1 && !edges[j + 1]->pathValidation()->validate(
+ optData_->waypoint.col(j), report)) {
+ return SolveStepStatus::COLLISION_AFTER;
+ }
+ return SolveStepStatus::VALID_SOLUTION;
+ }
+ return SolveStepStatus::NO_SOLUTION;
+}
+
+std::string StatesPathFinder::displayConfigsSolved() const {
+ const OptimizationData& d = *optData_;
+ std::ostringstream oss;
+ oss << "configs = [" << std::endl;
+ oss << " " << pinocchio::displayConfig(d.q1) << ", # 0" << std::endl;
+ for (size_type j = 0; j < d.waypoint.cols(); j++)
+ oss << " " << pinocchio::displayConfig(d.waypoint.col(j)) << ", # "
+ << j + 1 << std::endl;
+ if (!goalDefinedByConstraints_) {
+ oss << " " << pinocchio::displayConfig(d.q2) << " # "
+ << d.waypoint.cols() + 1 << std::endl;
+ }
+ oss << "]" << std::endl;
+ std::string ans = oss.str();
+ hppDout(info, ans);
+ return ans;
+}
+
+bool StatesPathFinder::solveOptimizationProblem() {
+ OptimizationData& d = *optData_;
+ // Try to solve with sets of random configs for each waypoint
+ std::size_t nTriesMax =
+ problem_->getParameter("StatesPathFinder/nTriesUntilBacktrack")
+ .intValue();
+ std::size_t nTriesMax1 = nTriesMax * 10; // more tries for the first waypoint
+ std::size_t nFailsMax =
+ nTriesMax * 20; // fails before reseting the whole solution
+ std::size_t nBadSolvesMax =
+ nTriesMax * 50; // bad solve fails before reseting the whole solution
+ std::vector<std::size_t> nTriesDone(d.solvers.size() + 1, 0);
+ std::size_t nFails = 0;
+ std::size_t nBadSolves = 0;
+ std::size_t wp = 1; // waypoint index starting at 1 (wp 0 = q1)
+ std::size_t wp_max = 0; // all waypoints up to this index are valid solved
+ matrix_t longestSolved(d.nq, d.N);
+ longestSolved.setZero();
+ while (wp <= d.solvers.size()) {
+ if (wp == 1) {
+ // stop if number of tries for the first waypoint exceeds
+ if (nTriesDone[1] >= nTriesMax1) {
+ // if cannot solve all the way, return longest VALID sequence
+ d.waypoint = longestSolved;
+ displayConfigsSolved();
return false;
}
-
- void StatesPathFinder::initializeRHS(std::size_t j) {
- OptimizationData& d = *optData_;
- Solver_t& solver (d.solvers [j]);
- for (std::size_t i=0; i<constraints_.size (); ++i) {
- const ImplicitPtr_t& c (constraints_ [i]);
- bool ok = true;
- switch (d.M_status ((size_type)i, (size_type)j)) {
- case OptimizationData::EQUAL_TO_PREVIOUS:
- assert (j != 0);
- ok = solver.rightHandSideFromConfig (c, d.waypoint.col (j-1));
- break;
- case OptimizationData::EQUAL_TO_INIT:
- ok = solver.rightHandSideFromConfig (c, d.q1);
- break;
- case OptimizationData::EQUAL_TO_GOAL:
- assert (!goalDefinedByConstraints_);
- ok = solver.rightHandSideFromConfig (c, d.q2);
- break;
- case OptimizationData::ABSENT:
- default:
- ;
- }
- ok |= contains(solver, constraints_[i]);
- if(!ok) {
- std::ostringstream err_msg;
- err_msg << "\nConstraint " << i << " missing for waypoint " << j+1
- << " (" << c->function().name() << ")\n"
- << "The constraints in this solver are:\n";
- for (const std::string& name: constraintNamesFromSolverAtWaypoint(j+1))
- err_msg << name << "\n";
- hppDout(warning, err_msg.str());
- }
- assert(ok);
- }
+ // Reset the fail counter while the solution is empty
+ nFails = 0;
+ nBadSolves = 0;
+
+ } else if (nFails >= nFailsMax || nBadSolves >= nBadSolvesMax) {
+ // Completely reset a solution when too many tries have failed
+
+ // update the longest valid sequence of waypoints solved
+ if (wp - 1 > wp_max) {
+ // update the maximum index of valid waypoint
+ wp_max = wp - 1;
+ // save the sequence
+ longestSolved.leftCols(wp_max) = d.waypoint.leftCols(wp_max);
}
- void StatesPathFinder::initWPRandom(std::size_t wp) {
- assert(wp >=1 && wp <= (std::size_t) optData_->waypoint.cols());
- initializeRHS(wp-1);
- optData_->waypoint.col (wp-1) = *(problem()->configurationShooter()->shoot());
- }
- void StatesPathFinder::initWPNear(std::size_t wp) {
- assert(wp >=1 && wp <= (std::size_t) optData_->waypoint.cols());
- initializeRHS(wp-1);
- if (wp == 1)
- optData_->waypoint.col (wp-1) = optData_->q1;
- else
- optData_->waypoint.col (wp-1) = optData_->waypoint.col (wp-2);
- }
- void StatesPathFinder::initWP(std::size_t wp, ConfigurationIn_t q) {
- assert(wp >=1 && wp <= (std::size_t) optData_->waypoint.cols());
- initializeRHS(wp-1);
- optData_->waypoint.col (wp-1) = q;
- }
+ std::fill(nTriesDone.begin() + 2, nTriesDone.end(), 0);
+ wp = 1;
- StatesPathFinder::SolveStepStatus StatesPathFinder::solveStep(std::size_t wp) {
- assert(wp >=1 && wp <= (std::size_t) optData_->waypoint.cols());
- std::size_t j = wp-1;
- Solver_t& solver (optData_->solvers [j]);
- Solver_t::Status status = solver.solve (optData_->waypoint.col (j),
- constraints::solver::lineSearch::Backtracking ());
- if (status == Solver_t::SUCCESS) {
- assert (checkWaypointRightHandSide(j));
- const graph::Edges_t& edges = lastBuiltTransitions_;
- core::ValidationReportPtr_t report;
- // check collision based on preceeding edge to the waypoint
- if (!edges[j]->pathValidation()->validate (
- optData_->waypoint.col (j), report))
- return SolveStepStatus::COLLISION_BEFORE;
- // if wp is not the goal node, check collision based on following edge
- if (j < edges.size()-1 && !edges[j+1]->pathValidation()->validate (
- optData_->waypoint.col (j), report)) {
- return SolveStepStatus::COLLISION_AFTER;
- }
- return SolveStepStatus::VALID_SOLUTION;
- }
- return SolveStepStatus::NO_SOLUTION;
+#ifdef HPP_DEBUG
+ if (nFails >= nFailsMax) {
+ hppDout(warning, " Solution "
+ << graphData_->idxSol
+ << ": too many collisions. Resetting back to WP1");
}
-
- std::string StatesPathFinder::displayConfigsSolved() const {
- const OptimizationData& d = *optData_;
- std::ostringstream oss;
- oss << "configs = [" << std::endl;
- oss << " " << pinocchio::displayConfig(d.q1) << ", # 0" << std::endl;
- for (size_type j = 0; j < d.waypoint.cols(); j++)
- oss << " " << pinocchio::displayConfig(d.waypoint.col(j)) << ", # " << j+1 << std::endl;
- if (!goalDefinedByConstraints_) {
- oss << " " << pinocchio::displayConfig(d.q2) << " # " << d.waypoint.cols()+1 << std::endl;
- }
- oss << "]" << std::endl;
- std::string ans = oss.str();
- hppDout(info, ans);
- return ans;
+ if (nBadSolves >= nBadSolvesMax) {
+ hppDout(warning,
+ " Solution "
+ << graphData_->idxSol
+ << ": too many bad solve statuses. Resetting back to WP1");
}
-
- bool StatesPathFinder::solveOptimizationProblem ()
- {
- OptimizationData& d = *optData_;
- // Try to solve with sets of random configs for each waypoint
- std::size_t nTriesMax = problem_->getParameter
- ("StatesPathFinder/nTriesUntilBacktrack").intValue();
- std::size_t nTriesMax1 = nTriesMax*10; // more tries for the first waypoint
- std::size_t nFailsMax = nTriesMax*20; // fails before reseting the whole solution
- std::size_t nBadSolvesMax = nTriesMax*50; // bad solve fails before reseting the whole solution
- std::vector<std::size_t> nTriesDone(d.solvers.size()+1, 0);
- std::size_t nFails = 0;
- std::size_t nBadSolves = 0;
- std::size_t wp = 1; // waypoint index starting at 1 (wp 0 = q1)
- std::size_t wp_max = 0; // all waypoints up to this index are valid solved
- matrix_t longestSolved(d.nq, d.N);
- longestSolved.setZero();
- while (wp <= d.solvers.size()) {
- if (wp == 1) {
- // stop if number of tries for the first waypoint exceeds
- if (nTriesDone[1] >= nTriesMax1) {
- // if cannot solve all the way, return longest VALID sequence
- d.waypoint = longestSolved;
- displayConfigsSolved();
- return false;
- }
- // Reset the fail counter while the solution is empty
- nFails = 0;
- nBadSolves = 0;
-
- } else if (nFails >= nFailsMax || nBadSolves >= nBadSolvesMax) {
- // Completely reset a solution when too many tries have failed
-
- // update the longest valid sequence of waypoints solved
- if (wp - 1 > wp_max) {
- // update the maximum index of valid waypoint
- wp_max = wp - 1;
- // save the sequence
- longestSolved.leftCols(wp_max) = d.waypoint.leftCols(wp_max);
- }
-
- std::fill(nTriesDone.begin()+2, nTriesDone.end(), 0);
- wp = 1;
-
-#ifdef HPP_DEBUG
- if (nFails >= nFailsMax) {
- hppDout (warning, " Solution " << graphData_->idxSol << ": too many collisions. Resetting back to WP1");
- }
- if (nBadSolves >= nBadSolvesMax) {
- hppDout (warning, " Solution " << graphData_->idxSol << ": too many bad solve statuses. Resetting back to WP1");
- }
#endif
- continue;
-
- } else if (nTriesDone[wp] >= nTriesMax) {
- // enough tries for a waypoint: backtrack
+ continue;
- // update the longest valid sequence of waypoints solved
- if (wp -1 > wp_max) {
- // update the maximum index of valid waypoint
- wp_max = wp - 1;
- // save the sequence
- longestSolved.leftCols(wp_max) = d.waypoint.leftCols(wp_max);
- }
+ } else if (nTriesDone[wp] >= nTriesMax) {
+ // enough tries for a waypoint: backtrack
- do {
- nTriesDone[wp] = 0;
- wp--; // backtrack: try a new solution for the latest waypoint
- } while (wp>1 && (d.isTargetWaypoint[wp-1] || nTriesDone[wp] >= nTriesMax));
-
- continue;
- }
-
- // Initialize right hand sides, and
- // Choose a starting configuration for the solver.solve method:
- // - from previous waypoint if it's the first time we see this solver
- // given current solvers 0 to j-1
- // - with a random configuration if the other initialization has been
- // tried and failed
- if (nTriesDone[wp] == 0)
- initWPNear(wp);
- else
- initWPRandom(wp);
-
- nTriesDone[wp]++; // Backtrack to last state when this gets too big
-
- SolveStepStatus out = solveStep(wp);
- hppDout(info, "solveStep exit code at WP" << wp << ": " << out);
- switch (out) {
- case SolveStepStatus::VALID_SOLUTION: // Valid solution, go to next waypoint
- wp++; break;
- case SolveStepStatus::NO_SOLUTION: // Bad solve status, considered usual so has higher threshold before going back to first waypoint
- nBadSolves++; break;
- case SolveStepStatus::COLLISION_BEFORE: // Collision. If that happens too much, go back to first waypoint
- case SolveStepStatus::COLLISION_AFTER:
- nFails++; break;
- default:
- throw(std::logic_error("Unintended exit code for solveStep"));
- }
- }
-
- displayConfigsSolved();
- // displayRhsMatrix ();
-
- return true;
+ // update the longest valid sequence of waypoints solved
+ if (wp - 1 > wp_max) {
+ // update the maximum index of valid waypoint
+ wp_max = wp - 1;
+ // save the sequence
+ longestSolved.leftCols(wp_max) = d.waypoint.leftCols(wp_max);
}
- // Get list of configurations from solution of optimization problem
- core::Configurations_t StatesPathFinder::getConfigList () const
- {
- OptimizationData& d = *optData_;
- core::Configurations_t pv;
- ConfigurationPtr_t q1 (new Configuration_t (d.q1));
- pv.push_back(q1);
- for (std::size_t i = 0; i < d.N; ++i) {
- ConfigurationPtr_t q (new Configuration_t (d.waypoint.col (i)));
- pv.push_back(q);
- }
- if (!goalDefinedByConstraints_) {
- ConfigurationPtr_t q2 (new Configuration_t (d.q2));
- pv.push_back(q2);
- }
- return pv;
- }
-
- // Loop over all the possible paths in the constraint graph between
- // the state of the initial configuration
- // and either [the state of the final configurations if given] OR
- // [one of potential goal states if goal defined as set of constraints]
- // and compute waypoint configurations in each state.
- core::Configurations_t StatesPathFinder::computeConfigList (
- ConfigurationIn_t q1, ConfigurationPtr_t q2)
- {
- const graph::GraphPtr_t& graph(problem_->constraintGraph ());
- GraphSearchData& d = *graphData_;
-
- size_t& idxSol = d.idxSol;
-
- bool maxDepthReached;
- while (!(maxDepthReached = findTransitions (d))) { // mut
- // if there is a working sequence, try it first before getting another transition list
- Edges_t transitions = (nTryConfigList_ > 0)? lastBuiltTransitions_ : getTransitionList (d, idxSol); // const, const
- while (! transitions.empty()) {
+ do {
+ nTriesDone[wp] = 0;
+ wp--; // backtrack: try a new solution for the latest waypoint
+ } while (wp > 1 &&
+ (d.isTargetWaypoint[wp - 1] || nTriesDone[wp] >= nTriesMax));
+
+ continue;
+ }
+
+ // Initialize right hand sides, and
+ // Choose a starting configuration for the solver.solve method:
+ // - from previous waypoint if it's the first time we see this solver
+ // given current solvers 0 to j-1
+ // - with a random configuration if the other initialization has been
+ // tried and failed
+ if (nTriesDone[wp] == 0)
+ initWPNear(wp);
+ else
+ initWPRandom(wp);
+
+ nTriesDone[wp]++; // Backtrack to last state when this gets too big
+
+ SolveStepStatus out = solveStep(wp);
+ hppDout(info, "solveStep exit code at WP" << wp << ": " << out);
+ switch (out) {
+ case SolveStepStatus::VALID_SOLUTION: // Valid solution, go to next
+ // waypoint
+ wp++;
+ break;
+ case SolveStepStatus::NO_SOLUTION: // Bad solve status, considered usual
+ // so has higher threshold before
+ // going back to first waypoint
+ nBadSolves++;
+ break;
+ case SolveStepStatus::COLLISION_BEFORE: // Collision. If that happens too
+ // much, go back to first
+ // waypoint
+ case SolveStepStatus::COLLISION_AFTER:
+ nFails++;
+ break;
+ default:
+ throw(std::logic_error("Unintended exit code for solveStep"));
+ }
+ }
+
+ displayConfigsSolved();
+ // displayRhsMatrix ();
+
+ return true;
+}
+
+// Get list of configurations from solution of optimization problem
+core::Configurations_t StatesPathFinder::getConfigList() const {
+ OptimizationData& d = *optData_;
+ core::Configurations_t pv;
+ ConfigurationPtr_t q1(new Configuration_t(d.q1));
+ pv.push_back(q1);
+ for (std::size_t i = 0; i < d.N; ++i) {
+ ConfigurationPtr_t q(new Configuration_t(d.waypoint.col(i)));
+ pv.push_back(q);
+ }
+ if (!goalDefinedByConstraints_) {
+ ConfigurationPtr_t q2(new Configuration_t(d.q2));
+ pv.push_back(q2);
+ }
+ return pv;
+}
+
+// Loop over all the possible paths in the constraint graph between
+// the state of the initial configuration
+// and either [the state of the final configurations if given] OR
+// [one of potential goal states if goal defined as set of constraints]
+// and compute waypoint configurations in each state.
+core::Configurations_t StatesPathFinder::computeConfigList(
+ ConfigurationIn_t q1, ConfigurationPtr_t q2) {
+ const graph::GraphPtr_t& graph(problem_->constraintGraph());
+ GraphSearchData& d = *graphData_;
+
+ size_t& idxSol = d.idxSol;
+
+ bool maxDepthReached;
+ while (!(maxDepthReached = findTransitions(d))) { // mut
+ // if there is a working sequence, try it first before getting another
+ // transition list
+ Edges_t transitions = (nTryConfigList_ > 0)
+ ? lastBuiltTransitions_
+ : getTransitionList(d, idxSol); // const, const
+ while (!transitions.empty()) {
#ifdef HPP_DEBUG
- std::ostringstream ss;
- ss << " Trying solution " << idxSol << ": \n\t";
- for (std::size_t j = 0; j < transitions.size(); ++j)
- ss << transitions[j]->name() << ", \n\t";
- hppDout (info, ss.str());
-#endif // HPP_DEBUG
- if (optData_) {
- delete optData_;
- optData_ = nullptr;
- }
- optData_ = new OptimizationData (problem(), q1, q2, transitions,
- goalDefinedByConstraints_);
-
- if (buildOptimizationProblem (transitions)) {
- lastBuiltTransitions_ = transitions;
- if (nTryConfigList_ > 0 || analyseOptimizationProblem2 (transitions, problem())) {
- if (solveOptimizationProblem ()) {
- core::Configurations_t path = getConfigList ();
- hppDout (warning, " Solution " << idxSol << ": solved configurations list");
- return path;
- } else {
- hppDout (info, " Failed solution " << idxSol << " at step 5 (solve opt pb)");
- }
- } else {
- hppDout (info, " Failed solution " << idxSol << " at step 4 (analyse opt pb)");
- }
- } else {
- hppDout (info, " Failed solution " << idxSol << " at step 3 (build opt pb)");
- }
- transitions = getTransitionList(d, ++idxSol);
- // reset of the number of tries for a sequence
- nTryConfigList_ = 0;
- }
- }
- core::Configurations_t empty_path;
- ConfigurationPtr_t q (new Configuration_t (q1));
- empty_path.push_back(q);
- return empty_path;
+ std::ostringstream ss;
+ ss << " Trying solution " << idxSol << ": \n\t";
+ for (std::size_t j = 0; j < transitions.size(); ++j)
+ ss << transitions[j]->name() << ", \n\t";
+ hppDout(info, ss.str());
+#endif // HPP_DEBUG
+ if (optData_) {
+ delete optData_;
+ optData_ = nullptr;
}
-
- void StatesPathFinder::reset() {
- // when debugging, if we want to start from a certain transition list,
- // we can set it here
- graphData_->idxSol = 0;
- if (optData_) {
- delete optData_;
- optData_ = nullptr;
- }
- lastBuiltTransitions_.clear();
- idxConfigList_ = 0;
- nTryConfigList_ = 0;
- }
-
- void StatesPathFinder::startSolve ()
- {
- PathPlanner::startSolve();
- assert(problem_);
- q1_ = problem_->initConfig();
- assert(q1_);
-
- // core::PathProjectorPtr_t pathProjector
- // (core::pathProjector::Progressive::create(inStateProblem_, 0.01));
- // inStateProblem_->pathProjector(pathProjector);
- inStateProblem_->pathProjector(problem_->pathProjector());
- const graph::GraphPtr_t& graph(problem_->constraintGraph ());
- graphData_.reset(new GraphSearchData());
- GraphSearchData& d = *graphData_;
- d.s1 = graph->getState (*q1_);
- d.maxDepth = problem_->getParameter
- ("StatesPathFinder/maxDepth").intValue();
-
- d.queue1.push_back (d.addInitState());
- d.queueIt = d.queue1.size();
-
-#ifdef HPP_DEBUG
- // Print out the names of all the states in graph in debug mode
- States_t allStates = graph->stateSelector()->getStates();
- hppDout(info, "Constraint graph has " << allStates.size() << " nodes");
- for (auto state: allStates) {
- hppDout(info, "State: id = " << state->id()
- << " name = \"" << state->name() << "\"");
- }
- hppDout(info, "Constraint graph has " << graph->nbComponents()
- << " components");
-#endif
- // Detect whether the goal is defined by a configuration or by a
- // set of constraints
- ProblemTargetPtr_t target(problem()->target());
- GoalConfigurationsPtr_t goalConfigs
- (HPP_DYNAMIC_PTR_CAST(GoalConfigurations, target));
- if (goalConfigs){
- goalDefinedByConstraints_ = false;
- core::Configurations_t q2s = goalConfigs->configurations();
- if (q2s.size() != 1) {
- std::ostringstream os;
- os << "StatesPathFinder accept one and only one goal "
- "configuration, ";
- os << q2s.size() << " provided.";
- throw std::logic_error(os.str().c_str());
+ optData_ = new OptimizationData(problem(), q1, q2, transitions,
+ goalDefinedByConstraints_);
+
+ if (buildOptimizationProblem(transitions)) {
+ lastBuiltTransitions_ = transitions;
+ if (nTryConfigList_ > 0 ||
+ analyseOptimizationProblem2(transitions, problem())) {
+ if (solveOptimizationProblem()) {
+ core::Configurations_t path = getConfigList();
+ hppDout(warning,
+ " Solution " << idxSol << ": solved configurations list");
+ return path;
+ } else {
+ hppDout(info, " Failed solution " << idxSol
+ << " at step 5 (solve opt pb)");
}
- q2_ = q2s[0];
- d.s2.push_back(graph->getState(*q2_));
} else {
- TaskTargetPtr_t taskTarget(HPP_DYNAMIC_PTR_CAST(TaskTarget,target));
- if(!taskTarget){
- std::ostringstream os;
- os << "StatesPathFinder only accept goal defined as "
- "either a configuration or a set of constraints.";
- throw std::logic_error(os.str().c_str());
- }
- assert (!q2_);
- goalDefinedByConstraints_ = true;
- goalConstraints_ = taskTarget->constraints();
- hppDout(info, "goal defined as a set of constraints");
-
- int maxNumConstr = -1;
- for (StatePtr_t state: graph->stateSelector()->getStates()) {
- NumericalConstraints_t stateConstr = state->numericalConstraints();
- int numConstr = 0;
- for (auto goalConstraint: goalConstraints_) {
- if (std::find(stateConstr.begin(), stateConstr.end(),
- goalConstraint) != stateConstr.end()) {
- ++numConstr;
- hppDout(info, "State \"" << state->name() << "\" "
- << "has goal constraint: \""
- << goalConstraint->function().name() << "\"");
- }
- }
- if (numConstr > maxNumConstr) {
- d.s2.clear();
- d.s2.push_back(state);
- maxNumConstr = numConstr;
- } else if (numConstr == maxNumConstr) {
- d.s2.push_back(state);
- }
- }
- d.idxSol = 0;
+ hppDout(info, " Failed solution " << idxSol
+ << " at step 4 (analyse opt pb)");
}
- reset();
+ } else {
+ hppDout(info,
+ " Failed solution " << idxSol << " at step 3 (build opt pb)");
}
+ transitions = getTransitionList(d, ++idxSol);
+ // reset of the number of tries for a sequence
+ nTryConfigList_ = 0;
+ }
+ }
+ core::Configurations_t empty_path;
+ ConfigurationPtr_t q(new Configuration_t(q1));
+ empty_path.push_back(q);
+ return empty_path;
+}
+
+void StatesPathFinder::reset() {
+ // when debugging, if we want to start from a certain transition list,
+ // we can set it here
+ graphData_->idxSol = 0;
+ if (optData_) {
+ delete optData_;
+ optData_ = nullptr;
+ }
+ lastBuiltTransitions_.clear();
+ idxConfigList_ = 0;
+ nTryConfigList_ = 0;
+}
+
+void StatesPathFinder::startSolve() {
+ PathPlanner::startSolve();
+ assert(problem_);
+ q1_ = problem_->initConfig();
+ assert(q1_);
+
+ // core::PathProjectorPtr_t pathProjector
+ // (core::pathProjector::Progressive::create(inStateProblem_, 0.01));
+ // inStateProblem_->pathProjector(pathProjector);
+ inStateProblem_->pathProjector(problem_->pathProjector());
+ const graph::GraphPtr_t& graph(problem_->constraintGraph());
+ graphData_.reset(new GraphSearchData());
+ GraphSearchData& d = *graphData_;
+ d.s1 = graph->getState(*q1_);
+ d.maxDepth = problem_->getParameter("StatesPathFinder/maxDepth").intValue();
+
+ d.queue1.push_back(d.addInitState());
+ d.queueIt = d.queue1.size();
- void StatesPathFinder::oneStep ()
- {
- if (idxConfigList_ == 0) {
- // TODO: accommodate when goal is a set of constraints
- assert(q1_);
- configList_ = computeConfigList(*q1_, q2_);
- if (configList_.size() <= 1) { // max depth reached
- reset();
- throw core::path_planning_failed("Maximal depth reached.");
- }
- }
- size_t & idxSol = graphData_->idxSol;
- ConfigurationPtr_t q1, q2;
- if (idxConfigList_ >= configList_.size() - 1) {
- reset();
- throw core::path_planning_failed(
- "Final config reached but goal is not reached.");
- }
- q1 = configList_[idxConfigList_];
- q2 = configList_[idxConfigList_+1];
- const graph::EdgePtr_t& edge(lastBuiltTransitions_[idxConfigList_]);
- // Copy edge constraints
- core::ConstraintSetPtr_t constraints(HPP_DYNAMIC_PTR_CAST(
- core::ConstraintSet, edge->pathConstraint()->copy()));
- // Initialize right hand side
- constraints->configProjector()->rightHandSideFromConfig(*q1);
- assert(constraints->isSatisfied(*q2));
- inStateProblem_->constraints(constraints);
- inStateProblem_->pathValidation(edge->pathValidation());
- inStateProblem_->steeringMethod(edge->steeringMethod());
- inStateProblem_->initConfig(q1);
- inStateProblem_->resetGoalConfigs();
- inStateProblem_->addGoalConfig(q2);
-
- /// use inner state planner to plan path between two configurations.
- /// these configs lie on same leaf (same RHS wrt edge constraints)
- /// eg consecutive configs in the solved config list
- core::PathPlannerPtr_t inStatePlanner
- (core::DiffusingPlanner::create(inStateProblem_));
- inStatePlanner->maxIterations(problem_->getParameter
- ("StatesPathFinder/innerPlannerMaxIterations").intValue());
- value_type innerPlannerTimeout = problem_->getParameter
- ("StatesPathFinder/innerPlannerTimeOut").floatValue();
- // only set timeout if it is more than 0. default is infinity
- if (innerPlannerTimeout > 0.) {
- inStatePlanner->timeOut(innerPlannerTimeout);
- }
- hppDout(info, "calling InStatePlanner_.solve for transition "
- << idxConfigList_);
- core::PathVectorPtr_t path;
- try {
- path = inStatePlanner->solve();
- for (std::size_t r = 0; r < path->numberPaths()-1; r++)
- assert(path->pathAtRank(r)->end() ==
- path->pathAtRank(r+1)->initial());
- idxConfigList_++;
- if (idxConfigList_ == configList_.size()-1) {
- hppDout(warning, "Solution " << idxSol << ": Success"
- << "\n-----------------------------------------------");
- }
- } catch(const core::path_planning_failed& error) {
- std::ostringstream oss;
- oss << "Error " << error.what() << "\n";
- oss << "Solution " << idxSol << ": Failed to build path at edge " << idxConfigList_ << ": ";
- oss << lastBuiltTransitions_[idxConfigList_]->name();
- hppDout(warning, oss.str());
-
- idxConfigList_ = 0;
- // Retry nTryMax times to build another solution for the same states list
- size_type nTryMax = problem_->getParameter
- ("StatesPathFinder/maxTriesBuildPath").intValue();
- if (++nTryConfigList_ >= nTryMax) {
- nTryConfigList_ = 0;
- idxSol++;
- }
- }
- roadmap()->merge(inStatePlanner->roadmap());
- // if (path) {
- // core::PathOptimizerPtr_t inStateOptimizer
- // (core::pathOptimization::RandomShortcut::create(inStateProblem_));
- // core::PathVectorPtr_t opt = inStateOptimizer->optimize(path);
- // roadmap()->insertPathVector(opt, true);
- // }
- }
-
- void StatesPathFinder::tryConnectInitAndGoals()
- {
- GraphSearchData& d = *graphData_;
- // if start state is not one of the potential goal states, return
- if (std::find(d.s2.begin(), d.s2.end(), d.s1) == d.s2.end()) {
- return;
+#ifdef HPP_DEBUG
+ // Print out the names of all the states in graph in debug mode
+ States_t allStates = graph->stateSelector()->getStates();
+ hppDout(info, "Constraint graph has " << allStates.size() << " nodes");
+ for (auto state : allStates) {
+ hppDout(info, "State: id = " << state->id() << " name = \"" << state->name()
+ << "\"");
+ }
+ hppDout(info,
+ "Constraint graph has " << graph->nbComponents() << " components");
+#endif
+ // Detect whether the goal is defined by a configuration or by a
+ // set of constraints
+ ProblemTargetPtr_t target(problem()->target());
+ GoalConfigurationsPtr_t goalConfigs(
+ HPP_DYNAMIC_PTR_CAST(GoalConfigurations, target));
+ if (goalConfigs) {
+ goalDefinedByConstraints_ = false;
+ core::Configurations_t q2s = goalConfigs->configurations();
+ if (q2s.size() != 1) {
+ std::ostringstream os;
+ os << "StatesPathFinder accept one and only one goal "
+ "configuration, ";
+ os << q2s.size() << " provided.";
+ throw std::logic_error(os.str().c_str());
+ }
+ q2_ = q2s[0];
+ d.s2.push_back(graph->getState(*q2_));
+ } else {
+ TaskTargetPtr_t taskTarget(HPP_DYNAMIC_PTR_CAST(TaskTarget, target));
+ if (!taskTarget) {
+ std::ostringstream os;
+ os << "StatesPathFinder only accept goal defined as "
+ "either a configuration or a set of constraints.";
+ throw std::logic_error(os.str().c_str());
+ }
+ assert(!q2_);
+ goalDefinedByConstraints_ = true;
+ goalConstraints_ = taskTarget->constraints();
+ hppDout(info, "goal defined as a set of constraints");
+
+ int maxNumConstr = -1;
+ for (StatePtr_t state : graph->stateSelector()->getStates()) {
+ NumericalConstraints_t stateConstr = state->numericalConstraints();
+ int numConstr = 0;
+ for (auto goalConstraint : goalConstraints_) {
+ if (std::find(stateConstr.begin(), stateConstr.end(), goalConstraint) !=
+ stateConstr.end()) {
+ ++numConstr;
+ hppDout(info, "State \"" << state->name() << "\" "
+ << "has goal constraint: \""
+ << goalConstraint->function().name()
+ << "\"");
}
-
- // get the loop edge connecting the initial state to itself
- const graph::Edges_t& loopEdges(problem_->constraintGraph()
- ->getEdges(d.s1, d.s1));
- // check that there is 1 loop edge
- assert (loopEdges.size() == 1);
- // add the loop transition as transition list
- GraphSearchData::state_with_depth_ptr_t _state = d.queue1.front();
- GraphSearchData::state_with_depth_ptr_t _endState = d.addParent (_state, loopEdges[0]);
- d.solutions.push_back(_endState);
-
- // try connecting initial and final configurations directly
- if (!goalDefinedByConstraints_) PathPlanner::tryConnectInitAndGoals();
- }
-
- std::vector<std::string> StatesPathFinder::constraintNamesFromSolverAtWaypoint
- (std::size_t wp)
- {
- assert (wp > 0 && wp <= optData_->solvers.size());
- constraints::solver::BySubstitution& solver (optData_->solvers [wp-1]);
- std::vector<std::string> ans;
- for (std::size_t i = 0; i < solver.constraints().size(); i++)
- ans.push_back(solver.constraints()[i]->function().name());
- return ans;
}
-
- std::vector<std::string> StatesPathFinder::lastBuiltTransitions() const
- {
- std::vector<std::string> ans;
- for (const EdgePtr_t& edge: lastBuiltTransitions_)
- ans.push_back(edge->name());
- return ans;
+ if (numConstr > maxNumConstr) {
+ d.s2.clear();
+ d.s2.push_back(state);
+ maxNumConstr = numConstr;
+ } else if (numConstr == maxNumConstr) {
+ d.s2.push_back(state);
}
-
- using core::Parameter;
- using core::ParameterDescription;
-
- HPP_START_PARAMETER_DECLARATION(StatesPathFinder)
- core::Problem::declareParameter(ParameterDescription(Parameter::INT,
- "StatesPathFinder/maxDepth",
- "Maximum number of transitions to look for.",
- Parameter((size_type)std::numeric_limits<int>::max())));
- core::Problem::declareParameter(ParameterDescription(Parameter::INT,
- "StatesPathFinder/maxIteration",
- "Maximum number of iterations of the Newton Raphson algorithm.",
- Parameter((size_type)60)));
- core::Problem::declareParameter(ParameterDescription(Parameter::FLOAT,
- "StatesPathFinder/errorThreshold",
- "Error threshold of the Newton Raphson algorithm."
- "Should be at most the same as error threshold in constraint graph",
- Parameter(1e-4)));
- core::Problem::declareParameter(ParameterDescription(Parameter::INT,
- "StatesPathFinder/nTriesUntilBacktrack",
- "Number of tries when sampling configurations before backtracking"
- "in function solveOptimizationProblem.",
- Parameter((size_type)3)));
- core::Problem::declareParameter(ParameterDescription(Parameter::INT,
- "StatesPathFinder/maxTriesCollisionAnalysis",
- "Number of solve tries before stopping the collision analysis,"
- "before the actual solving part."
- "Set to 0 to skip this part of the algorithm.",
- Parameter((size_type)100)));
- core::Problem::declareParameter(ParameterDescription(Parameter::INT,
- "StatesPathFinder/maxTriesBuildPath",
- "Number of solutions with a given states list to try to build a"
- "continuous path from, before skipping to the next states list",
- Parameter((size_type)5)));
- core::Problem::declareParameter(ParameterDescription(Parameter::FLOAT,
- "StatesPathFinder/innerPlannerTimeOut",
- "This will set ::timeOut accordingly in the inner"
- "planner used for building a path after intermediate"
- "configurations have been found."
- "If set to 0, no timeout: only maxIterations will be used to stop"
- "the innerPlanner if it does not find a path.",
- Parameter(2.0)));
- core::Problem::declareParameter(ParameterDescription(Parameter::INT,
- "StatesPathFinder/innerPlannerMaxIterations",
- "This will set ::maxIterations accordingly in the inner"
- "planner used for building a path after intermediate"
- "configurations have been found",
- Parameter((size_type)1000)));
- HPP_END_PARAMETER_DECLARATION(StatesPathFinder)
- } // namespace pathPlanner
- } // namespace manipulation
-} // namespace hpp
+ }
+ d.idxSol = 0;
+ }
+ reset();
+}
+
+void StatesPathFinder::oneStep() {
+ if (idxConfigList_ == 0) {
+ // TODO: accommodate when goal is a set of constraints
+ assert(q1_);
+ configList_ = computeConfigList(*q1_, q2_);
+ if (configList_.size() <= 1) { // max depth reached
+ reset();
+ throw core::path_planning_failed("Maximal depth reached.");
+ }
+ }
+ size_t& idxSol = graphData_->idxSol;
+ ConfigurationPtr_t q1, q2;
+ if (idxConfigList_ >= configList_.size() - 1) {
+ reset();
+ throw core::path_planning_failed(
+ "Final config reached but goal is not reached.");
+ }
+ q1 = configList_[idxConfigList_];
+ q2 = configList_[idxConfigList_ + 1];
+ const graph::EdgePtr_t& edge(lastBuiltTransitions_[idxConfigList_]);
+ // Copy edge constraints
+ core::ConstraintSetPtr_t constraints(HPP_DYNAMIC_PTR_CAST(
+ core::ConstraintSet, edge->pathConstraint()->copy()));
+ // Initialize right hand side
+ constraints->configProjector()->rightHandSideFromConfig(*q1);
+ assert(constraints->isSatisfied(*q2));
+ inStateProblem_->constraints(constraints);
+ inStateProblem_->pathValidation(edge->pathValidation());
+ inStateProblem_->steeringMethod(edge->steeringMethod());
+ inStateProblem_->initConfig(q1);
+ inStateProblem_->resetGoalConfigs();
+ inStateProblem_->addGoalConfig(q2);
+
+ /// use inner state planner to plan path between two configurations.
+ /// these configs lie on same leaf (same RHS wrt edge constraints)
+ /// eg consecutive configs in the solved config list
+ core::PathPlannerPtr_t inStatePlanner(
+ core::DiffusingPlanner::create(inStateProblem_));
+ inStatePlanner->maxIterations(
+ problem_->getParameter("StatesPathFinder/innerPlannerMaxIterations")
+ .intValue());
+ value_type innerPlannerTimeout =
+ problem_->getParameter("StatesPathFinder/innerPlannerTimeOut")
+ .floatValue();
+ // only set timeout if it is more than 0. default is infinity
+ if (innerPlannerTimeout > 0.) {
+ inStatePlanner->timeOut(innerPlannerTimeout);
+ }
+ hppDout(info,
+ "calling InStatePlanner_.solve for transition " << idxConfigList_);
+ core::PathVectorPtr_t path;
+ try {
+ path = inStatePlanner->solve();
+ for (std::size_t r = 0; r < path->numberPaths() - 1; r++)
+ assert(path->pathAtRank(r)->end() == path->pathAtRank(r + 1)->initial());
+ idxConfigList_++;
+ if (idxConfigList_ == configList_.size() - 1) {
+ hppDout(
+ warning, "Solution "
+ << idxSol << ": Success"
+ << "\n-----------------------------------------------");
+ }
+ } catch (const core::path_planning_failed& error) {
+ std::ostringstream oss;
+ oss << "Error " << error.what() << "\n";
+ oss << "Solution " << idxSol << ": Failed to build path at edge "
+ << idxConfigList_ << ": ";
+ oss << lastBuiltTransitions_[idxConfigList_]->name();
+ hppDout(warning, oss.str());
+
+ idxConfigList_ = 0;
+ // Retry nTryMax times to build another solution for the same states list
+ size_type nTryMax =
+ problem_->getParameter("StatesPathFinder/maxTriesBuildPath").intValue();
+ if (++nTryConfigList_ >= nTryMax) {
+ nTryConfigList_ = 0;
+ idxSol++;
+ }
+ }
+ roadmap()->merge(inStatePlanner->roadmap());
+ // if (path) {
+ // core::PathOptimizerPtr_t inStateOptimizer
+ // (core::pathOptimization::RandomShortcut::create(inStateProblem_));
+ // core::PathVectorPtr_t opt = inStateOptimizer->optimize(path);
+ // roadmap()->insertPathVector(opt, true);
+ // }
+}
+
+void StatesPathFinder::tryConnectInitAndGoals() {
+ GraphSearchData& d = *graphData_;
+ // if start state is not one of the potential goal states, return
+ if (std::find(d.s2.begin(), d.s2.end(), d.s1) == d.s2.end()) {
+ return;
+ }
+
+ // get the loop edge connecting the initial state to itself
+ const graph::Edges_t& loopEdges(
+ problem_->constraintGraph()->getEdges(d.s1, d.s1));
+ // check that there is 1 loop edge
+ assert(loopEdges.size() == 1);
+ // add the loop transition as transition list
+ GraphSearchData::state_with_depth_ptr_t _state = d.queue1.front();
+ GraphSearchData::state_with_depth_ptr_t _endState =
+ d.addParent(_state, loopEdges[0]);
+ d.solutions.push_back(_endState);
+
+ // try connecting initial and final configurations directly
+ if (!goalDefinedByConstraints_) PathPlanner::tryConnectInitAndGoals();
+}
+
+std::vector<std::string> StatesPathFinder::constraintNamesFromSolverAtWaypoint(
+ std::size_t wp) {
+ assert(wp > 0 && wp <= optData_->solvers.size());
+ constraints::solver::BySubstitution& solver(optData_->solvers[wp - 1]);
+ std::vector<std::string> ans;
+ for (std::size_t i = 0; i < solver.constraints().size(); i++)
+ ans.push_back(solver.constraints()[i]->function().name());
+ return ans;
+}
+
+std::vector<std::string> StatesPathFinder::lastBuiltTransitions() const {
+ std::vector<std::string> ans;
+ for (const EdgePtr_t& edge : lastBuiltTransitions_)
+ ans.push_back(edge->name());
+ return ans;
+}
+
+using core::Parameter;
+using core::ParameterDescription;
+
+HPP_START_PARAMETER_DECLARATION(StatesPathFinder)
+core::Problem::declareParameter(ParameterDescription(
+ Parameter::INT, "StatesPathFinder/maxDepth",
+ "Maximum number of transitions to look for.",
+ Parameter((size_type)std::numeric_limits<int>::max())));
+core::Problem::declareParameter(ParameterDescription(
+ Parameter::INT, "StatesPathFinder/maxIteration",
+ "Maximum number of iterations of the Newton Raphson algorithm.",
+ Parameter((size_type)60)));
+core::Problem::declareParameter(ParameterDescription(
+ Parameter::FLOAT, "StatesPathFinder/errorThreshold",
+ "Error threshold of the Newton Raphson algorithm."
+ "Should be at most the same as error threshold in constraint graph",
+ Parameter(1e-4)));
+core::Problem::declareParameter(ParameterDescription(
+ Parameter::INT, "StatesPathFinder/nTriesUntilBacktrack",
+ "Number of tries when sampling configurations before backtracking"
+ "in function solveOptimizationProblem.",
+ Parameter((size_type)3)));
+core::Problem::declareParameter(ParameterDescription(
+ Parameter::INT, "StatesPathFinder/maxTriesCollisionAnalysis",
+ "Number of solve tries before stopping the collision analysis,"
+ "before the actual solving part."
+ "Set to 0 to skip this part of the algorithm.",
+ Parameter((size_type)100)));
+core::Problem::declareParameter(ParameterDescription(
+ Parameter::INT, "StatesPathFinder/maxTriesBuildPath",
+ "Number of solutions with a given states list to try to build a"
+ "continuous path from, before skipping to the next states list",
+ Parameter((size_type)5)));
+core::Problem::declareParameter(ParameterDescription(
+ Parameter::FLOAT, "StatesPathFinder/innerPlannerTimeOut",
+ "This will set ::timeOut accordingly in the inner"
+ "planner used for building a path after intermediate"
+ "configurations have been found."
+ "If set to 0, no timeout: only maxIterations will be used to stop"
+ "the innerPlanner if it does not find a path.",
+ Parameter(2.0)));
+core::Problem::declareParameter(ParameterDescription(
+ Parameter::INT, "StatesPathFinder/innerPlannerMaxIterations",
+ "This will set ::maxIterations accordingly in the inner"
+ "planner used for building a path after intermediate"
+ "configurations have been found",
+ Parameter((size_type)1000)));
+HPP_END_PARAMETER_DECLARATION(StatesPathFinder)
+} // namespace pathPlanner
+} // namespace manipulation
+} // namespace hpp
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 2249e414cd72941e72993e6764fa994415003c06..ac5d640db353717333004905f07048c88e75c59d 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -62,8 +62,8 @@
#include "hpp/manipulation/path-optimization/enforce-transition-semantic.hh"
#include "hpp/manipulation/path-optimization/random-shortcut.hh"
#include "hpp/manipulation/path-planner/end-effector-trajectory.hh"
-#include "hpp/manipulation/path-planner/states-path-finder.hh"
#include "hpp/manipulation/path-planner/in-state-path.hh"
+#include "hpp/manipulation/path-planner/states-path-finder.hh"
#include "hpp/manipulation/problem-target/state.hh"
#include "hpp/manipulation/problem.hh"
#include "hpp/manipulation/roadmap.hh"
@@ -125,8 +125,8 @@ ProblemSolver::ProblemSolver() : core::ProblemSolver(), robot_(), problem_() {
pathPlanners.add("M-RRT", ManipulationPlanner::create);
pathPlanners.add("EndEffectorTrajectory",
pathPlanner::EndEffectorTrajectory::createWithRoadmap);
- pathPlanners.add ("StatesPathFinder",
- pathPlanner::StatesPathFinder::createWithRoadmap);
+ pathPlanners.add("StatesPathFinder",
+ pathPlanner::StatesPathFinder::createWithRoadmap);
pathValidations.add("Graph-Discretized",
createDiscretizedCollisionGraphPathValidation);
pathValidations.add("Graph-DiscretizedCollision",