From 03d7c4d6fb574b5a5091cdfc4de704bd76bb6a15 Mon Sep 17 00:00:00 2001 From: HiroIshida Date: Fri, 27 Dec 2024 10:19:10 +0900 Subject: [PATCH 1/2] test(ompl): case when ineq_cst is None --- python/tests/test_ompl_solver.py | 33 ++++++++++++++++++++------------ 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/python/tests/test_ompl_solver.py b/python/tests/test_ompl_solver.py index 77c1b11..e4e68a3 100644 --- a/python/tests/test_ompl_solver.py +++ b/python/tests/test_ompl_solver.py @@ -11,21 +11,30 @@ from plainmp.utils import primitive_to_plainmp_sdf algos = (Algorithm.RRTConnect, Algorithm.KPIECE1) -test_conditions = [(True, algo, False) for algo in algos] + [(False, algo, False) for algo in algos] -test_conditions.append((True, Algorithm.KPIECE1, True)) -test_conditions.append((True, Algorithm.RRT, True)) +test_conditions = [(True, algo, False, False) for algo in algos] + [ + (False, algo, False, False) for algo in algos +] +test_conditions.append((True, Algorithm.KPIECE1, True, False)) +test_conditions.append((True, Algorithm.RRT, True, False)) +test_conditions.append( + (True, Algorithm.RRTConnect, False, True) +) # test without inequality constraint -@pytest.mark.parametrize("goal_is_pose,algo,use_goal_sampler", test_conditions) -def test_ompl_solver(goal_is_pose: bool, algo: Algorithm, use_goal_sampler: bool): +@pytest.mark.parametrize("goal_is_pose,algo,use_goal_sampler,no_ineq_cst", test_conditions) +def test_ompl_solver( + goal_is_pose: bool, algo: Algorithm, use_goal_sampler: bool, no_ineq_cst: bool +): fetch = FetchSpec() - cst = fetch.create_collision_const() - - table = Box([1.0, 2.0, 0.05], with_sdf=True) - table.translate([1.0, 0.0, 0.8]) - ground = Box([2.0, 2.0, 0.05], with_sdf=True) - sdf = UnionSDF([primitive_to_plainmp_sdf(table), primitive_to_plainmp_sdf(ground)]) - cst.set_sdf(sdf) + if no_ineq_cst: + cst = None + else: + cst = fetch.create_collision_const() + table = Box([1.0, 2.0, 0.05], with_sdf=True) + table.translate([1.0, 0.0, 0.8]) + ground = Box([2.0, 2.0, 0.05], with_sdf=True) + sdf = UnionSDF([primitive_to_plainmp_sdf(table), primitive_to_plainmp_sdf(ground)]) + cst.set_sdf(sdf) lb, ub = fetch.angle_bounds() start = np.array([0.0, 1.31999949, 1.40000015, -0.20000077, 1.71999929, 0.0, 1.6600001, 0.0]) if goal_is_pose: From 507d55ebf4ba638f25471eb520b79f53a6cd2624 Mon Sep 17 00:00:00 2001 From: HiroIshida Date: Fri, 27 Dec 2024 10:19:40 +0900 Subject: [PATCH 2/2] feat: add null ineq and eq constraints --- cpp/plainmp/bindings/constraint.cpp | 16 ++++++++++++++++ cpp/plainmp/constraints/primitive.hpp | 23 +++++++++++++++++++++++ 2 files changed, 39 insertions(+) diff --git a/cpp/plainmp/bindings/constraint.cpp b/cpp/plainmp/bindings/constraint.cpp index d687ead..88a7e91 100644 --- a/cpp/plainmp/bindings/constraint.cpp +++ b/cpp/plainmp/bindings/constraint.cpp @@ -100,6 +100,22 @@ void bind_constraint_submodule(py::module& m) { .def("update_kintree", &ComInPolytopeCst::update_kintree) .def("is_valid", &ComInPolytopeCst::is_valid) .def("evaluate", &ComInPolytopeCst::evaluate); + + py::class_( + cst_m, "NullEqConstraint") + .def(py::init>, + const std::vector&, BaseType>()) + .def("update_kintree", &NullEqConstraint::update_kintree) + .def("evaluate", &NullEqConstraint::evaluate); + + py::class_( + cst_m, "NullIneqConstraint") + .def(py::init>, + const std::vector&, BaseType>()) + .def("update_kintree", &NullIneqConstraint::update_kintree) + .def("evaluate", &NullEqConstraint::evaluate) + .def("is_valid", &NullIneqConstraint::is_valid); + py::class_(cst_m, "EqCompositeCst") .def(py::init>()) .def("update_kintree", &EqCompositeCst::update_kintree) diff --git a/cpp/plainmp/constraints/primitive.hpp b/cpp/plainmp/constraints/primitive.hpp index f2ec737..6b4a6e6 100644 --- a/cpp/plainmp/constraints/primitive.hpp +++ b/cpp/plainmp/constraints/primitive.hpp @@ -112,4 +112,27 @@ class IneqConstraintBase : public ConstraintBase { virtual bool is_valid_dirty() = 0; }; +class NullEqConstraint : public EqConstraintBase { + public: + using Ptr = std::shared_ptr; + using EqConstraintBase::EqConstraintBase; + std::pair evaluate_dirty() override { + return {Eigen::VectorXd::Zero(0), Eigen::MatrixXd::Zero(0, q_dim())}; + } + size_t cst_dim() const override { return 0; } + std::string get_name() const override { return "NullEqConstraint"; } +}; + +class NullIneqConstraint : public IneqConstraintBase { + public: + using Ptr = std::shared_ptr; + using IneqConstraintBase::IneqConstraintBase; + std::pair evaluate_dirty() override { + return {Eigen::VectorXd::Zero(0), Eigen::MatrixXd::Zero(0, q_dim())}; + } + size_t cst_dim() const override { return 0; } + std::string get_name() const override { return "NullIneqConstraint"; } + bool is_valid_dirty() override { return true; } +}; + }; // namespace plainmp::constraint