Cspace free polytope bindings non dev

bindings/pydrake/geometry/BUILD.bazel

@@ -34,11 +34,13 @@ drake_pybind_library(
     cc_deps = [
         ":optimization_pybind",
         "//bindings/pydrake:documentation_pybind",
+        "//bindings/pydrake:polynomial_types_pybind",
         "//bindings/pydrake/common:default_scalars_pybind",
         "//bindings/pydrake/common:deprecation_pybind",
         "//bindings/pydrake/common:identifier_pybind",
         "//bindings/pydrake/common:monostate_pybind",
         "//bindings/pydrake/common:serialize_pybind",
+        "//bindings/pydrake/common:sorted_pair_pybind",
         "//bindings/pydrake/common:type_pack",
         "//bindings/pydrake/common:type_safe_index_pybind",
         "//bindings/pydrake/common:value_pybind",

bindings/pydrake/geometry/geometry_py_optimization.cc

@@ -2,15 +2,25 @@
  drake::geometry::optimization namespace. They can be found in the
  pydrake.geometry.optimization module. */
 
+#include "drake/bindings/pydrake/common/cpp_template_pybind.h"
 #include "drake/bindings/pydrake/common/default_scalars_pybind.h"
 #include "drake/bindings/pydrake/common/deprecation_pybind.h"
 #include "drake/bindings/pydrake/common/identifier_pybind.h"
+#include "drake/bindings/pydrake/common/sorted_pair_pybind.h"
+#include "drake/bindings/pydrake/common/value_pybind.h"
 #include "drake/bindings/pydrake/documentation_pybind.h"
 #include "drake/bindings/pydrake/geometry/geometry_py.h"
 #include "drake/bindings/pydrake/geometry/optimization_pybind.h"
+#include "drake/bindings/pydrake/polynomial_types_pybind.h"
+#include "drake/bindings/pydrake/pydrake_pybind.h"
 #include "drake/common/yaml/yaml_io.h"
 #include "drake/geometry/optimization/affine_subspace.h"
+#include "drake/geometry/optimization/c_iris_collision_geometry.h"
 #include "drake/geometry/optimization/cartesian_product.h"
+#include "drake/geometry/optimization/cspace_free_polytope.h"
+#include "drake/geometry/optimization/cspace_free_polytope_base.h"
+#include "drake/geometry/optimization/cspace_free_structs.h"
+#include "drake/geometry/optimization/cspace_separating_plane.h"
 #include "drake/geometry/optimization/graph_of_convex_sets.h"
 #include "drake/geometry/optimization/hpolyhedron.h"
 #include "drake/geometry/optimization/hyperellipsoid.h"
@@ -24,6 +34,39 @@
 namespace drake {
 namespace pydrake {
 
+// CSpaceSeparatingPlane, CIrisSeparatingPlane
+template <typename T>
+void DoSeparatingPlaneDeclaration(py::module m, T) {
+  constexpr auto& doc = pydrake_doc.drake.geometry.optimization;
+  py::tuple param = GetPyParam<T>();
+  using Class = geometry::optimization::CSpaceSeparatingPlane<T>;
+  constexpr auto& base_cls_doc = doc.CSpaceSeparatingPlane;
+  {
+    auto cls =
+        DefineTemplateClassWithDefault<Class>(
+            m, "CSpaceSeparatingPlane", param, base_cls_doc.doc)
+            // Use py_rvp::copy here because numpy.ndarray with dtype=object
+            // arrays must be copied, and cannot be referenced.
+            .def_readonly("a", &Class::a, py_rvp::copy, base_cls_doc.a.doc)
+            .def_readonly("b", &Class::b, base_cls_doc.b.doc)
+            .def_readonly("positive_side_geometry",
+                &Class::positive_side_geometry,
+                base_cls_doc.positive_side_geometry.doc)
+            .def_readonly("negative_side_geometry",
+                &Class::negative_side_geometry,
+                base_cls_doc.negative_side_geometry.doc)
+            .def_readonly("expressed_body", &Class::expressed_body,
+                base_cls_doc.expressed_body.doc)
+            .def_readonly("plane_degree", &Class::plane_degree)
+            // Use py_rvp::copy here because numpy.ndarray with dtype=object
+            // arrays must be copied, and cannot be referenced.
+            .def_readonly("decision_variables", &Class::decision_variables,
+                py_rvp::copy, base_cls_doc.a.doc);
+    DefCopyAndDeepCopy(&cls);
+    AddValueInstantiation<Class>(m);
+  }
+}
+
 void DefineGeometryOptimization(py::module m) {
   // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace.
   using namespace drake;
@@ -715,7 +758,292 @@ void DefineGeometryOptimization(py::module m) {
             cls_doc.SolveShortestPath.doc_deprecated);
 #pragma GCC diagnostic pop
   }
+  {
+    // Definitions for c_iris_collision_geometry.h/cc
+    py::enum_<PlaneSide>(m, "PlaneSide", doc.PlaneSide.doc)
+        .value("kPositive", PlaneSide::kPositive)
+        .value("kNegative", PlaneSide::kNegative);
+
+    py::enum_<CIrisGeometryType>(
+        m, "CIrisGeometryType", doc.CIrisGeometryType.doc)
+        .value("kPolytope", CIrisGeometryType::kPolytope,
+            doc.CIrisGeometryType.kPolytope.doc)
+        .value("kSphere", CIrisGeometryType::kSphere,
+            doc.CIrisGeometryType.kSphere.doc)
+        .value("kCylinder", CIrisGeometryType::kCylinder,
+            doc.CIrisGeometryType.kCylinder.doc)
+        .value("kCapsule", CIrisGeometryType::kCapsule,
+            doc.CIrisGeometryType.kCapsule.doc);
+
+    py::class_<CIrisCollisionGeometry>(
+        m, "CIrisCollisionGeometry", doc.CIrisCollisionGeometry.doc)
+        .def("type", &CIrisCollisionGeometry::type,
+            doc.CIrisCollisionGeometry.type.doc)
+        .def("geometry", &CIrisCollisionGeometry::geometry,
+            py_rvp::reference_internal, doc.CIrisCollisionGeometry.geometry.doc)
+        .def("body_index", &CIrisCollisionGeometry::body_index,
+            doc.CIrisCollisionGeometry.body_index.doc)
+        .def("id", &CIrisCollisionGeometry::id,
+            doc.CIrisCollisionGeometry.id.doc)
+        .def("X_BG", &CIrisCollisionGeometry::X_BG,
+            doc.CIrisCollisionGeometry.X_BG.doc)
+        .def("num_rationals", &CIrisCollisionGeometry::num_rationals,
+            doc.CIrisCollisionGeometry.num_rationals.doc);
+  }
+  {
+    py::enum_<SeparatingPlaneOrder>(
+        m, "SeparatingPlaneOrder", doc.SeparatingPlaneOrder.doc)
+        .value("kAffine", SeparatingPlaneOrder::kAffine,
+            doc.SeparatingPlaneOrder.kAffine.doc);
+    type_visit([m](auto dummy) { DoSeparatingPlaneDeclaration(m, dummy); },
+        type_pack<double, symbolic::Variable>());
+  }
+  {
+    // Definitions for cpsace_free_structs.h/cc
+    constexpr auto& prog_doc = doc.SeparationCertificateProgramBase;
+    auto prog_cls = py::class_<SeparationCertificateProgramBase>(
+        m, "SeparationCertificateProgramBase", prog_doc.doc)
+                        .def(
+                            "prog",
+                            [](const SeparationCertificateProgramBase* self) {
+                              return self->prog.get();
+                            },
+                            py_rvp::reference_internal)
+                        .def_readonly("plane_index",
+                            &SeparationCertificateProgramBase::plane_index);
+
+    constexpr auto& result_doc = doc.SeparationCertificateResultBase;
+    auto result_cls =
+        py::class_<SeparationCertificateResultBase>(
+            m, "SeparationCertificateResultBase", result_doc.doc)
+            .def_readonly("a", &SeparationCertificateResultBase::a)
+            .def_readonly("b", &SeparationCertificateResultBase::b)
+            .def_readonly("plane_decision_var_vals",
+                &SeparationCertificateResultBase::plane_decision_var_vals)
+            .def_readonly("result", &SeparationCertificateResultBase::result);
+
+    constexpr auto& find_options_doc = doc.FindSeparationCertificateOptions;
+    auto find_options_cls =
+        py::class_<FindSeparationCertificateOptions>(
+            m, "FindSeparationCertificateOptions", find_options_doc.doc)
+            .def(py::init<>())
+            .def_readwrite(
+                "num_threads", &FindSeparationCertificateOptions::num_threads)
+            .def_readwrite(
+                "verbose", &FindSeparationCertificateOptions::verbose)
+            .def_readwrite(
+                "solver_id", &FindSeparationCertificateOptions::solver_id)
+            .def_readwrite("terminate_at_failure",
+                &FindSeparationCertificateOptions::terminate_at_failure)
+            .def_readwrite("solver_options",
+                &FindSeparationCertificateOptions::solver_options);
+  }
+  {
+    using BaseClass = CspaceFreePolytopeBase;
+    const auto& base_cls_doc = doc.CspaceFreePolytopeBase;
+    py::class_<BaseClass> cspace_free_polytope_base_cls(
+        m, "CspaceFreePolytopeBase", base_cls_doc.doc);
+    cspace_free_polytope_base_cls
+        // TODO(Alexandre.Amice): Bind rational_forward_kinematics to resolve
+        // #20025.
+        .def("map_geometries_to_separating_planes",
+            &BaseClass::map_geometries_to_separating_planes,
+            base_cls_doc.map_geometries_to_separating_planes.doc)
+        .def("separating_planes", &BaseClass::separating_planes,
+            base_cls_doc.separating_planes.doc)
+        .def("y_slack", &BaseClass::y_slack, base_cls_doc.y_slack.doc);
 
+    py::class_<BaseClass::Options>(
+        cspace_free_polytope_base_cls, "Options", base_cls_doc.Options.doc)
+        .def(py::init<>())
+        .def_readwrite("with_cross_y", &BaseClass::Options::with_cross_y);
+
+    using Class = CspaceFreePolytope;
+    const auto& cls_doc = doc.CspaceFreePolytope;
+    py::class_<Class, BaseClass> cspace_free_polytope_cls(
+        m, "CspaceFreePolytope", cls_doc.doc);
+    cspace_free_polytope_cls
+        .def(py::init<const multibody::MultibodyPlant<double>*,
+                 const geometry::SceneGraph<double>*, SeparatingPlaneOrder,
+                 const Eigen::Ref<const Eigen::VectorXd>&,
+                 const Class::Options&>(),
+            py::arg("plant"), py::arg("scene_graph"), py::arg("plane_order"),
+            py::arg("q_star"), py::arg("options") = Class::Options(),
+            // Keep alive, reference: `self` keeps `plant` alive.
+            py::keep_alive<1, 2>(),
+            // Keep alive, reference: `self` keeps `scene_graph` alive.
+            py::keep_alive<1, 3>(), cls_doc.ctor.doc)
+        .def(
+            "FindSeparationCertificateGivenPolytope",
+            [](const CspaceFreePolytope* self,
+                const Eigen::Ref<const Eigen::MatrixXd>& C,
+                const Eigen::Ref<const Eigen::VectorXd>& d,
+                const CspaceFreePolytope::IgnoredCollisionPairs&
+                    ignored_collision_pairs,
+                const CspaceFreePolytope::
+                    FindSeparationCertificateGivenPolytopeOptions& options) {
+              std::unordered_map<SortedPair<geometry::GeometryId>,
+                  CspaceFreePolytope::SeparationCertificateResult>
+                  certificates;
+              bool success = self->FindSeparationCertificateGivenPolytope(
+                  C, d, ignored_collision_pairs, options, &certificates);
+              return std::pair(success, certificates);
+            },
+            py::arg("C"), py::arg("d"), py::arg("ignored_collision_pairs"),
+            py::arg("options"))
+        .def("SearchWithBilinearAlternation",
+            &Class::SearchWithBilinearAlternation,
+            py::arg("ignored_collision_pairs"), py::arg("C_init"),
+            py::arg("d_init"), py::arg("options"),
+            cls_doc.SearchWithBilinearAlternation.doc)
+        .def("BinarySearch", &Class::BinarySearch,
+            py::arg("ignored_collision_pairs"), py::arg("C"), py::arg("d"),
+            py::arg("s_center"), py::arg("options"), cls_doc.BinarySearch.doc)
+        .def("MakeIsGeometrySeparableProgram",
+            &Class::MakeIsGeometrySeparableProgram, py::arg("geometry_pair"),
+            py::arg("C"), py::arg("d"),
+            cls_doc.MakeIsGeometrySeparableProgram.doc)
+        .def("SolveSeparationCertificateProgram",
+            &Class::SolveSeparationCertificateProgram,
+            py::arg("certificate_program"), py::arg("options"),
+            cls_doc.SolveSeparationCertificateProgram.doc);
+    py::class_<Class::SeparatingPlaneLagrangians>(cspace_free_polytope_cls,
+        "SeparatingPlaneLagrangians", cls_doc.SeparatingPlaneLagrangians.doc)
+        .def(py::init<int, int>(), py::arg("C_rows"), py::arg("s_size"),
+            cls_doc.SeparatingPlaneLagrangians.ctor.doc)
+        .def("GetSolution", &Class::SeparatingPlaneLagrangians::GetSolution,
+            py::arg("result"),
+            cls_doc.SeparatingPlaneLagrangians.GetSolution.doc)
+        // Use py_rvp::copy here because numpy.ndarray with dtype=object arrays
+        // must be copied, and cannot be referenced.
+        .def("polytope", &Class::SeparatingPlaneLagrangians::mutable_polytope,
+            py_rvp::copy)
+        .def("s_lower", &Class::SeparatingPlaneLagrangians::mutable_s_lower)
+        .def("s_upper", &Class::SeparatingPlaneLagrangians::mutable_s_upper);
+
+    using SepCertClass = Class::SeparationCertificateResult;
+    py::class_<SepCertClass>(cspace_free_polytope_cls,
+        "SeparationCertificateResult", cls_doc.SeparationCertificateResult.doc)
+        .def_readonly("plane_index", &SepCertClass::plane_index)
+        .def_readonly("positive_side_rational_lagrangians",
+            &Class::SeparationCertificateResult::
+                positive_side_rational_lagrangians,
+            cls_doc.SeparationCertificateResult
+                .positive_side_rational_lagrangians.doc)
+        .def_readonly("negative_side_rational_lagrangians",
+            &Class::SeparationCertificateResult::
+                negative_side_rational_lagrangians,
+            cls_doc.SeparationCertificateResult
+                .negative_side_rational_lagrangians.doc)
+        // Use py_rvp::copy here because numpy.ndarray with dtype=object
+        // arrays must be copied, and cannot be referenced.
+        .def_readonly("a", &SepCertClass::a, py_rvp::copy,
+            doc.SeparationCertificateResultBase.a.doc)
+        .def_readonly(
+            "b", &SepCertClass::b, doc.SeparationCertificateResultBase.b.doc)
+        .def_readonly("result", &SepCertClass::result)
+        // Use py_rvp::copy here because numpy.ndarray with dtype=object
+        // arrays must be copied, and cannot be referenced.
+        .def_readonly("plane_decision_var_vals",
+            &SepCertClass::plane_decision_var_vals, py_rvp::copy);
+
+    py::class_<Class::SeparationCertificate>(cspace_free_polytope_cls,
+        "SeparationCertificate", cls_doc.SeparationCertificate.doc)
+        .def("GetSolution", &Class::SeparationCertificate::GetSolution,
+            py::arg("plane_index"), py::arg("a"), py::arg("b"),
+            py::arg("plane_decision_vars"), py::arg("result"),
+            cls_doc.SeparationCertificate.GetSolution.doc)
+        .def_readwrite("positive_side_rational_lagrangians",
+            &Class::SeparationCertificate::positive_side_rational_lagrangians)
+        .def_readwrite("negative_side_rational_lagrangians",
+            &Class::SeparationCertificate::negative_side_rational_lagrangians);
+
+    py::class_<Class::SeparationCertificateProgram,
+        SeparationCertificateProgramBase>(cspace_free_polytope_cls,
+        "SeparationCertificateProgram",
+        cls_doc.SeparationCertificateProgram.doc)
+        .def(py::init<>())
+        .def_readonly(
+            "plane_index", &Class::SeparationCertificateProgram::plane_index)
+        .def_readonly(
+            "certificate", &Class::SeparationCertificateProgram::certificate);
+
+    py::class_<Class::FindSeparationCertificateGivenPolytopeOptions,
+        FindSeparationCertificateOptions>(cspace_free_polytope_cls,
+        "FindSeparationCertificateGivenPolytopeOptions",
+        cls_doc.FindSeparationCertificateGivenPolytopeOptions.doc)
+        .def(py::init<>())
+        .def_readwrite("ignore_redundant_C",
+            &Class::FindSeparationCertificateGivenPolytopeOptions::
+                ignore_redundant_C);
+
+    py::enum_<Class::EllipsoidMarginCost>(cspace_free_polytope_cls,
+        "EllipsoidMarginCost", cls_doc.EllipsoidMarginCost.doc)
+        .value("kSum", Class::EllipsoidMarginCost::kSum)
+        .value("kGeometricMean", Class::EllipsoidMarginCost::kGeometricMean);
+
+    py::class_<Class::FindPolytopeGivenLagrangianOptions>(
+        cspace_free_polytope_cls, "FindPolytopeGivenLagrangianOptions",
+        cls_doc.FindPolytopeGivenLagrangianOptions.doc)
+        .def(py::init<>())
+        .def_readwrite("backoff_scale",
+            &Class::FindPolytopeGivenLagrangianOptions::backoff_scale)
+        .def_readwrite("ellipsoid_margin_epsilon",
+            &Class::FindPolytopeGivenLagrangianOptions::
+                ellipsoid_margin_epsilon)
+        .def_readwrite(
+            "solver_id", &Class::FindPolytopeGivenLagrangianOptions::solver_id)
+        .def_readwrite("solver_options",
+            &Class::FindPolytopeGivenLagrangianOptions::solver_options)
+        .def_readwrite("s_inner_pts",
+            &Class::FindPolytopeGivenLagrangianOptions::s_inner_pts)
+        .def_readwrite("search_s_bounds_lagrangians",
+            &Class::FindPolytopeGivenLagrangianOptions::
+                search_s_bounds_lagrangians)
+        .def_readwrite("ellipsoid_margin_cost",
+            &Class::FindPolytopeGivenLagrangianOptions::ellipsoid_margin_cost);
+
+    py::class_<Class::SearchResult>(
+        cspace_free_polytope_cls, "SearchResult", cls_doc.SearchResult.doc)
+        .def(py::init<>())
+        .def("C", &Class::SearchResult::C)
+        .def("d", &Class::SearchResult::d)
+        // Use py_rvp::copy here because numpy.ndarray with dtype=object arrays
+        // must be copied, and cannot be referenced.
+        .def("a", &Class::SearchResult::a, py_rvp::copy)
+        .def("b", &Class::SearchResult::b)
+        .def("num_iter", &Class::SearchResult::num_iter)
+        .def("certified_polytope", &Class::SearchResult::certified_polytope);
+
+    py::class_<Class::BilinearAlternationOptions>(cspace_free_polytope_cls,
+        "BilinearAlternationOptions", cls_doc.BilinearAlternationOptions.doc)
+        .def(py::init<>())
+        .def_readwrite("max_iter", &Class::BilinearAlternationOptions::max_iter,
+            cls_doc.BilinearAlternationOptions.max_iter.doc)
+        .def_readwrite("convergence_tol",
+            &Class::BilinearAlternationOptions::convergence_tol,
+            cls_doc.BilinearAlternationOptions.convergence_tol.doc)
+        .def_readwrite("find_polytope_options",
+            &Class::BilinearAlternationOptions::find_polytope_options,
+            cls_doc.BilinearAlternationOptions.find_polytope_options.doc)
+        .def_readonly("find_lagrangian_options",
+            &Class::BilinearAlternationOptions::find_lagrangian_options,
+            cls_doc.BilinearAlternationOptions.find_lagrangian_options.doc)
+        .def_readwrite("ellipsoid_scaling",
+            &Class::BilinearAlternationOptions::ellipsoid_scaling,
+            cls_doc.BilinearAlternationOptions.ellipsoid_scaling.doc);
+
+    py::class_<Class::BinarySearchOptions>(cspace_free_polytope_cls,
+        "BinarySearchOptions", cls_doc.BinarySearchOptions.doc)
+        .def(py::init<>())
+        .def_readwrite("scale_max", &Class::BinarySearchOptions::scale_max)
+        .def_readwrite("scale_min", &Class::BinarySearchOptions::scale_min)
+        .def_readwrite("max_iter", &Class::BinarySearchOptions::max_iter)
+        .def_readwrite(
+            "convergence_tol", &Class::BinarySearchOptions::convergence_tol)
+        .def_readonly("find_lagrangian_options",
+            &Class::BinarySearchOptions::find_lagrangian_options);
+  }
   // NOLINTNEXTLINE(readability/fn_size)
 }