include/hpp/manipulation/path-planner/states-path-finder.hh

+// Copyright (c) 2017, Joseph Mirabel
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr),
+//          Florent Lamiraux (florent.lamiraux@laas.fr)
+//          Alexandre Thiault (athiault@laas.fr)
+//          Le Quang Anh (quang-anh.le@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// 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/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>
+#include <unordered_map>
+
+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,
+                                           ConfigurationIn_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,
+                                  core::ProblemConstPtr_t _problem);
+
+  /// Step 5 of the algorithm
+  void initializeRHS(std::size_t j);
+  bool solveOptimizationProblem();
+
+  // Data structure used to store a constraint right hand side as value and its
+  // name as key, both in form of hash numbers (so that names and rhs of two
+  // constraints can be easily merge). Exemple : ConstraintMap_t map =
+  // {{nameStringHash, rhsVectorHash}}; With rhsVectorHash the hash of a
+  // vector_t of rights hand side constraints with hashRHS, and nameStringHash
+  // the hash of a std::string - obtained for instance with std::hash.
+  typedef std::unordered_map<size_t, size_t> ConstraintMap_t;  // map (name,
+                                                               // rhs)
+
+  /// @brief Get a configuration in accordance with the statuts matrix at a step
+  /// j for the constraint i
+  /// @param i number of the constraint in the status matrix
+  /// @param j step of the potential solution (index of a waypoint)
+  /// @return a configuration Configuration_t which follows the status matrix
+  /// indication at the given indices
+  Configuration_t getConfigStatus(size_type i, size_type j) const;
+
+  /// @brief Get the right hand side of a constraint w.r.t a set configuration
+  /// for this constraint
+  /// @param constraint the constraint to compute the right hand side of
+  /// @param q the configuration in which the constraint is set
+  /// @return a right hand side vector_t
+  vector_t getConstraintRHS(ImplicitPtr_t constraint, Configuration_t q) const;
+
+  /// @brief Hash a vector of right hand side into a long unsigned integer
+  /// @param rhs the right hand side vector vector_t
+  /// @return a size_t integer hash
+  size_t hashRHS(vector_t rhs) const;
+
+  /// @brief Check if a solution (a list of transition) contains impossible to
+  /// solve steps due to inevitable collisions
+  /// @param pairMap The ConstraintMap_tf table of pairs of incompatibles
+  /// constraints
+  /// @param constraintMap The hasmap table of constraints which are in pairMap
+  /// @return a bool which is true is there is no impossible to solve steps,
+  /// true otherwise
+  bool checkSolvers(ConstraintMap_t const& pairMap,
+                    ConstraintMap_t const& constraintMap) const;
+
+  /// @brief For a certain step wp during solving check for collision impossible
+  /// to solve.
+  /// @param pairMap The ConstraintMap_t table of pairs of incompatibles
+  /// constraints
+  /// @param constraintMap The hasmap table of constraints which are in pairMap
+  /// @param wp The index of the current step
+  /// @return a bool which is true if there is no collision or impossible to
+  /// solve ones, false otherwise.
+  bool saveIncompatibleRHS(ConstraintMap_t& pairMap,
+                           ConstraintMap_t& constraintMap, size_type const wp);
+
+  // For a joint get his most, constrained with it, far parent
+  core::JointConstPtr_t maximalJoint(size_t const wp, core::JointConstPtr_t a);
+
+  /// 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
+  Configuration_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

include/hpp/manipulation/path-planner/transition-planner.hh

