fix: handle case when ineq_cst is None

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_<NullEqConstraint, NullEqConstraint::Ptr, EqConstraintBase>(
+      cst_m, "NullEqConstraint")
+      .def(py::init<std::shared_ptr<kin::KinematicModel<double>>,
+                    const std::vector<std::string>&, BaseType>())
+      .def("update_kintree", &NullEqConstraint::update_kintree)
+      .def("evaluate", &NullEqConstraint::evaluate);
+
+  py::class_<NullIneqConstraint, NullIneqConstraint::Ptr, IneqConstraintBase>(
+      cst_m, "NullIneqConstraint")
+      .def(py::init<std::shared_ptr<kin::KinematicModel<double>>,
+                    const std::vector<std::string>&, BaseType>())
+      .def("update_kintree", &NullIneqConstraint::update_kintree)
+      .def("evaluate", &NullEqConstraint::evaluate)
+      .def("is_valid", &NullIneqConstraint::is_valid);
+
   py::class_<EqCompositeCst, EqCompositeCst::Ptr>(cst_m, "EqCompositeCst")
       .def(py::init<std::vector<EqConstraintBase::Ptr>>())
       .def("update_kintree", &EqCompositeCst::update_kintree)

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<NullEqConstraint>;
+  using EqConstraintBase::EqConstraintBase;
+  std::pair<Eigen::VectorXd, Eigen::MatrixXd> 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<NullIneqConstraint>;
+  using IneqConstraintBase::IneqConstraintBase;
+  std::pair<Eigen::VectorXd, Eigen::MatrixXd> 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

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: