.github/workflows/nix.yml

+name: "CI - Nix"
+
+on:
+  push:
+
+jobs:
+  tests:
+    name: "Nix build on ${{ matrix.os }}"
+    runs-on: "${{ matrix.os }}-latest"
+    strategy:
+      matrix:
+        os: [ubuntu]
+    steps:
+      - uses: actions/checkout@v4
+      - uses: cachix/install-nix-action@v27
+      - uses: cachix/cachix-action@v15
+        with:
+          name: gepetto
+          authToken: '${{ secrets.CACHIX_AUTH_TOKEN }}'
+      - run: nix build -L

.gitlab-ci.yml

-include: http://rainboard.laas.fr/project/hpp-manipulation/.gitlab-ci.yml
+include: https://rainboard.laas.fr/project/hpp-manipulation/.gitlab-ci.yml

.mergify.yml

+pull_request_rules:
+  - name: merge automatically when CI passes and PR is approved
+    conditions:
+      - check-success = "gitlab-ci"
+      - check-success = "Nix build on ubuntu"
+      - check-success = "pre-commit.ci - pr"
+      - or:
+        - author = pre-commit-ci[bot]
+        - author = dependabot[bot]
+    actions:
+      merge:

.pre-commit-config.yaml

 ci:
-    autoupdate_branch: 'devel'
+  autoupdate_branch: devel
 repos:
--   repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v16.0.6
-    hooks:
-    -   id: clang-format
-        args: [--style=Google]
--   repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v4.4.0
-    hooks:
-    -   id: check-added-large-files
-    -   id: check-ast
-    -   id: check-executables-have-shebangs
-    -   id: check-json
-    -   id: check-merge-conflict
-    -   id: check-symlinks
-    -   id: check-toml
-    -   id: check-yaml
-    -   id: debug-statements
-    -   id: destroyed-symlinks
-    -   id: detect-private-key
-    -   id: end-of-file-fixer
-    -   id: fix-byte-order-marker
-    -   id: mixed-line-ending
-    -   id: trailing-whitespace
--   repo: https://github.com/psf/black
-    rev: 23.3.0
-    hooks:
-    -   id: black
--   repo: https://github.com/PyCQA/flake8
-    rev: 6.0.0
-    hooks:
-    -   id: flake8
--   repo: https://github.com/cheshirekow/cmake-format-precommit
-    rev: v0.6.13
-    hooks:
-    - id: cmake-format
+- repo: https://github.com/astral-sh/ruff-pre-commit
+  rev: v0.8.1
+  hooks:
+  - id: ruff
+    args:
+    - --fix
+    - --exit-non-zero-on-fix
+  - id: ruff-format
+- repo: https://github.com/cheshirekow/cmake-format-precommit
+  rev: v0.6.13
+  hooks:
+  - id: cmake-format
+- repo: https://github.com/pappasam/toml-sort
+  rev: v0.24.2
+  hooks:
+  - id: toml-sort-fix
+    exclude: poetry.lock
+- repo: https://github.com/pre-commit/mirrors-clang-format
+  rev: v19.1.4
+  hooks:
+  - id: clang-format
+    args:
+    - --style=Google
+- repo: https://github.com/pre-commit/pre-commit-hooks
+  rev: v5.0.0
+  hooks:
+  - id: check-added-large-files
+  - id: check-ast
+  - id: check-executables-have-shebangs
+  - id: check-json
+  - id: check-merge-conflict
+  - id: check-symlinks
+  - id: check-toml
+  - id: check-yaml
+  - id: debug-statements
+  - id: destroyed-symlinks
+  - id: detect-private-key
+  - id: end-of-file-fixer
+  - id: fix-byte-order-marker
+  - id: mixed-line-ending
+  - id: trailing-whitespace

CMakeLists.txt

@@ -23,7 +23,7 @@
 # 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.
 
-cmake_minimum_required(VERSION 3.1)
+cmake_minimum_required(VERSION 3.10)
 
 set(PROJECT_NAME hpp-manipulation)
 set(PROJECT_DESCRIPTION "Classes for manipulation planning.")
@@ -31,9 +31,40 @@ set(PROJECT_DESCRIPTION "Classes for manipulation planning.")
 set(PROJECT_USE_CMAKE_EXPORT TRUE)
 set(CXX_DISABLE_WERROR TRUE)
 