+// Copyright (c) 2023 CNRS
+// Authors: Florent Lamiraux
+//
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// 1. Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//
+// 2. Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
+// DAMAGE.
+
+#ifndef HPP_MANIPULATION_PATH_PLANNER_TRANSITION_PLANNER_HH
+#define HPP_MANIPULATION_PATH_PLANNER_TRANSITION_PLANNER_HH
+
+#include <hpp/core/path-planner.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/fwd.hh>
+
+namespace hpp {
+namespace manipulation {
+namespace pathPlanner {
+
+/// \addtogroup path_planner
+/// \{
+
+/// Plan paths in a leaf of a transition
+///
+/// In many manipulation applications, the sequence of actions is knwown in
+/// advance or computed by a task planner. There is a need then to connect
+/// configurations that lie on the same leaf of a transition. This class
+/// performs this computation.
+///
+/// The constraint graph is stored in the Problem instance of the planner.
+/// To select the transition, call method \link TransitionPlanner::setEdge
+/// setEdge \endlink with the index of the transition.
+///
+/// At construction, a core::Problem instance is created, as well as a
+/// core::PathPlanner instance. They are respectively called the inner problem
+/// and the inner planner.
+///
+/// The leaf of the transition is defined by the initial configuration passed
+/// to method \link TransitionPlanner::planPath planPath \endlink.
+/// The right hand side of the inner problem constraints is initialized with
+/// this configuration.
+///
+/// The class stores path optimizers that are called when invoking method
+/// \link TransitionPlanner::optimizePath optimizePath \endlink.
+///
+/// Method \link TransitionPlanner::timeParameterization timeParameterization
+/// \endlink computes a time parameterization of a given path.
+class HPP_MANIPULATION_DLLAPI TransitionPlanner : public core::PathPlanner {
+ public:
+  typedef core::PathPlannerPtr_t PathPlannerPtr_t;
+  typedef core::PathProjectorPtr_t PathProjectorPtr_t;
+  typedef core::PathPtr_t PathPtr_t;
+  typedef core::PathOptimizerPtr_t PathOptimizerPtr_t;
+  typedef core::PathVector PathVector;
+  typedef core::PathVectorPtr_t PathVectorPtr_t;
+  typedef core::Parameter Parameter;
+
+  /// Create instance and return share pointer
+  static TransitionPlannerPtr_t createWithRoadmap(
+      const core::ProblemConstPtr_t& problem,
+      const core::RoadmapPtr_t& roadmap);
+  /// Get the inner planner
+  PathPlannerPtr_t innerPlanner() const { return innerPlanner_; }
+  /// Set the inner planner
+  void innerPlanner(const PathPlannerPtr_t& planner) {
+    innerPlanner_ = planner;
+  }
+  /// Get the inner problem
+  core::ProblemPtr_t innerProblem() const { return innerProblem_; }
+  /// Initialize path planning
+  /// Set right hand side of problem constraints with initial configuration
+  virtual void startSolve();
+
+  /// One step of the planner
+  /// Calls the same method of the internally stored planner
+  virtual void oneStep();
+
+  /// Solve the problem defined by input configurations
+  /// \param qInit initial configuration,
+  /// \param qGoals, goal configurations,
+  /// \param resetRoadmap whether to reset the roadmap
+  PathVectorPtr_t planPath(const Configuration_t qInit, matrixIn_t qGoals,
+                           bool resetRoadmap);
+  /// Call the steering method between two configurations
+  /// \param q1, q2 the start and end configurations,
+  /// \param validate whether resulting path should be tested for collision
+  /// \retval success True if path has been computed and validated
+  ///         successfully
+  /// \retval status a message in case of failure.
+  /// If a path projector has been selected, the path is tested for
+  /// continuity.
+  PathPtr_t directPath(ConfigurationIn_t q1, ConfigurationIn_t q2,
+                       bool validate, bool& success, std::string& status);
+  /// Validate a configuration with the path validation of an edge.
+  /// \param q configuration to validate,
+  /// \param id index of the edge in the constraint graph.
+  bool validateConfiguration(ConfigurationIn_t q, std::size_t id,
+                             core::ValidationReportPtr_t& report) const;
+  /// Optimize path using the selected path optimizers
+  /// \param path input path
+  /// \return optimized path
+  PathVectorPtr_t optimizePath(const PathPtr_t& path);
+
+  /// Compute time parameterization of path
+  /// \param path input path
+  /// \return time parameterized trajectory
+  /// Uses core::pathOptimization::SimpleTimeParameterization.
+  PathVectorPtr_t timeParameterization(const PathVectorPtr_t& path);
+
+  /// Set transition along which we wish to plan a path
+  /// \param id index of the edge in the constraint graph
+  void setEdge(std::size_t id);
+
+  /// Create a Reeds and Shepp steering method and path it to the problem.
+  void setReedsAndSheppSteeringMethod(double turningRadius);
+
+  /// Set the path projector
+  void pathProjector(const PathProjectorPtr_t pathProjector);
+
+  /// Clear path optimizers
+  void clearPathOptimizers();
+
+  /// Add a path optimizer
+  void addPathOptimizer(const PathOptimizerPtr_t& pathOptimizer);
+
+  /// Set parameter to the inner problem
+  void setParameter(const std::string& key, const Parameter& value);
+
+ protected:
+  /// Constructor
+  /// Cast problem into manipulation::Problem and store shared pointer
+  ///
+  /// Create an inner problem and forward the following elements of the input
+  /// problem to the inner problem:
+  /// \li parameters,
+  /// \li collision obstacles.
+  /// Add instances of core::CollisionValidation and
+  /// core::JointBoundValidation to the inner problem.
+  TransitionPlanner(const core::ProblemConstPtr_t& problem,
+                    const core::RoadmapPtr_t& roadmap);
+  /// store weak pointer to itself
+  void init(TransitionPlannerWkPtr_t weak);
+
+ private:
+  /// Check problem and forward maxIterations and timeout to inner problem.
+  void checkProblemAndForwardParameters();
+  /// Get pointer to edge from an id
+  graph::EdgePtr_t getEdgeOrThrow(std::size_t id) const;
+  /// Pointer to the problem of the inner planner
+  core::ProblemPtr_t innerProblem_;
+  /// Pointer to the inner path planner
+  PathPlannerPtr_t innerPlanner_;
+  /// Vector of optimizers to call after solve
+  std::vector<PathOptimizerPtr_t> pathOptimizers_;
+  /// Time parameterization instance
+  core::PathOptimizerPtr_t timeParameterization_;
+  /// weak pointer to itself
+  TransitionPlannerWkPtr_t weakPtr_;
+};  // class TransitionPlanner
+}  // namespace pathPlanner
+}  // namespace manipulation
+}  // namespace hpp
+#endif  // HPP_MANIPULATION_PATH_PLANNER_TRANSITION_PLANNER_HH

include/hpp/manipulation/roadmap-node.hh

