.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.4
-    hooks:
-    -   id: clang-format
-        args: [--style=Google]
--   repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v4.3.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: 22.10.0
-    hooks:
-    -   id: black
--   repo: https://github.com/PyCQA/flake8
-    rev: 5.0.4
-    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 08c2c18b9033c2f0a86b885b589f4fa63a163caf
+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/edge.hh

@@ -81,30 +81,6 @@ class HPP_MANIPULATION_DLLAPI Edge : public GraphComponent {
   static EdgePtr_t create(const std::string& name, const GraphWkPtr_t& graph,
                           const StateWkPtr_t& from, const StateWkPtr_t& to);
 
-  /// Generate a reachable configuration in the target state
-  ///
-  /// \param nStart node containing the configuration defining the right
-  ///        hand side of the edge constraint,
-  /// \param[in,out] q input configuration used to initialize the
-  ///                numerical solver and output configuration lying
-  ///                in the target state and reachable along the edge
-  ///                from nnear
-  /// \deprecated Use generateTargetConfig instead.
-  virtual bool applyConstraints(core::NodePtr_t nStart, ConfigurationOut_t q)
-      const HPP_MANIPULATION_DEPRECATED;
-
-  /// Generate a reachable configuration in the target state
-  ///
-  /// \param qStart node containing the configuration defining the right
-  ///        hand side of the edge path constraint,
-  /// \param[in,out] q input configuration used to initialize the
-  ///                numerical solver and output configuration lying
-  ///                in the target state and reachable along the edge
-  ///                from nnear.
-  /// \deprecated Use generateTargetConfig instead.
-  virtual bool applyConstraints(ConfigurationIn_t qStart, ConfigurationOut_t q)
-      const HPP_MANIPULATION_DEPRECATED;
-
   /// Generate a reachable configuration in the target state
   ///
   /// \param nStart node containing the configuration defining the right
@@ -188,10 +164,6 @@ class HPP_MANIPULATION_DLLAPI Edge : public GraphComponent {
       std::ostream& os,
       dot::DrawingAttributes da = dot::DrawingAttributes()) const;
 
-  /// Constraint of the destination state and of the path
-  /// \deprecated Use targetConstraint instead
-  ConstraintSetPtr_t configConstraint() const HPP_MANIPULATION_DEPRECATED;
-
   /// Constraint of the destination state and of the path
   ConstraintSetPtr_t targetConstraint() const;
 
@@ -210,9 +182,6 @@ class HPP_MANIPULATION_DLLAPI Edge : public GraphComponent {
   /// Constructor
   Edge(const std::string& name);
 
-  virtual ConstraintSetPtr_t buildConfigConstraint()
-      HPP_MANIPULATION_DEPRECATED;
-
   /// Build path and target state constraint set.
   virtual ConstraintSetPtr_t buildTargetConstraint();
 
@@ -303,18 +272,6 @@ class HPP_MANIPULATION_DLLAPI WaypointEdge : public Edge {
   virtual bool build(core::PathPtr_t& path, ConfigurationIn_t q1,
                      ConfigurationIn_t q2) const;
 
-  /// Generate a reachable configuration in the target state
-  ///
-  /// \param qStart node containing the configuration defining the right
-  ///        hand side of the edge path constraint,
-  /// \param[in,out] q input configuration used to initialize the
-  ///                numerical solver and output configuration lying
-  ///                in the target state and reachable along the edge
-  ///                from nnear.
-  /// deprecated Used generateTargetConfig instead.
-  virtual bool applyConstraints(ConfigurationIn_t qStart, ConfigurationOut_t q)
-      const HPP_MANIPULATION_DEPRECATED;
-
   /// Generate a reachable configuration in the target state
   ///
   /// \param qStart node containing the configuration defining the right
@@ -448,30 +405,6 @@ class HPP_MANIPULATION_DLLAPI LevelSetEdge : public Edge {
                                   const StateWkPtr_t& from,
                                   const StateWkPtr_t& to);
 
-  /// Generate a reachable configuration in the target state
-  ///
-  /// \param nStart node containing the configuration defining the right
-  ///        hand side of the edge constraint,
-  /// \param[in,out] q input configuration used to initialize the
-  ///                numerical solver and output configuration lying
-  ///                in the target state and reachable along the edge
-  ///                from nnear
-  /// \deprecated Use generateTargetConfig instead.
-  virtual bool applyConstraints(core::NodePtr_t nStart, ConfigurationOut_t q)
-      const HPP_MANIPULATION_DEPRECATED;
-
-  /// Generate a reachable configuration in the target state
-  ///
-  /// \param qStart node containing the configuration defining the right
-  ///        hand side of the edge path constraint,
-  /// \param[in,out] q input configuration used to initialize the
-  ///                numerical solver and output configuration lying
-  ///                in the target state and reachable along the edge
-  ///                from nnear.
-  /// \deprecated Use generateTargetConfig instead.
-  virtual bool applyConstraints(ConfigurationIn_t qStart, ConfigurationOut_t q)
-      const HPP_MANIPULATION_DEPRECATED;
-
   /// Generate a reachable configuration in the target state
   ///
   /// \param nStart node containing the configuration defining the right
@@ -505,10 +438,6 @@ class HPP_MANIPULATION_DLLAPI LevelSetEdge : public Edge {
                                   ConfigurationIn_t qLeaf,
                                   ConfigurationOut_t q) const;
 
-  /// \deprecated Use buildTargetConstraint instead
-  virtual ConstraintSetPtr_t buildConfigConstraint()
-      HPP_MANIPULATION_DEPRECATED;
-
   /// Build path and target state constraints
   virtual ConstraintSetPtr_t buildTargetConstraint();
 

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;
@@ -92,10 +92,6 @@ class HPP_MANIPULATION_DLLAPI GraphComponent {
   /// The component needs to be initialized again.
   virtual void invalidate() { isInit_ = false; }
 
-  /// Declare a component as dirty
-  /// \deprecated call invalidate instead
-  void setDirty() HPP_MANIPULATION_DEPRECATED;
-
   /// Set whether hierachical constraints are solved level by level
   /// \sa hpp::constraints::solver::HierarchicalIterative
   void solveLevelByLevel(bool solveLevelByLevel) {

include/hpp/manipulation/graph/graph.hh

@@ -46,8 +46,7 @@ namespace graph {
 
 /// Description of the constraint graph.
 ///
-/// This class contains a graph representing a robot with several
-/// end-effectors.
+/// This class contains a graph representing a a manipulation problem
 ///
 /// One must make sure not to create loop with shared pointers.
 /// To ensure that, the classes are defined as follow:
@@ -55,6 +54,10 @@ namespace graph {
 /// - A StateSelector owns the Node s related to one gripper.
 /// - A State owns its outgoing Edge s.
 /// - An Edge does not own anything.
+///
+/// \note The graph and all its components have a unique index starting
+///       at 0 for the graph itself. The index of a component can be retrieved
+///       using method GraphComponent::id.
 class HPP_MANIPULATION_DLLAPI Graph : public GraphComponent {
  public:
   /// Create a new Graph.
@@ -137,11 +140,6 @@ class HPP_MANIPULATION_DLLAPI Graph : public GraphComponent {
   /// \return The initialized projector.
   ConstraintSetPtr_t configConstraint(const StatePtr_t& state) const;
 
-  /// Constraints of path and target state of an edge
-  /// \deprecated Use tagetConstraint instead.
-  ConstraintSetPtr_t configConstraint(const EdgePtr_t& edge) const
-      HPP_MANIPULATION_DEPRECATED;
-
   /// Constraints a configuration in target state should satisfy
   /// \param edge a transition
   /// \return The set of constraints a configuration lying in the
@@ -260,7 +258,7 @@ class HPP_MANIPULATION_DLLAPI Graph : public GraphComponent {
   void init(const GraphWkPtr_t& weak, DevicePtr_t robot);
 
   /// Constructor
-  /// \param sm a steering method to create paths from edges
+  /// \param problem a pointer to the problem
   Graph(const std::string& name, const ProblemPtr_t& problem);
 
   /// Print the object in a stream.

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;
 
@@ -203,7 +203,6 @@ class HPP_MANIPULATION_DLLLOCAL StateHistogram
   /// The constraint graph
   graph::GraphPtr_t graph_;
 };
-typedef StateHistogram NodeHistogram HPP_MANIPULATION_DEPRECATED;
 typedef shared_ptr<StateHistogram> NodeHistogramPtr_t;
 }  // namespace graph
 }  // namespace manipulation

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,7 +98,12 @@ 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 Transform3s& localPosition) {
+    localPosition_ = localPosition;
+  }
 
   /// Set constraint mask
   void mask(const std::vector<bool>& mask);
@@ -170,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),
@@ -178,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; }
 
@@ -186,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