.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: v15.0.7
-    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.1.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})
@@ -86,6 +117,8 @@ set(${PROJECT_NAME}_HEADERS
     include/hpp/manipulation/path-optimization/random-shortcut.hh
     include/hpp/manipulation/path-optimization/spline-gradient-based.hh
     include/hpp/manipulation/path-planner/end-effector-trajectory.hh
+    include/hpp/manipulation/path-planner/states-path-finder.hh
+    include/hpp/manipulation/path-planner/transition-planner.hh
     include/hpp/manipulation/problem-target/state.hh
     include/hpp/manipulation/serialization.hh
     include/hpp/manipulation/steering-method/cross-state-optimization.hh
@@ -120,6 +153,8 @@ set(${PROJECT_NAME}_SOURCES
     src/path-optimization/random-shortcut.cc
     src/path-optimization/enforce-transition-semantic.cc
     src/path-planner/end-effector-trajectory.cc
+    src/path-planner/states-path-finder.cc
+    src/path-planner/transition-planner.cc
     src/problem-target/state.cc
     src/serialization.cc
     src/steering-method/end-effector-trajectory.cc
@@ -128,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(

TODO.txt

+- InStatePath::solve
+    BiRRT* does not work properly at all:
+        - discontinuities due to an algorithmic mistake involving qProj_
+        - not using path projectors, while it should
+    DiffusingPlanner does not work properly sometimes
+        - Some collisions were detected on paths solves for romeo-placard
+          both with discrete and continuous (Progressive, 0.2) path validations
+
+
+- InStatePath::mergeLeafRoadmapWith
+    this is inefficient because the roadmap recomputes the connected
+    component at every step. A better merge function should be added to roadmap.cc
+
+
+- path optimizations after solving once :
+    They are done "locally" (for each of the leafs separately) in C++ with a
+    hard-coded optimizer (Graph-RandomShortcut) and then globally with the
+    command ps.addPathOptimizer("Graph-RandomShortcut"). Due to how this works,
+    this is like doing two times the same thing so the first should be removed.
+    However bugs have been observed when the global optimization is used (Core
+    dumped). For unknown reasons, in GraphOptimizer::optimize, the dynamic
+    cast of current->constraints() may fail on *some* 0-length paths. Ignoring
+    empty paths (the "if" I have added) seems to make the problem disappear
+    but the reason why some 0-length paths don't have the right Constraint type
+    is still to be found.

cmake @ 29c0eb4e

-Subproject commit 9403226002b930d592ca83b50d3cd714a1c2dc01
+Subproject commit 29c0eb4e659304f44d55a0389e2749812d858659

doc/Doxyfile.extra.in

@@ -8,3 +8,4 @@ ALIASES   += Link{1}="\ref \1"
 ALIASES   += Link{2}="\ref \1 \"\2\""
 ALIASES   += LHPP{2}="\Link{hpp::\1::\2,\2}"
 ALIASES   += LPinocchio{1}="\LHPP{pinocchio,\1}"
+USE_MATHJAX= YES

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,16 +88,32 @@ 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;
 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;
+HPP_PREDEF_CLASS(TransitionPlanner);
+typedef shared_ptr<TransitionPlanner> TransitionPlannerPtr_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
 typedef core::PathOptimizer PathOptimizer;
 typedef core::PathOptimizerPtr_t PathOptimizerPtr_t;
 HPP_PREDEF_CLASS(GraphOptimizer);

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),
@@ -183,6 +183,7 @@ class HPP_MANIPULATION_DLLAPI Handle {
         robot_(robot),
         clearance_(0),
         mask_(6, true),
+        maskComp_(6, false),
         weakPtr_() {}
   void init(HandleWkPtr_t weakPtr) { weakPtr_ = weakPtr; }
 
@@ -191,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/end-effector-trajectory.hh

@@ -52,6 +52,46 @@ typedef shared_ptr<IkSolverInitialization> IkSolverInitializationPtr_t;
 HPP_PREDEF_CLASS(EndEffectorTrajectory);
 typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
 
+/// \addtogroup path_planning
+/// \{
+
+/// Plan a path for a robot with constrained trajectory of an end effector
+///
+/// This path planner only works with a steering method of type
+/// steeringMethod::EndEffectorTrajectory. The steering
+/// method defines the desired end-effector trajectory using a time-varying
+/// constraint.
+///
+/// To plan a path between two configurations \c q_init and \c q_goal, the
+/// configurations must satisfy the constraint at the beginning and at the
+/// end of the definition interval respectively.
+///
+/// The interval of definition \f$[0,T]\f$ of the output path is defined by the
+/// time-varying constraint of the steering method. This interval is uniformly
+/// discretized in a number of samples that can be accessed using method \link
+/// EndEffectorTrajectory::nDiscreteSteps nDiscreteSteps \endlink.
+///
+/// The path is planned by successively calling method \link
+/// EndEffectorTrajectory::oneStep oneStep \endlink that performs the following
+/// actions.
+///   - A vector of configurations is produced by appending random
+///     configurations to \c q_init. The number of random configurations can
+///     be accessed by methods \link EndEffectorTrajectory::nRandomConfig
+///     nRandomConfig \endlink.
+///   - for each configuration in the vector,
+///     - the initial configuration of the path is computed by projecting the
+///       configuration on the constraint,
+///     - the configuration at following samples is computed by projecting the
+///       configuration at the previous sample using the time-varying
+///       constraint.
+///     - In case of failure
+///        - in projecting a configuration or
+///        - in validating the path for collision,
+///       the loop restart with the next random configuration.
+///
+/// Note that continuity is not tested but enforced by projecting the
+/// configuration of the previous sample to compute the configuration at
+/// a given sample.
 class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory : public core::PathPlanner {
  public:
   /// Return shared pointer to new instance
@@ -129,6 +169,7 @@ class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory : public core::PathPlanner {
   /// Feasibility
   bool feasibilityOnly_;
 };  // class EndEffectorTrajectory
+// \}
 }  // namespace pathPlanner
 }  // namespace manipulation
 }  // namespace hpp