@@ -41,11 +41,10 @@ namespace hpp {
 namespace manipulation {
 class HPP_MANIPULATION_DLLAPI RoadmapNode : public core::Node {
  public:
-  RoadmapNode(const ConfigurationPtr_t& configuration)
+  RoadmapNode(ConfigurationIn_t configuration)
       : core::Node(configuration), state_() {}
 
-  RoadmapNode(const ConfigurationPtr_t& configuration,
-              ConnectedComponentPtr_t cc);
+  RoadmapNode(ConfigurationIn_t configuration, ConnectedComponentPtr_t cc);
 
   /// \name Cache
   /// \{

include/hpp/manipulation/roadmap.hh

@@ -68,7 +68,7 @@ class HPP_MANIPULATION_DLLAPI Roadmap : public core::Roadmap {
   /// Get the nearest neighbor in a given graph::Node and in a given
   /// ConnectedComponent.
   RoadmapNodePtr_t nearestNodeInState(
-      const ConfigurationPtr_t& configuration,
+      ConfigurationIn_t configuration,
       const ConnectedComponentPtr_t& connectedComponent,
       const graph::StatePtr_t& state, value_type& minDistance) const;
 
@@ -104,7 +104,7 @@ class HPP_MANIPULATION_DLLAPI Roadmap : public core::Roadmap {
   Roadmap(const core::DistancePtr_t& distance, const core::DevicePtr_t& robot);
 
   /// Node factory
-  core::NodePtr_t createNode(const ConfigurationPtr_t& config) const;
+  core::NodePtr_t createNode(ConfigurationIn_t config) const;
 
   void init(const RoadmapPtr_t& shPtr) {
     Parent::init(shPtr);

include/hpp/manipulation/steering-method/end-effector-trajectory.hh

@@ -35,15 +35,40 @@
 
 namespace hpp {
 namespace manipulation {
-/// \addtogroup steering_method
-/// \{
 namespace steeringMethod {
 HPP_PREDEF_CLASS(EndEffectorTrajectory);
 typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
 
 using core::PathPtr_t;
 
-/// Build StraightPath constrained by a varying right hand side constraint.
+/// \addtogroup steering_method
+/// \{
+
+/// Build piecewise straight paths for a robot end-effector
+///
+/// To use this steering method, the user needs to provide
+///  \li a constraint with value in \f$SE(3)\f$. An easy way to create such a
+///  constraint is to use method hpp::manipulation::Handle::createGrasp. The
+///  constraint is passed to the steering method using method \link
+/// EndEffectorTrajectory::trajectoryConstraint trajectoryConstraint \endlink.
+///  \li the time-varying right hand side of this constraint along the path
+///  the user wants to create in the form of a hpp::core::Path instance
+///  with values in \f$SE(3)\f$. For that, \link
+/// EndEffectorTrajectory::makePiecewiseLinearTrajectory
+/// makePiecewiseLinearTrajectory \endlink method may be useful.
+///
+/// \warning the constraint should also be inserted in the \link
+/// hpp::core::SteeringMethod::constraints set of constraints \endlink
+/// of the steering method.
+///
+/// Once the steering method has been initialized, it can be called between
+/// two configurations \c q1 and \c q2. The interval of definition \f$[0,T]\f$
+/// of the output path is the same as the one of the path provided as the right
+/// hand side of the constraint.
+/// Note that \c q1 and \c q2 should satisfy the constraint at times 0 and
+/// \f$T\f$ respectively. The output path is a \link hpp::core::StraightPath
+/// linear interpolation \endlink between \c q1 and \c q2 projected on the
+/// steering method constraints.
 class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory
     : public core::SteeringMethod {
  public:
@@ -56,10 +81,35 @@ class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory
     return ptr;
   }
 
-  /// Build a trajectory in SE(3).
-  /// \param points a Nx7 matrix whose rows corresponds to a pose.
-  /// \param weights a 6D vector, weights to be applied when computing
-  ///        the distance between two SE3 points.
+  /** Build a trajectory in SE(3).
+      \param points a Nx7 matrix whose rows corresponds to poses.
+      \param weights a 6D vector, weights to be applied when computing
+             the distance between two SE3 points.
+
+      The trajectory \f$T\f$ is defined as follows. Let \f$N\f$ be the number of
+      lines of matrix \c points, \f$p_i\f$ be the i-th line of \c points and
+      let \f$W\f$ be the
+      diagonal matrix with the coefficients of \c weights:
+      \f[
+      W = \left(\begin{array}{cccccc}
+      w_1 & 0 & 0 & 0 & 0 & 0\\
+      0 & w_2 & 0 & 0 & 0 & 0\\
+      0 & 0 & w_3 & 0 & 0 & 0\\
+      0 & 0 & 0 & w_4 & 0 & 0\\
+      0 & 0 & 0 & 0 & w_5 & 0\\
+      0 & 0 & 0 & 0 & 0 & w_6\\
+      \end{array}\right)
+      \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}
+
+      where \f$t_0 = 0\f$ and
+      \f{eqnarray*}{
+       t_{i+1}-t_i = \|W(\mathbf{p}_{i+1}-\mathbf{p}_i)\| && \mbox{for } i
+       \mbox{ such that } 1 \leq i \leq N-1
+       \f} */
   static PathPtr_t makePiecewiseLinearTrajectory(matrixIn_t points,
                                                  vectorIn_t weights);
 
@@ -116,8 +166,8 @@ class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory
   interval_t timeRange_;
   constraints::ImplicitPtr_t constraint_;
 };
-}  // namespace steeringMethod
 /// \}
+}  // namespace steeringMethod
 }  // namespace manipulation
 }  // namespace hpp
 

package.xml

 <?xml version='1.0'?>
  <package format='2'>
  <name>hpp-manipulation</name>
- <version>4.15.1</version>
+ <version>6.0.0</version>
  <description>Classes for manipulation planning. </description>
 
  <maintainer email='hpp@laas.fr'>Joseph Mirabel</maintainer>
@@ -26,8 +26,8 @@
  <build_depend>urdfdom</build_depend>
  <exec_depend>urdfdom</exec_depend>
  <build_export_depend>urdfdom</build_export_depend>
- <build_depend>hpp-fcl</build_depend>
- <exec_depend>hpp-fcl</exec_depend>
- <build_export_depend>hpp-fcl</build_export_depend>
+ <build_depend>coal</build_depend>
+ <exec_depend>coal</exec_depend>
+ <build_export_depend>coal</build_export_depend>
  <test_depend>romeo_description</test_depend>
  </package>

pyproject.toml

+[build-system]
+build-backend = "cmeel.build"
+requires = [
+  "cmeel-boost ~= 1.83.0",
+  "cmeel[build]",
+  "hpp-core[build]"
+]
+
+[project]
+dependencies = [
+  "cmeel-boost ~= 1.83.0",
+  "hpp-core"
+]
+description = "Classes for manipulation planning."
+license = "BSD-2-Clause"
+name = "hpp-manipulation"
+version = "6.0.0"
+
+[tool.ruff]
+extend-exclude = ["cmake"]
+
+[tool.ruff.lint]
+extend-select = ["I", "NPY", "RUF", "UP", "W"]
+
+[tool.tomlsort]
+all = true

src/astar.hh

@@ -137,8 +137,8 @@ class Astar {
 
   inline value_type heuristic(RoadmapNodePtr_t node,
                               RoadmapNodePtr_t to) const {
-    const ConfigurationPtr_t& config = node->configuration();
-    return (*distance_)(*config, *to->configuration());
+    const Configuration_t& config = node->configuration();
+    return (*distance_)(config, to->configuration());
   }
 
   inline value_type edgeCost(const core::EdgePtr_t& edge) const {

src/device.cc

@@ -50,7 +50,7 @@ pinocchio::DevicePtr_t Device::clone() const {
   return shPtr;
 }
 
-void Device::setRobotRootPosition(const std::string& rn, const Transform3f& t) {
+void Device::setRobotRootPosition(const std::string& rn, const Transform3s& t) {
   FrameIndices_t idxs = robotFrames(rn);
   if (idxs.size() == 0)
     throw std::invalid_argument("No frame for robot name " + rn);
@@ -65,7 +65,7 @@ void Device::setRobotRootPosition(const std::string& rn, const Transform3f& t) {
     return;
   }
 
-  Transform3f shift(t * rootFrame.placement.inverse());
+  Transform3s shift(t * rootFrame.placement.inverse());
   // Find all the frames that have the same parent joint.
   for (std::size_t i = 1; i < idxs.size(); ++i) {
     Frame& frame = m.frames[idxs[i]];
@@ -86,6 +86,8 @@ void Device::setRobotRootPosition(const std::string& rn, const Transform3f& t) {
     }
   }
   invalidate();
+  // Update the pool of device data.
+  numberDeviceData(numberDeviceData());
 }
 
 std::vector<std::string> Device::robotNames() const {

src/graph-optimizer.cc

@@ -83,7 +83,7 @@ PathVectorPtr_t GraphOptimizer::optimize(const PathVectorPtr_t& path) {
       // p.pathValidation(gpv->innerValidation());
       p->pathProjector(problem()->pathProjector());
       p->steeringMethod(edge->steeringMethod()->copy());
-      p->constraints(p->steeringMethod()->constraints());
+      p->constraints(edge->pathConstraint());
       if (p->constraints() && p->constraints()->configProjector()) {
         p->constraints()->configProjector()->rightHandSideFromConfig(
             toOpt->initial());

src/graph-path-validation.cc

@@ -133,11 +133,11 @@ bool GraphPathValidation::impl_validate(const PathPtr_t& path, bool reverse,
   const core::interval_t &newTR = newPath.timeRange(),
                          oldTR = oldPath.timeRange();
   Configuration_t q(newPath.outputSize());
-  if (!newPath(q, newTR.first))
+  if (!newPath.eval(q, newTR.first))
     throw std::logic_error(
         "Initial configuration of the valid part cannot be projected.");
   const graph::StatePtr_t& origState = constraintGraph_->getState(q);
-  if (!newPath(q, newTR.second))
+  if (!newPath.eval(q, newTR.second))
     throw std::logic_error(
         "End configuration of the valid part cannot be projected.");
   // This may throw in the following case:
@@ -171,7 +171,7 @@ bool GraphPathValidation::impl_validate(const PathPtr_t& path, bool reverse,
     validPart = path->extract(std::make_pair(oldTR.first, oldTR.first));
     return false;
   }
-  if (!oldPath(q, oldTR.first)) {
+  if (!oldPath.eval(q, oldTR.first)) {
     std::stringstream oss;
     oss << "Initial configuration of the path to be validated failed to"
            " be projected. After maximal number of iterations, q="
@@ -182,7 +182,7 @@ bool GraphPathValidation::impl_validate(const PathPtr_t& path, bool reverse,
     throw std::logic_error(oss.str().c_str());
   }
   const graph::StatePtr_t& oldOstate = constraintGraph_->getState(q);
-  if (!oldPath(q, oldTR.second)) {
+  if (!oldPath.eval(q, oldTR.second)) {
     std::stringstream oss;
     oss << "End configuration of the path to be validated failed to"
            " be projected. After maximal number of iterations, q="

src/graph/edge.cc

@@ -359,7 +359,7 @@ bool Edge::build(core::PathPtr_t& path, ConfigurationIn_t q1,
 
 bool Edge::generateTargetConfig(core::NodePtr_t nStart,
                                 ConfigurationOut_t q) const {
-  return generateTargetConfig(*(nStart->configuration()), q);
+  return generateTargetConfig(nStart->configuration(), q);
 }
 
 bool Edge::generateTargetConfig(ConfigurationIn_t qStart,
@@ -577,7 +577,7 @@ bool LevelSetEdge::generateTargetConfig(ConfigurationIn_t qStart,
     hppDout(warning, "Edge " << name() << ": Distrib is empty");
     return false;
   }
-  const Configuration_t& qLeaf = *(distrib()->configuration());
+  const Configuration_t& qLeaf = distrib()->configuration();
 
   return generateTargetConfigOnLeaf(qStart, qLeaf, q);
 }
@@ -592,8 +592,8 @@ bool LevelSetEdge::generateTargetConfig(core::NodePtr_t nStart,
     hppDout(warning, "Edge " << name() << ": Distrib is empty");
     return false;
   }
-  const Configuration_t &qLeaf = *(distrib()->configuration()),
-                        qStart = *(nStart->configuration());
+  const Configuration_t &qLeaf = distrib()->configuration(),
+                        qStart = nStart->configuration();
 
   return generateTargetConfigOnLeaf(qStart, qLeaf, q);
 }

src/graph/helper.cc

@@ -953,7 +953,7 @@ GraphPtr_t graphBuilder(const ProblemSolverPtr_t& ps,
     std::get<2>(objects[i]) = i;
     std::get<1>(objects[i]).resize(od.handles.size());
     Handles_t::iterator it = std::get<1>(objects[i]).begin();
-    for (const std::string hn : od.handles) {
+    for (const std::string& hn : od.handles) {
       *it = robot.handles.get(hn);
       ++it;
     }

src/graph/state-selector.cc

@@ -90,8 +90,7 @@ StatePtr_t StateSelector::getState(ConfigurationIn_t config) const {
 }
 
 StatePtr_t StateSelector::getState(RoadmapNodePtr_t node) const {
-  if (!node->cacheUpToDate())
-    node->graphState(getState(*(node->configuration())));
+  if (!node->cacheUpToDate()) node->graphState(getState(node->configuration()));
   return node->graphState();
 }
 

src/graph/statistics.cc

@@ -153,8 +153,8 @@ LeafHistogram::LeafHistogram(const Foliation f) : f_(f), threshold_(0) {
 }
 
 void LeafHistogram::add(const RoadmapNodePtr_t& n) {
-  if (!f_.contains(*n->configuration())) return;
-  iterator it = insert(LeafBin(f_.parameter(*n->configuration()), &threshold_));
+  if (!f_.contains(n->configuration())) return;
+  iterator it = insert(LeafBin(f_.parameter(n->configuration()), &threshold_));
   it->push_back(n);
   if (numberOfObservations() % 10 == 0) {
     hppDout(info, *this);

src/handle.cc

@@ -74,6 +74,7 @@ bool isHandleOnFreeflyer(const Handle& handle) {
 }
 
 inline int maskSize(const std::vector<bool>& mask) {
+  assert(mask.size() == 6);
   std::size_t res(0);
   for (std::size_t i = 0; i < 6; ++i) {
     if (mask[i]) ++res;
@@ -98,6 +99,7 @@ inline bool is6Dmask(const std::vector<bool>& mask) {
 }
 
 inline std::vector<bool> complementMask(const std::vector<bool>& mask) {
+  assert(mask.size() == 6);
   std::vector<bool> m(6);
   for (std::size_t i = 0; i < 6; ++i) m[i] = !mask[i];
   return m;
@@ -202,8 +204,8 @@ ImplicitPtr_t Handle::createGraspAndComplement(const GripperPtr_t& gripper,
 ImplicitPtr_t Handle::createPreGrasp(const GripperPtr_t& gripper,
                                      const value_type& shift,
                                      std::string n) const {
-  Transform3f M = gripper->objectPositionInJoint() *
-                  Transform3f(I3, vector3_t(shift, 0, 0));
+  Transform3s M = gripper->objectPositionInJoint() *
+                  Transform3s(I3, vector3_t(shift, 0, 0));
   if (n.empty())
     n = "Pregrasp_ " + maskToStr(mask_) + "_" + name() + "_" + gripper->name();
   ImplicitPtr_t result(Implicit::create(

src/manipulation-planner.cc

@@ -75,7 +75,7 @@ graph::StatePtr_t getState(const graph::GraphPtr_t graph,
   if (mnode != NULL)
     return mnode->graphState();
   else
-    return graph->getState(*node->configuration());
+    return graph->getState(node->configuration());
 }
 
 core::PathPtr_t connect(const Configuration_t& q1, const Configuration_t& q2,
@@ -177,13 +177,13 @@ void ManipulationPlanner::oneStep() {
   core::Nodes_t newNodes;
   core::PathPtr_t path;
 
-  typedef std::tuple<core::NodePtr_t, ConfigurationPtr_t, core::PathPtr_t>
+  typedef std::tuple<core::NodePtr_t, Configuration_t, core::PathPtr_t>
       DelayedEdge_t;
   typedef std::vector<DelayedEdge_t> DelayedEdges_t;
   DelayedEdges_t delayedEdges;
 
   // Pick a random node
-  ConfigurationPtr_t q_rand = shooter_->shoot();
+  Configuration_t q_rand = shooter_->shoot();
 
   // Extend each connected component
   for (core::ConnectedComponents_t::const_iterator itcc =
@@ -210,12 +210,11 @@ void ManipulationPlanner::oneStep() {
         value_type t_final = path->timeRange().second;
         if (t_final != path->timeRange().first) {
           bool success;
-          ConfigurationPtr_t q_new(
-              new Configuration_t(path->eval(t_final, success)));
+          Configuration_t q_new(path->eval(t_final, success));
           assert(success);
           assert(!path->constraints() ||
-                 path->constraints()->isSatisfied(*q_new));
-          assert(problem_->constraintGraph()->getState(*q_new));
+                 path->constraints()->isSatisfied(q_new));
+          assert(problem_->constraintGraph()->getState(q_new));
           delayedEdges.push_back(DelayedEdge_t(near, q_new, path));
         }
       }
@@ -226,7 +225,7 @@ void ManipulationPlanner::oneStep() {
   // Insert delayed edges
   for (const auto& edge : delayedEdges) {
     const core::NodePtr_t& near = std::get<0>(edge);
-    const ConfigurationPtr_t& q_new = std::get<1>(edge);
+    Configuration_t q_new = std::get<1>(edge);
     const core::PathPtr_t& validPath = std::get<2>(edge);
     core::NodePtr_t newNode = roadmap()->addNode(q_new);
     roadmap()->addEdge(near, newNode, validPath);
@@ -263,21 +262,21 @@ void ManipulationPlanner::oneStep() {
 }
 
 bool ManipulationPlanner::extend(RoadmapNodePtr_t n_near,
-                                 const ConfigurationPtr_t& q_rand,
+                                 ConfigurationIn_t q_rand,
                                  core::PathPtr_t& validPath) {
   graph::GraphPtr_t graph = problem_->constraintGraph();
   PathProjectorPtr_t pathProjector = problem_->pathProjector();
   pinocchio::DevicePtr_t robot(problem_->robot());
   value_type eps(graph->errorThreshold());
   // Select next node in the constraint graph.
-  const ConfigurationPtr_t q_near = n_near->configuration();
+  const Configuration_t q_near = n_near->configuration();
   HPP_START_TIMECOUNTER(chooseEdge);
   graph::EdgePtr_t edge = graph->chooseEdge(n_near);
   HPP_STOP_TIMECOUNTER(chooseEdge);
   if (!edge) {
     return false;
   }
-  qProj_ = *q_rand;
+  qProj_ = q_rand;
   HPP_START_TIMECOUNTER(generateTargetConfig);
   SuccessStatistics& es = edgeStat(edge);
   if (!edge->generateTargetConfig(n_near, qProj_)) {
@@ -286,7 +285,7 @@ bool ManipulationPlanner::extend(RoadmapNodePtr_t n_near,
     es.addFailure(reasons_[PROJECTION]);
     return false;
   }
-  if (pinocchio::isApprox(robot, qProj_, *q_near, eps)) {
+  if (pinocchio::isApprox(robot, qProj_, q_near, eps)) {
     es.addFailure(reasons_[FAILURE]);
     es.addFailure(reasons_[PATH_PROJECTION_ZERO]);
     return false;
@@ -294,7 +293,7 @@ bool ManipulationPlanner::extend(RoadmapNodePtr_t n_near,
   HPP_STOP_TIMECOUNTER(generateTargetConfig);
   core::PathPtr_t path;
   HPP_START_TIMECOUNTER(buildPath);
-  if (!edge->build(path, *q_near, qProj_)) {
+  if (!edge->build(path, q_near, qProj_)) {
     HPP_STOP_TIMECOUNTER(buildPath);
     es.addFailure(reasons_[FAILURE]);
     es.addFailure(reasons_[STEERING_METHOD]);
@@ -396,7 +395,7 @@ inline std::size_t ManipulationPlanner::tryConnectToRoadmap(
   value_type distance;
   for (core::Nodes_t::const_iterator itn1 = nodes.begin(); itn1 != nodes.end();
        ++itn1) {
-    const Configuration_t& q1(*(*itn1)->configuration());
+    const Configuration_t& q1((*itn1)->configuration());
     graph::StatePtr_t s1 = getState(graph, *itn1);
     connectSucceed = false;
     for (core::ConnectedComponents_t::const_iterator itcc =
@@ -411,7 +410,7 @@ inline std::size_t ManipulationPlanner::tryConnectToRoadmap(
         bool _2to1 = (*itn1)->isInNeighbor(*itn2);
         assert(!_1to2 || !_2to1);
 
-        const Configuration_t& q2(*(*itn2)->configuration());
+        const Configuration_t& q2((*itn2)->configuration());
         graph::StatePtr_t s2 = getState(graph, *itn2);
         assert(q1 != q2);
 
@@ -445,7 +444,7 @@ inline std::size_t ManipulationPlanner::tryConnectNewNodes(
   std::size_t nbConnection = 0;
   for (core::Nodes_t::const_iterator itn1 = nodes.begin(); itn1 != nodes.end();
        ++itn1) {
-    const Configuration_t& q1(*(*itn1)->configuration());
+    const Configuration_t& q1((*itn1)->configuration());
     graph::StatePtr_t s1 = getState(graph, *itn1);
 
     for (core::Nodes_t::const_iterator itn2 = std::next(itn1);
@@ -455,7 +454,7 @@ inline std::size_t ManipulationPlanner::tryConnectNewNodes(
       bool _1to2 = (*itn1)->isOutNeighbor(*itn2);
       bool _2to1 = (*itn1)->isInNeighbor(*itn2);
       assert(!_1to2 || !_2to1);
-      const Configuration_t& q2(*(*itn2)->configuration());
+      const Configuration_t& q2((*itn2)->configuration());
       graph::StatePtr_t s2 = getState(graph, *itn2);
       assert(q1 != q2);
 

src/path-planner/end-effector-trajectory.cc

@@ -73,7 +73,7 @@ void EndEffectorTrajectory::startSolve() {
     throw std::runtime_error(msg);
   }
 
-  if (!problem()->initConfig()) {
+  if (problem()->initConfig().size() == 0) {
     std::string msg("No init config in problem.");
     hppDout(error, msg);
     throw std::runtime_error(msg);
@@ -151,8 +151,10 @@ void EndEffectorTrajectory::oneStep() {
 
   core::interval_t timeRange(sm->timeRange());
 
+  // Generate a vector if configuration where the first one is the initial
+  // configuration and the following ones are random configurations
   std::vector<core::Configuration_t> qs(
-      configurations(*problem()->initConfig()));
+      configurations(problem()->initConfig()));
   if (qs.empty()) {
     hppDout(info, "Failed to generate initial configs.");
     return;
@@ -166,12 +168,23 @@ void EndEffectorTrajectory::oneStep() {
   vector_t times(nDiscreteSteps_ + 1);
   matrix_t steps(problem()->robot()->configSize(), nDiscreteSteps_ + 1);
 
+  // Discretize definition interval of the steering method into times
   times[0] = timeRange.first;
   for (int j = 1; j < nDiscreteSteps_; ++j)
     times[j] = timeRange.first +
                j * (timeRange.second - timeRange.first) / nDiscreteSteps_;
   times[nDiscreteSteps_] = timeRange.second;
 
+  // For each random configuration,
+  //   - compute initial configuration of path by projecting the random
+  //     configuration (initial configuration for the first time),
+  //   - compute following samples by projecting current sample after
+  //     updating right hand side.
+  // If failure, try next random configuration.
+  // Failure can be due to
+  //   - projection,
+  //   - collision of final configuration,
+  //   - validation of path (for collision mainly).
   for (i = 0; i < qs.size(); ++i) {
     if (resetRightHandSide) {
       constraints->rightHandSideAt(times[0]);
@@ -200,6 +213,7 @@ void EndEffectorTrajectory::oneStep() {
     if (!success) continue;
     success = false;
 
+    // Test collision of final configuration.
     if (!cfgValidation->validate(steps.col(nDiscreteSteps_), cfgReport)) {
       hppDout(info, "Destination config is in collision.");
       continue;
@@ -220,10 +234,13 @@ void EndEffectorTrajectory::oneStep() {
       continue;
     }
 
-    roadmap()->initNode(make_shared<Configuration_t>(steps.col(0)));
+    // In case of success,
+    //   - insert q_init as initial configuration of the roadmap,
+    //   - insert final configuration as goal node in the roadmap,
+    //   - add a roadmap edge between them and stop.
+    roadmap()->initNode(steps.col(0));
     core::NodePtr_t init = roadmap()->initNode();
-    core::NodePtr_t goal = roadmap()->addGoalNode(
-        make_shared<Configuration_t>(steps.col(nDiscreteSteps_)));
+    core::NodePtr_t goal = roadmap()->addGoalNode(steps.col(nDiscreteSteps_));
     roadmap()->addEdge(init, goal, path);
     success = true;
     if (feasibilityOnly_) break;

src/path-planner/transition-planner.cc

+// Copyright (c) 2023 CNRS
+// Authors: Florent Lamiraux
+//
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// 1. Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//
+// 2. Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
+// DAMAGE.
+
+#include <hpp/core/collision-validation.hh>
+#include <hpp/core/config-projector.hh>
+#include <hpp/core/config-validations.hh>
+#include <hpp/core/constraint-set.hh>
+#include <hpp/core/distance/reeds-shepp.hh>
+#include <hpp/core/joint-bound-validation.hh>
+#include <hpp/core/path-optimization/rs-time-parameterization.hh>
+#include <hpp/core/path-optimization/simple-time-parameterization.hh>
+#include <hpp/core/path-optimizer.hh>
+#include <hpp/core/path-planner/bi-rrt-star.hh>
+#include <hpp/core/path-projector.hh>
+#include <hpp/core/path-validation-report.hh>
+#include <hpp/core/path-validation.hh>
+#include <hpp/core/path-vector.hh>
+#include <hpp/core/problem.hh>
+#include <hpp/core/steering-method/reeds-shepp.hh>
+#include <hpp/manipulation/graph/edge.hh>
+#include <hpp/manipulation/graph/graph.hh>
+#include <hpp/manipulation/path-planner/transition-planner.hh>
+#include <hpp/manipulation/problem.hh>
+#include <hpp/manipulation/roadmap.hh>
+
+namespace hpp {
+namespace manipulation {
+namespace pathPlanner {
+TransitionPlannerPtr_t TransitionPlanner::createWithRoadmap(
+    const core::ProblemConstPtr_t& problem, const core::RoadmapPtr_t& roadmap) {
+  TransitionPlanner* ptr(new TransitionPlanner(problem, roadmap));
+  TransitionPlannerPtr_t shPtr(ptr);
+  ptr->init(shPtr);
+  return shPtr;
+}
+
+void TransitionPlanner::checkProblemAndForwardParameters() {
+  // Check that edge has been selected
+  // Initialize the planner
+  if (!innerProblem_->constraints() ||
+      !innerProblem_->constraints()->configProjector())
+    throw std::logic_error(
+        "TransitionPlanner::startSolve: inner problem has"
+        " no constraints. You probably forgot to select "
+        "the transition.");
+  // Check that the initial configuration has been initialized
+  if (innerProblem_->initConfig().size() !=
+      innerProblem_->robot()->configSize()) {
+    std::ostringstream os;
+    os << "TransitionPlanner::startSolve: initial configuration size ("
+       << innerProblem_->initConfig().size()
+       << ") differs from robot configuration size ( "
+       << innerProblem_->robot()->configSize() << "). Did you initialize it ?";
+    throw std::logic_error(os.str().c_str());
+  }
+  // Forward maximal number of iterations to inner planner
+  innerPlanner_->maxIterations(this->maxIterations());
+  // Forward timeout to inner planner
+  innerPlanner_->timeOut(this->timeOut());
+}
+
+void TransitionPlanner::startSolve() {
+  checkProblemAndForwardParameters();
+  innerProblem_->constraints()->configProjector()->rightHandSideFromConfig(
+      innerProblem_->initConfig());
+  // Call parent implementation
+  core::PathPlanner::startSolve();
+}
+
+void TransitionPlanner::oneStep() { innerPlanner_->oneStep(); }
+
+core::PathVectorPtr_t TransitionPlanner::planPath(const Configuration_t qInit,
+                                                  matrixIn_t qGoals,
+                                                  bool resetRoadmap) {
+  ConfigProjectorPtr_t configProjector(
+      innerProblem_->constraints()->configProjector());
+  if (configProjector) {
+    configProjector->rightHandSideFromConfig(qInit);
+  }
+  Configuration_t q(qInit);
+  innerProblem_->initConfig(q);
+  innerProblem_->resetGoalConfigs();
+  for (size_type r = 0; r < qGoals.rows(); ++r) {
+    Configuration_t q(qGoals.row(r));
+    if (!configProjector->isSatisfied(q)) {
+      std::ostringstream os;
+      os << "hpp::manipulation::TransitionPlanner::computePath: "
+         << "goal configuration at rank " << r
+         << " does not satisfy the leaf constraint.";
+      throw std::logic_error(os.str().c_str());
+    }
+    innerProblem_->addGoalConfig(q);
+  }
+  if (resetRoadmap) {
+    roadmap()->clear();
+  }
+  checkProblemAndForwardParameters();
+  PathVectorPtr_t path = innerPlanner_->solve();
+  path = optimizePath(path);
+  return path;
+}
+
+core::PathPtr_t TransitionPlanner::directPath(ConfigurationIn_t q1,
+                                              ConfigurationIn_t q2,
+                                              bool validate, bool& success,
+                                              std::string& status) {
+  core::PathPtr_t res(innerProblem_->steeringMethod()->steer(q1, q2));
+  if (!res) {
+    success = false;
+    status = std::string("Steering method failed");
+    return res;
+  }
+  status = std::string("");
+  core::PathProjectorPtr_t pathProjector(innerProblem_->pathProjector());
+  bool success1 = true, success2 = true;
+  core::PathPtr_t projectedPath = res;
+  if (pathProjector) {
+    success1 = pathProjector->apply(res, projectedPath);
+    if (!success1) {
+      status += std::string("Failed to project the path. ");
+    }
+  }
+  core::PathPtr_t validPart = projectedPath;
+  core::PathValidationPtr_t pathValidation(innerProblem_->pathValidation());
+  if (pathValidation && validate) {
+    core::PathValidationReportPtr_t report;
+    success2 =
+        pathValidation->validate(projectedPath, false, validPart, report);
+    if (!success2) {
+      status += std::string("Failed to validate the path. ");
+    }
+  }
+  success = success1 && success2;
+  return validPart;
+}
+
+bool TransitionPlanner::validateConfiguration(
+    ConfigurationIn_t q, std::size_t id,
+    core::ValidationReportPtr_t& report) const {
+  graph::EdgePtr_t edge(getEdgeOrThrow(id));
+  return edge->pathValidation()->validate(q, report);
+}
+
+core::PathVectorPtr_t TransitionPlanner::optimizePath(const PathPtr_t& path) {
+  PathVectorPtr_t pv(HPP_DYNAMIC_PTR_CAST(PathVector, path));
+  if (!pv) {
+    pv = core::PathVector::create(path->outputSize(),
+                                  path->outputDerivativeSize());
+    pv->appendPath(path);
+  }
+  for (auto po : pathOptimizers_) {
+    pv = po->optimize(pv);
+  }
+  return pv;
+}
+
+core::PathVectorPtr_t TransitionPlanner::timeParameterization(
+    const PathVectorPtr_t& path) {
+  return timeParameterization_->optimize(path);
+}
+
+void TransitionPlanner::setEdge(std::size_t id) {
+  graph::EdgePtr_t edge(getEdgeOrThrow(id));
+  innerProblem_->constraints(edge->pathConstraint());
+  innerProblem_->pathValidation(edge->pathValidation());
+  innerProblem_->steeringMethod(edge->steeringMethod());
+}
+
+void TransitionPlanner::setReedsAndSheppSteeringMethod(double turningRadius) {
+  core::JointPtr_t root(innerProblem_->robot()->rootJoint());
+  core::SteeringMethodPtr_t sm(core::steeringMethod::ReedsShepp::create(
+      innerProblem_, turningRadius, root, root));
+  core::DistancePtr_t dist(core::distance::ReedsShepp::create(innerProblem_));
+  innerProblem_->steeringMethod(sm);
+  innerProblem_->distance(dist);
+  timeParameterization_ =
+      core::pathOptimization::RSTimeParameterization::create(innerProblem_);
+}
+
+void TransitionPlanner::pathProjector(const PathProjectorPtr_t pathProjector) {
+  innerProblem_->pathProjector(pathProjector);
+}
+
+void TransitionPlanner::clearPathOptimizers() { pathOptimizers_.clear(); }
+
+/// Add a path optimizer
+void TransitionPlanner::addPathOptimizer(
+    const core::PathOptimizerPtr_t& pathOptimizer) {
+  pathOptimizers_.push_back(pathOptimizer);
+}
+
+void TransitionPlanner::setParameter(const std::string& key,
+                                     const core::Parameter& value) {
+  innerProblem_->setParameter(key, value);
+}
+
+TransitionPlanner::TransitionPlanner(const core::ProblemConstPtr_t& problem,
+                                     const core::RoadmapPtr_t& roadmap)
+    : PathPlanner(problem, roadmap) {
+  ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
+  if (!p)
+    throw std::invalid_argument(
+        "The problem should be of type hpp::manipulation::Problem.");
+  // create the inner problem
+  innerProblem_ = core::Problem::create(p->robot());
+  // Pass parameters from manipulation problem
+  std::vector<std::string> keys(
+      p->parameters.getKeys<std::vector<std::string> >());
+  for (auto k : keys) {
+    innerProblem_->setParameter(k, p->parameters.get(k));
+  }
+  // Initialize config validations
+  innerProblem_->clearConfigValidations();
+  innerProblem_->configValidations()->add(
+      hpp::core::CollisionValidation::create(p->robot()));
+  innerProblem_->configValidations()->add(
+      hpp::core::JointBoundValidation::create(p->robot()));
+  // Add obstacles to inner problem
+  innerProblem_->collisionObstacles(p->collisionObstacles());
+  // Create default path planner
+  innerPlanner_ = hpp::core::pathPlanner::BiRrtStar::createWithRoadmap(
+      innerProblem_, roadmap);
+  // Create default time parameterization
+  timeParameterization_ =
+      core::pathOptimization::SimpleTimeParameterization::create(innerProblem_);
+}
+
+void TransitionPlanner::init(TransitionPlannerWkPtr_t weak) {
+  core::PathPlanner::init(weak);
+  weakPtr_ = weak;
+}
+
+graph::EdgePtr_t TransitionPlanner::getEdgeOrThrow(std::size_t id) const {
+  ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem()));
+  assert(p);
+  graph::GraphComponentPtr_t comp(p->constraintGraph()->get(id).lock());
+  graph::EdgePtr_t edge(HPP_DYNAMIC_PTR_CAST(graph::Edge, comp));
+  if (!edge) {
+    std::ostringstream os;
+    os << "hpp::manipulation::pathPlanner::TransitionPlanner::setEdge: index "
+       << id << " does not correspond to any edge of the constraint graph.";
+    throw std::logic_error(os.str().c_str());
+  }
+  return edge;
+}
+
+}  // namespace pathPlanner
+}  // namespace manipulation
+}  // namespace hpp