-include(cmake/hpp.cmake)
-include(cmake/boost.cmake)
-include(cmake/test.cmake)
+# Check if the submodule cmake have been initialized
+set(JRL_CMAKE_MODULES "${CMAKE_CURRENT_LIST_DIR}/cmake")
+if(EXISTS "${JRL_CMAKE_MODULES}/base.cmake")
+  message(STATUS "JRL cmakemodules found in 'cmake/' git submodule")
+else()
+  find_package(jrl-cmakemodules QUIET CONFIG)
+  if(jrl-cmakemodules_FOUND)
+    get_property(
+      JRL_CMAKE_MODULES
+      TARGET jrl-cmakemodules::jrl-cmakemodules
+      PROPERTY INTERFACE_INCLUDE_DIRECTORIES)
+    message(STATUS "JRL cmakemodules found on system at ${JRL_CMAKE_MODULES}")
+  elseif(${CMAKE_VERSION} VERSION_LESS "3.14.0")
+    message(
+      FATAL_ERROR
+        "\nCan't find jrl-cmakemodules. Please either:\n"
+        "  - use git submodule: 'git submodule update --init'\n"
+        "  - or install https://github.com/jrl-umi3218/jrl-cmakemodules\n"
+        "  - or upgrade your CMake version to >= 3.14 to allow automatic fetching\n"
+    )
+  else()
+    message(STATUS "JRL cmakemodules not found. Let's fetch it.")
+    include(FetchContent)
+    FetchContent_Declare(
+      "jrl-cmakemodules"
+      GIT_REPOSITORY "https://github.com/jrl-umi3218/jrl-cmakemodules.git")
+    FetchContent_MakeAvailable("jrl-cmakemodules")
+    FetchContent_GetProperties("jrl-cmakemodules" SOURCE_DIR JRL_CMAKE_MODULES)
+  endif()
+endif()
+
+include("${JRL_CMAKE_MODULES}/hpp.cmake")
+include("${JRL_CMAKE_MODULES}/boost.cmake")
+include("${JRL_CMAKE_MODULES}/test.cmake")
 
 compute_project_args(PROJECT_ARGS LANGUAGES CXX)
 project(${PROJECT_NAME} ${PROJECT_ARGS})
@@ -132,7 +163,8 @@ set(${PROJECT_NAME}_SOURCES
 
 add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES}
                                    ${${PROJECT_NAME}_HEADERS})
-target_include_directories(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>)
+target_include_directories(
+  ${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>)
 target_link_libraries(${PROJECT_NAME} hpp-core::hpp-core Boost::regex)
 
 install(

cmake @ 29c0eb4e

-Subproject commit 9403226002b930d592ca83b50d3cd714a1c2dc01
+Subproject commit 29c0eb4e659304f44d55a0389e2749812d858659

flake.lock

+{
+  "nodes": {
+    "flake-parts": {
+      "inputs": {
+        "nixpkgs-lib": [
+          "nixpkgs"
+        ]
+      },
+      "locked": {
+        "lastModified": 1719877454,
+        "narHash": "sha256-g5N1yyOSsPNiOlFfkuI/wcUjmtah+nxdImJqrSATjOU=",
+        "owner": "hercules-ci",
+        "repo": "flake-parts",
+        "rev": "4e3583423212f9303aa1a6337f8dffb415920e4f",
+        "type": "github"
+      },
+      "original": {
+        "owner": "hercules-ci",
+        "repo": "flake-parts",
+        "type": "github"
+      }
+    },
+    "nixpkgs": {
+      "locked": {
+        "lastModified": 1727174734,
+        "narHash": "sha256-xa3TynMF5vaWonmTOg/Ejc1Fmo0GkQnCaVRVkBc3z2I=",
+        "owner": "gepetto",
+        "repo": "nixpkgs",
+        "rev": "0ad139a0e4372abc12320c8c92ee90e0e5e296e1",
+        "type": "github"
+      },
+      "original": {
+        "owner": "gepetto",
+        "repo": "nixpkgs",
+        "type": "github"
+      }
+    },
+    "root": {
+      "inputs": {
+        "flake-parts": "flake-parts",
+        "nixpkgs": "nixpkgs"
+      }
+    }
+  },
+  "root": "root",
+  "version": 7
+}

flake.nix

+{
+  description = "Classes for manipulation planning";
+
+  inputs = {
+    nixpkgs.url = "github:gepetto/nixpkgs";
+    flake-parts = {
+      url = "github:hercules-ci/flake-parts";
+      inputs.nixpkgs-lib.follows = "nixpkgs";
+    };
+  };
+
+  outputs =
+    inputs:
+    inputs.flake-parts.lib.mkFlake { inherit inputs; } {
+      systems = [
+        "x86_64-linux"
+        "aarch64-linux"
+        "aarch64-darwin"
+        "x86_64-darwin"
+      ];
+      perSystem =
+        { pkgs, self', ... }:
+        {
+          devShells.default = pkgs.mkShell { inputsFrom = [ self'.packages.default ]; };
+          packages = {
+            default = self'.packages.hpp-manipulation;
+            hpp-manipulation = pkgs.hpp-manipulation.overrideAttrs (_: {
+              src = pkgs.lib.fileset.toSource {
+                root = ./.;
+                fileset = pkgs.lib.fileset.unions [
+                  ./CMakeLists.txt
+                  ./doc
+                  ./include
+                  ./package.xml
+                  ./plugins
+                  ./src
+                  ./tests
+                ];
+              };
+            });
+          };
+        };
+    };
+}

include/hpp/manipulation/device.hh

@@ -64,7 +64,7 @@ class HPP_MANIPULATION_DLLAPI Device : public pinocchio::HumanoidRobot {
   virtual std::ostream& print(std::ostream& os) const;
 
   void setRobotRootPosition(const std::string& robotName,
-                            const Transform3f& positionWRTParentJoint);
+                            const Transform3s& positionWRTParentJoint);
 
   virtual pinocchio::DevicePtr_t clone() const;
 

include/hpp/manipulation/fwd.hh

@@ -48,7 +48,6 @@ typedef std::vector<pinocchio::FrameIndex> FrameIndices_t;
 typedef pinocchio::Configuration_t Configuration_t;
 typedef pinocchio::ConfigurationIn_t ConfigurationIn_t;
 typedef pinocchio::ConfigurationOut_t ConfigurationOut_t;
-typedef core::ConfigurationPtr_t ConfigurationPtr_t;
 typedef pinocchio::GripperPtr_t GripperPtr_t;
 typedef pinocchio::LiegroupElement LiegroupElement;
 typedef pinocchio::LiegroupSpace LiegroupSpace;
@@ -89,7 +88,7 @@ typedef constraints::RelativeTransformationR3xSO3 RelativeTransformationR3xSO3;
 typedef constraints::RelativeTransformationPtr_t RelativeTransformationPtr_t;
 typedef core::value_type value_type;
 typedef core::size_type size_type;
-typedef core::Transform3f Transform3f;
+typedef core::Transform3s Transform3s;
 typedef core::vector_t vector_t;
 typedef core::vectorIn_t vectorIn_t;
 typedef core::vectorOut_t vectorOut_t;

include/hpp/manipulation/graph/dot.hh

@@ -54,7 +54,7 @@ struct DrawingAttributes {
   }
   std::string& operator[](const std::string& K) { return attr[K]; }
   DrawingAttributes()
-      : separator(", "), openSection("["), closeSection("]"), attr(){};
+      : separator(", "), openSection("["), closeSection("]"), attr() {};
 };
 
 struct Tooltip {
@@ -62,7 +62,7 @@ struct Tooltip {
   typedef std::list<std::string> TooltipLineVector;
   TooltipLineVector v;
 
-  Tooltip() : v(){};
+  Tooltip() : v() {};
   inline std::string toStr() const {
     std::stringstream ss;
     size_t i = v.size();

include/hpp/manipulation/graph/graph-component.hh

@@ -50,7 +50,7 @@ namespace graph {
 /// Define common methods of the graph components.
 class HPP_MANIPULATION_DLLAPI GraphComponent {
  public:
-  virtual ~GraphComponent(){};
+  virtual ~GraphComponent() {};
 
   /// Get the component name.
   const std::string& name() const;

include/hpp/manipulation/graph/state-selector.hh

@@ -41,7 +41,7 @@ namespace graph {
 /// be ordered because a configuration can be in several states.
 class HPP_MANIPULATION_DLLAPI StateSelector {
  public:
-  virtual ~StateSelector(){};
+  virtual ~StateSelector() {};
 
   /// Create a new StateSelector.
   static StateSelectorPtr_t create(const std::string& name);

include/hpp/manipulation/graph/statistics.hh

@@ -103,7 +103,7 @@ class HPP_MANIPULATION_DLLLOCAL NodeBin : public ::hpp::statistics::Bin {
 
 class HPP_MANIPULATION_DLLLOCAL Histogram {
  public:
-  virtual ~Histogram(){};
+  virtual ~Histogram() {};
 
   virtual void add(const RoadmapNodePtr_t& node) = 0;
 

include/hpp/manipulation/handle.hh

@@ -58,7 +58,7 @@ typedef constraints::ImplicitPtr_t ImplicitPtr_t;
 class HPP_MANIPULATION_DLLAPI Handle {
  public:
   static std::string className;
-  virtual ~Handle(){};
+  virtual ~Handle() {};
 
   /// Create constraint corresponding to a gripper grasping this object
   /// \param robot the robot that grasps the handle,
@@ -66,7 +66,7 @@ class HPP_MANIPULATION_DLLAPI Handle {
   /// \return the constraint of relative position between the handle and
   ///         the gripper.
   static HandlePtr_t create(const std::string& name,
-                            const Transform3f& localPosition,
+                            const Transform3s& localPosition,
                             const DeviceWkPtr_t& robot,
                             const JointPtr_t& joint) {
     Handle* ptr = new Handle(name, localPosition, robot, joint);
@@ -98,10 +98,10 @@ class HPP_MANIPULATION_DLLAPI Handle {
   /// \}
 
   /// Get local position in joint frame
-  const Transform3f& localPosition() const { return localPosition_; }
+  const Transform3s& localPosition() const { return localPosition_; }
 
   /// Set local position in joint frame
-  void localPosition(const Transform3f& localPosition) {
+  void localPosition(const Transform3s& localPosition) {
     localPosition_ = localPosition;
   }
 
@@ -175,7 +175,7 @@ class HPP_MANIPULATION_DLLAPI Handle {
   /// \param grasp object containing the grasp information
   /// \return the constraint of relative position between the handle and
   ///         the gripper.
-  Handle(const std::string& name, const Transform3f& localPosition,
+  Handle(const std::string& name, const Transform3s& localPosition,
          const DeviceWkPtr_t& robot, const JointPtr_t& joint)
       : name_(name),
         localPosition_(localPosition),
@@ -192,7 +192,7 @@ class HPP_MANIPULATION_DLLAPI Handle {
  private:
   std::string name_;
   /// Position of the handle in the joint frame.
-  Transform3f localPosition_;
+  Transform3s localPosition_;
   /// Joint to which the handle is linked.
   JointPtr_t joint_;
   /// Pointer to the robot

include/hpp/manipulation/manipulation-planner.hh

@@ -65,7 +65,7 @@ class HPP_MANIPULATION_DLLAPI ManipulationPlanner
   /// \retval validPath the longest valid path (possibly of length 0),
   ///         resulting from the extension.
   /// \return True if the returned path is valid.
-  bool extend(RoadmapNodePtr_t q_near, const ConfigurationPtr_t& q_rand,
+  bool extend(RoadmapNodePtr_t q_near, ConfigurationIn_t q_rand,
               core::PathPtr_t& validPath);
 
   /// Get the number of occurrence of each errors.

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

@@ -30,6 +30,7 @@
 #include <hpp/manipulation/config.hh>
 #include <hpp/manipulation/fwd.hh>
 #include <hpp/manipulation/problem.hh>
+#include <unordered_map>
 
 namespace hpp {
 namespace manipulation {
@@ -100,7 +101,7 @@ class HPP_MANIPULATION_DLLAPI StatesPathFinder : public core::PathPlanner {
  public:
   struct OptimizationData;
 
-  virtual ~StatesPathFinder(){};
+  virtual ~StatesPathFinder() {};
 
   static StatesPathFinderPtr_t create(const core::ProblemConstPtr_t& problem);
 
@@ -117,7 +118,7 @@ class HPP_MANIPULATION_DLLAPI StatesPathFinder : public core::PathPlanner {
   /// \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);
+                                           ConfigurationIn_t q2);
 
   // access functions for Python interface
   std::vector<std::string> constraintNamesFromSolverAtWaypoint(std::size_t wp);
@@ -193,14 +194,66 @@ class HPP_MANIPULATION_DLLAPI StatesPathFinder : public core::PathPlanner {
 
   /// 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);
+  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;
 
@@ -246,7 +299,7 @@ class HPP_MANIPULATION_DLLAPI StatesPathFinder : public core::PathPlanner {
   NumericalConstraints_t goalConstraints_;
   bool goalDefinedByConstraints_;
   // Variables used across several calls to oneStep
-  ConfigurationPtr_t q1_, q2_;
+  Configuration_t q1_, q2_;
   core::Configurations_t configList_;
   std::size_t idxConfigList_;
   size_type nTryConfigList_;

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

@@ -31,6 +31,7 @@
 
 #include <hpp/core/path-planner.hh>
 #include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/fwd.hh>
 
 namespace hpp {
 namespace manipulation {
@@ -108,8 +109,13 @@ class HPP_MANIPULATION_DLLAPI TransitionPlanner : public core::PathPlanner {
   /// \retval status a message in case of failure.
   /// If a path projector has been selected, the path is tested for
   /// continuity.
-  PathPtr_t directPath(const Configuration_t& q1, const Configuration_t& q2,
+  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
@@ -156,12 +162,18 @@ class HPP_MANIPULATION_DLLAPI TransitionPlanner : public core::PathPlanner {
   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

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
   /// \{