diff --git a/doc/configure.py b/doc/configure.py index b9e95cd..0b7d998 100755 --- a/doc/configure.py +++ b/doc/configure.py @@ -123,6 +123,7 @@ def _xmlDirFromPkgConfig(pkg): "hpp::manipulation": _xmlDirFromPkgConfig("hpp-manipulation"), "hpp::manipulation::graph": _xmlDirFromPkgConfig("hpp-manipulation"), "hpp::manipulation::pathPlanner": _xmlDirFromPkgConfig("hpp-manipulation"), + "hpp::manipulation::steeringMethod": _xmlDirFromPkgConfig("hpp-manipulation"), } diff --git a/flake.lock b/flake.lock index c1081bc..bc13ec9 100644 --- a/flake.lock +++ b/flake.lock @@ -60,11 +60,11 @@ "uv2nix": "uv2nix" }, "locked": { - "lastModified": 1772976349, - "narHash": "sha256-DH4c3vC/8liquLNrVRQoL/uf/NZSiLi7zItE6zxZSes=", + "lastModified": 1773524302, + "narHash": "sha256-0phiMh2h3tZoe3rJGWWKMMJhuLMMHa65hRtgkzbXBJQ=", "owner": "gepetto", "repo": "gazebros2nix", - "rev": "0a6e449fa1ab8e614ef1b1a6efe494148c80a866", + "rev": "0eb9151c41ec370e29d4d4ae1640aa4c6c4aa7a7", "type": "github" }, "original": { @@ -81,6 +81,7 @@ "flake-parts" ], "gazebros2nix": "gazebros2nix", + "home-manager": "home-manager", "nix-ros-overlay": [ "gepetto", "gazebros2nix", @@ -107,11 +108,11 @@ ] }, "locked": { - "lastModified": 1773065720, - "narHash": "sha256-KU7nySVMIH4W2r9NKhcFj5QjP34uD/EzDxsIfIFTHV4=", + "lastModified": 1773834979, + "narHash": "sha256-pxEUfvPZG3lMMiorJxKPt0kjjNE2gQDA3w/cRHvSUX4=", "owner": "gepetto", "repo": "nix", - "rev": "4dfbbb57b9e998529dd5f04da0f240341521f19c", + "rev": "57036f98001a3b66384cd68ace974d237e3fa78b", "type": "github" }, "original": { @@ -135,17 +136,89 @@ "type": "github" } }, + "home-manager": { + "inputs": { + "nixpkgs": [ + "gepetto", + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1773681845, + "narHash": "sha256-o8hrZrigP0JYcwnglCp8Zi8jQafWsxbDtRRPzuVwFxY=", + "owner": "nix-community", + "repo": "home-manager", + "rev": "0759e0e137305bc9d0c52c204c6d8dffe6f601a6", + "type": "github" + }, + "original": { + "owner": "nix-community", + "ref": "release-25.11", + "repo": "home-manager", + "type": "github" + } + }, + "hpp-manipulation": { + "inputs": { + "flake-parts": [ + "hpp-manipulation", + "gepetto", + "flake-parts" + ], + "gazebros2nix": [ + "hpp-manipulation", + "gepetto", + "gazebros2nix" + ], + "gepetto": [ + "gepetto" + ], + "nix-ros-overlay": [ + "hpp-manipulation", + "gepetto", + "nix-ros-overlay" + ], + "nixpkgs": [ + "hpp-manipulation", + "gepetto", + "nixpkgs" + ], + "systems": [ + "hpp-manipulation", + "gepetto", + "systems" + ], + "treefmt-nix": [ + "hpp-manipulation", + "gepetto", + "treefmt-nix" + ] + }, + "locked": { + "lastModified": 1773846451, + "narHash": "sha256-J3t/rLFMEWvlRFByGKbBGzH/dopFA1bq9T3Vlyyell8=", + "owner": "humanoid-path-planner", + "repo": "hpp-manipulation", + "rev": "e413c6f7e3e3315c403424ab9a2520d7868d1dc6", + "type": "github" + }, + "original": { + "owner": "humanoid-path-planner", + "repo": "hpp-manipulation", + "type": "github" + } + }, "nix-ros-overlay": { "inputs": { "flake-utils": "flake-utils", "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1772890336, - "narHash": "sha256-pjO4Od9QdCWDMmE/utZ2YVyxAL8+jo5oEpnQQ9dV0rc=", + "lastModified": 1773393994, + "narHash": "sha256-zm24obq1c4ielRR8KlUQ2M5XnNfUIcQjN++9s6CFvxc=", "owner": "lopsided98", "repo": "nix-ros-overlay", - "rev": "8fe70ded2467c777c14bbd7087e57b0d3d88110e", + "rev": "a522c5a050a6d50471a29bbf6a060e2df16abf44", "type": "github" }, "original": { @@ -248,11 +321,11 @@ ] }, "locked": { - "lastModified": 1772865871, - "narHash": "sha256-/ZTSg97aouL0SlPHaokA4r3iuH9QzHVuWPACD2CUCFY=", + "lastModified": 1773190977, + "narHash": "sha256-fkJvOxz80cJViPz6GVaayC8BVCs5fclJ8qHDaNUpoEA=", "owner": "pyproject-nix", "repo": "pyproject.nix", - "rev": "e537db02e72d553cea470976b9733581bcf5b3ed", + "rev": "10ebca8a137bf26b7fbd3e94b339bf68cee18693", "type": "github" }, "original": { @@ -272,6 +345,7 @@ "gazebros2nix" ], "gepetto": "gepetto", + "hpp-manipulation": "hpp-manipulation", "nix-ros-overlay": [ "gepetto", "nix-ros-overlay" @@ -369,11 +443,11 @@ ] }, "locked": { - "lastModified": 1772660329, - "narHash": "sha256-IjU1FxYqm+VDe5qIOxoW+pISBlGvVApRjiw/Y/ttJzY=", + "lastModified": 1773297127, + "narHash": "sha256-6E/yhXP7Oy/NbXtf1ktzmU8SdVqJQ09HC/48ebEGBpk=", "owner": "numtide", "repo": "treefmt-nix", - "rev": "3710e0e1218041bbad640352a0440114b1e10428", + "rev": "71b125cd05fbfd78cab3e070b73544abe24c5016", "type": "github" }, "original": { @@ -396,11 +470,11 @@ ] }, "locked": { - "lastModified": 1772545244, - "narHash": "sha256-Ys+5UMOqp2kRvnSjyBcvGnjOhkIXB88On1ZcAstz1vY=", + "lastModified": 1773359304, + "narHash": "sha256-knv2C6tIk5ysix+9TxWIenPvpB20kFjQ1CH6SJMBNsU=", "owner": "pyproject-nix", "repo": "uv2nix", - "rev": "482aba340ded40ef557d331315f227d5eba84ced", + "rev": "27b135ea72ab1637fc5845a61c101ea66d6636d6", "type": "github" }, "original": { diff --git a/flake.nix b/flake.nix index 42d159c..6f0c1ea 100644 --- a/flake.nix +++ b/flake.nix @@ -9,6 +9,10 @@ nix-ros-overlay.follows = "gepetto/nix-ros-overlay"; systems.follows = "gepetto/systems"; treefmt-nix.follows = "gepetto/treefmt-nix"; + + # https://github.com/humanoid-path-planner/hpp-manipulation/pull/262 + hpp-manipulation.url = "github:humanoid-path-planner/hpp-manipulation"; + hpp-manipulation.inputs.gepetto.follows = "gepetto"; }; outputs = @@ -20,24 +24,27 @@ imports = [ inputs.gepetto.flakeModule { - gazebros2nix.pyOverrides.hpp-python = - _final: python-final: - (super: { - propagatedBuildInputs = super.propagatedBuildInputs ++ [ - python-final.lxml - ]; - src = lib.fileset.toSource { - root = ./.; - fileset = lib.fileset.unions [ - ./CMakeLists.txt - ./doc - ./include - ./package.xml - ./src - ./tests + gazebros2nix = { + overlays = [ inputs.hpp-manipulation.overlays.default ]; + pyOverrides.hpp-python = + _final: python-final: + (super: { + propagatedBuildInputs = super.propagatedBuildInputs ++ [ + python-final.lxml ]; - }; - }); + src = lib.fileset.toSource { + root = ./.; + fileset = lib.fileset.unions [ + ./CMakeLists.txt + ./doc + ./include + ./package.xml + ./src + ./tests + ]; + }; + }); + }; } ]; } diff --git a/include/pyhpp/manipulation/steering-method/cartesian.hh b/include/pyhpp/manipulation/steering-method/cartesian.hh new file mode 100644 index 0000000..4c02b1e --- /dev/null +++ b/include/pyhpp/manipulation/steering-method/cartesian.hh @@ -0,0 +1,74 @@ +// +// Copyright (c) 2026, 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 PYHPP_MANIPULATION_STEERING_METHOD_CARTESIAN_HH +#define PYHPP_MANIPULATION_STEERING_METHOD_CARTESIAN_HH + +#include +#include + +namespace pyhpp { +namespace manipulation { +namespace steeringMethod { + +typedef hpp::constraints::Configuration_t Configuration_t; +typedef hpp::constraints::DifferentiableFunctionPtr_t + DifferentiableFunctionPtr_t; +typedef hpp::constraints::interval_t interval_t; +typedef hpp::constraints::ImplicitPtr_t ImplicitPtr_t; +typedef hpp::constraints::size_type size_type; +typedef hpp::constraints::value_type value_type; +typedef hpp::core::PathPtr_t PathPtr_t; + +class Cartesian { + public: + hpp::manipulation::steeringMethod::CartesianPtr_t obj; + Cartesian(const pyhpp::core::Problem& problem); + void setMaxIterations(size_type iterations); + size_type getMaxIterations() const; + void setErrorThreshold(value_type threshold); + value_type getErrorThreshold() const; + void setTrajectoryConstraint(const ImplicitPtr_t& ic); + ImplicitPtr_t getTrajectoryConstraint(); + void setRightHandSide1(const PathPtr_t& rhs, bool se3Output); + void setRightHandSide2(const DifferentiableFunctionPtr_t& rhs, + const interval_t& timeRange); + DifferentiableFunctionPtr_t getRightHandSide() const; + interval_t getTimeRange() const; + size_type getNDiscreteSteps() const; + void setNDiscreteSteps(size_type n); + boost::python::tuple planPath(const Configuration_t& q_init); +}; + +void exposeCartesian(); +} // namespace steeringMethod +} // namespace manipulation +} // namespace pyhpp + +#endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 4938e7f..37bdee6 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -159,6 +159,14 @@ add_python_library( LINK_LIBRARIES hpp-manipulation::hpp-manipulation) +add_python_library( + pyhpp/manipulation/steering_method + FILES + pyhpp/manipulation/steering_method/cartesian.cc + pyhpp/manipulation/steering_method/bindings.cc + LINK_LIBRARIES + hpp-manipulation::hpp-manipulation) + add_python_library( pyhpp/manipulation/urdf FILES pyhpp/manipulation/urdf/util.cc pyhpp/manipulation/urdf/bindings.cc LINK_LIBRARIES diff --git a/src/pyhpp/manipulation/path-planner.cc b/src/pyhpp/manipulation/path-planner.cc index 89bc482..aba4fe4 100644 --- a/src/pyhpp/manipulation/path-planner.cc +++ b/src/pyhpp/manipulation/path-planner.cc @@ -187,6 +187,10 @@ void TransitionPlanner::addPathOptimizer( // EndEffectorTrajectory implementation EndEffectorTrajectory::EndEffectorTrajectory( const pyhpp::core::Problem& problem) { + boost::python::object warnings = boost::python::import("warnings"); + warnings.attr("warn")( + "pyhpp.manipulation.EndEffectorTrajectory is deprecated. " + "Use pyhpp.manipulation.steering_method.Cartesian instead."); obj = hpp::manipulation::pathPlanner::EndEffectorTrajectory::createWithRoadmap( problem.obj, hpp::core::Roadmap::create(problem.obj->distance(), diff --git a/src/pyhpp/manipulation/steering-method.cc b/src/pyhpp/manipulation/steering-method.cc index ecd8808..46778fe 100644 --- a/src/pyhpp/manipulation/steering-method.cc +++ b/src/pyhpp/manipulation/steering-method.cc @@ -31,6 +31,7 @@ #include #include #include +#include #include // DocNamespace(hpp::manipulation) diff --git a/src/pyhpp/manipulation/steering_method/bindings.cc b/src/pyhpp/manipulation/steering_method/bindings.cc new file mode 100644 index 0000000..57510a1 --- /dev/null +++ b/src/pyhpp/manipulation/steering_method/bindings.cc @@ -0,0 +1,41 @@ +// +// Copyright (c) 2026, 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 +#include +#include + +BOOST_PYTHON_MODULE(bindings) { + INIT_PYHPP_MODULE; + + boost::python::import("pyhpp.pinocchio"); + boost::python::import("pyhpp.constraints"); + boost::python::import("pyhpp.core"); + pyhpp::manipulation::steeringMethod::exposeCartesian(); +} diff --git a/src/pyhpp/manipulation/steering_method/cartesian.cc b/src/pyhpp/manipulation/steering_method/cartesian.cc new file mode 100644 index 0000000..2fe4e92 --- /dev/null +++ b/src/pyhpp/manipulation/steering_method/cartesian.cc @@ -0,0 +1,155 @@ +// +// Copyright (c) 2026, 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 +#include +#include +#include +#include + +// DocNamespace(hpp::manipulation::steeringMethod) + +namespace pyhpp { +namespace manipulation { +namespace steeringMethod { +using boost::python::class_; + +Cartesian::Cartesian(const pyhpp::core::Problem& problem) + : obj(hpp::manipulation::steeringMethod::Cartesian::create(problem.obj)) {} + +void Cartesian::setMaxIterations(size_type iterations) { + obj->maxIterations(iterations); +} +size_type Cartesian::getMaxIterations() const { return obj->maxIterations(); } +void Cartesian::setErrorThreshold(value_type threshold) { + obj->errorThreshold(threshold); +} +value_type Cartesian::getErrorThreshold() const { + return obj->errorThreshold(); +} +void Cartesian::setTrajectoryConstraint(const ImplicitPtr_t& ic) { + obj->trajectoryConstraint(ic); +} +ImplicitPtr_t Cartesian::getTrajectoryConstraint() { + return obj->trajectoryConstraint(); +} +void Cartesian::setRightHandSide1(const PathPtr_t& rhs, bool se3Output) { + obj->rightHandSide(rhs, se3Output); +} +void Cartesian::setRightHandSide2(const DifferentiableFunctionPtr_t& rhs, + const interval_t& timeRange) { + return obj->rightHandSide(rhs, timeRange); +} +DifferentiableFunctionPtr_t Cartesian::getRightHandSide() const { + return obj->rightHandSide(); +} +interval_t Cartesian::getTimeRange() const { return obj->timeRange(); } +size_type Cartesian::getNDiscreteSteps() const { return obj->nDiscreteSteps(); } +void Cartesian::setNDiscreteSteps(size_type n) { obj->nDiscreteSteps(n); } +boost::python::tuple Cartesian::planPath(const Configuration_t& q_init) { + PathPtr_t result; + bool success; + success = obj->planPath(q_init, result); + return boost::python::make_tuple(success, result); +} + +void exposeCartesian() { + // DocClass(Cartesian) + boost::python::class_( + "Cartesian", boost::python::init()) + .add_property("maxIterations", &Cartesian::getMaxIterations, + &Cartesian::setMaxIterations, + "Maximal number of iterations of numerical solver.") + .add_property("errorThreshold", &Cartesian::getErrorThreshold, + &Cartesian::setErrorThreshold, + "Error threshold of numerical solver.") + .add_property("trajectoryConstraint", &Cartesian::getTrajectoryConstraint, + &Cartesian::setTrajectoryConstraint, + "Constraint with a time-varying right hand side.") + .add_property( + "nDiscreteSteps", &Cartesian::getNDiscreteSteps, + &Cartesian::setNDiscreteSteps, + "Number of discretization steps in the interval of definition\n" + "where configurations are computed.") + .def("timeRange", &Cartesian::getTimeRange, + "Get interval of definition of right hand side of trajectory " + "constraint.") + .def("setRightHandSide", &Cartesian::setRightHandSide1, + "Set right hand side from a hpp::core::Path\n" + "\n:param rhs:function from an interval to SE(3).\n:param " + "se3Output:set to True if the output of path must be understood as " + "SE3.") + .def( + "setRightHandSide", &Cartesian::setRightHandSide2, + "Set right hand side from a hpp:constraints::DifferentiableFunction\n" + "\n:param rhs function.\n:param timeRange interval of definition of " + "the function.") + .def("getRightHandSide", &Cartesian::getRightHandSide, + "Get right hand side function of trajectory constraint.") + .def("planPath", &Cartesian::planPath, + "Plan a path starting from an initial configuration.\n" + "\n" + ":param q_init:initial configuration\n" + ":return: (success, result) where\n" + " result is the resulting path in case of success, a valid " + "portion of path satisfying\n" + " the trajectory constraint along a sub-interval starting at 0 " + "otherwise.\n" + "\n" + "The successive steps of the computations are the following.\n" + " - The interval of definition is discretized into a number of " + "sub-intervals defined by\n" + " method \link Cartesian::nDiscreteSteps\n" + " nDiscreteSteps\endlink. For each discretized value, a " + "configuration is computed by\n" + " projecting the previous one onto the time-varying constraint " + "or the initial\n" + " configuration for the first discretized value.\n" + " - the path interpolating these configurations and associated to " + "the time-varying\n" + " constraint is tested for collision. If no collision is " + "detected, the function\n" + " returns true.\n" + "\n" + "In case of failure in the first step, the interpolated path until " + "the last successful\n" + "projection is returned without collision checking.\n" + "In case of failure in the second step, a collision-free path " + "defined over a sub-interval\n" + "starting at 0 and satisfying the constraints is returned."); + boost::python::def("makePiecewiseLinearTrajectory", + hpp::manipulation::steeringMethod::Cartesian:: + makePiecewiseLinearTrajectory, + "Build a piecewise linear path.\n" + "See C++ documentation of class " + "hpp::manipulation::steeringMethod::Cartesian."); +} +} // namespace steeringMethod +} // namespace manipulation +} // namespace pyhpp