diff --git a/bindings/pydrake/geometry/BUILD.bazel b/bindings/pydrake/geometry/BUILD.bazel index 144c6386e51c..e752f74ba27b 100644 --- a/bindings/pydrake/geometry/BUILD.bazel +++ b/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", diff --git a/bindings/pydrake/geometry/geometry_py_optimization.cc b/bindings/pydrake/geometry/geometry_py_optimization.cc index 4b85ad852750..ef49a7f70527 100644 --- a/bindings/pydrake/geometry/geometry_py_optimization.cc +++ b/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 +void DoSeparatingPlaneDeclaration(py::module m, T) { + constexpr auto& doc = pydrake_doc.drake.geometry.optimization; + py::tuple param = GetPyParam(); + using Class = geometry::optimization::CSpaceSeparatingPlane; + constexpr auto& base_cls_doc = doc.CSpaceSeparatingPlane; + { + auto cls = + DefineTemplateClassWithDefault( + 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(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_(m, "PlaneSide", doc.PlaneSide.doc) + .value("kPositive", PlaneSide::kPositive) + .value("kNegative", PlaneSide::kNegative); + + py::enum_( + 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_( + 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_( + m, "SeparatingPlaneOrder", doc.SeparatingPlaneOrder.doc) + .value("kAffine", SeparatingPlaneOrder::kAffine, + doc.SeparatingPlaneOrder.kAffine.doc); + type_visit([m](auto dummy) { DoSeparatingPlaneDeclaration(m, dummy); }, + type_pack()); + } + { + // Definitions for cpsace_free_structs.h/cc + constexpr auto& prog_doc = doc.SeparationCertificateProgramBase; + auto prog_cls = py::class_( + 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_( + 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_( + 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_ 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_( + 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_ cspace_free_polytope_cls( + m, "CspaceFreePolytope", cls_doc.doc); + cspace_free_polytope_cls + .def(py::init*, + const geometry::SceneGraph*, SeparatingPlaneOrder, + const Eigen::Ref&, + 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& C, + const Eigen::Ref& d, + const CspaceFreePolytope::IgnoredCollisionPairs& + ignored_collision_pairs, + const CspaceFreePolytope:: + FindSeparationCertificateGivenPolytopeOptions& options) { + std::unordered_map, + 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_(cspace_free_polytope_cls, + "SeparatingPlaneLagrangians", cls_doc.SeparatingPlaneLagrangians.doc) + .def(py::init(), 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_(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_(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_(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_(cspace_free_polytope_cls, + "FindSeparationCertificateGivenPolytopeOptions", + cls_doc.FindSeparationCertificateGivenPolytopeOptions.doc) + .def(py::init<>()) + .def_readwrite("ignore_redundant_C", + &Class::FindSeparationCertificateGivenPolytopeOptions:: + ignore_redundant_C); + + py::enum_(cspace_free_polytope_cls, + "EllipsoidMarginCost", cls_doc.EllipsoidMarginCost.doc) + .value("kSum", Class::EllipsoidMarginCost::kSum) + .value("kGeometricMean", Class::EllipsoidMarginCost::kGeometricMean); + + py::class_( + 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_( + 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_(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_(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) } diff --git a/bindings/pydrake/geometry/test/optimization_test.py b/bindings/pydrake/geometry/test/optimization_test.py index 0cf070d944d1..1df3ae7008cc 100644 --- a/bindings/pydrake/geometry/test/optimization_test.py +++ b/bindings/pydrake/geometry/test/optimization_test.py @@ -10,19 +10,21 @@ from pydrake.common.test_utilities.deprecation import catch_drake_warnings from pydrake.common.test_utilities.pickle_compare import assert_pickle from pydrake.geometry import ( - Box, Capsule, Cylinder, Ellipsoid, FramePoseVector, GeometryFrame, - GeometryInstance, ProximityProperties, SceneGraph, Sphere, + Box, Capsule, Cylinder, Convex, Ellipsoid, FramePoseVector, GeometryFrame, + GeometryInstance, ProximityProperties, SceneGraph, Sphere, GeometryId ) from pydrake.math import RigidTransform from pydrake.multibody.inverse_kinematics import InverseKinematics from pydrake.multibody.parsing import Parser from pydrake.multibody.plant import AddMultibodyPlantSceneGraph +from pydrake.multibody.tree import BodyIndex from pydrake.systems.framework import DiagramBuilder from pydrake.solvers import ( Binding, ClpSolver, Constraint, Cost, MathematicalProgram, - MathematicalProgramResult, SolverOptions, + MathematicalProgramResult, SolverOptions, CommonSolverOption, + ScsSolver, MosekSolver ) -from pydrake.symbolic import Variable +from pydrake.symbolic import Variable, Polynomial class TestGeometryOptimization(unittest.TestCase): @@ -679,3 +681,394 @@ def test_graph_of_convex_sets(self): with catch_drake_warnings(expected_count=1) as w: spp.RemoveVertex(target.id()) self.assertEqual(len(spp.Vertices()), 0) + + +class TestCspaceFreePolytope(unittest.TestCase): + def setUp(self): + unittest.TestCase.setUp(self) + limits_urdf = """ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + """ + + builder = DiagramBuilder() + self.plant, self.scene_graph = AddMultibodyPlantSceneGraph( + builder, 0.01) + Parser(self.plant).AddModelsFromString(limits_urdf, "urdf") + + self.plant.Finalize() + + diagram = builder.Build() + + # Tests the constructor + options = mut.CspaceFreePolytope.Options() + options.with_cross_y = False + self.cspace_free_polytope = mut.CspaceFreePolytope( + plant=self.plant, + scene_graph=self.scene_graph, + plane_order=mut.SeparatingPlaneOrder.kAffine, + q_star=np.zeros(self.plant.num_positions()), + options=options) + + def test_CspaceFreePolytope_SeparatingPlaneOrder(self): + # Access the separating plane orders. + possible_orders = [mut.SeparatingPlaneOrder.kAffine] + self.assertEqual(len(possible_orders), 1) + + def test_CspaceFreePolytope_Options(self): + dut = mut.CspaceFreePolytope + + solver_options = SolverOptions() + solver_options.SetOption(CommonSolverOption.kPrintToConsole, 1) + + # FindSeparationCertificateOptions + find_separation_options = mut.FindSeparationCertificateOptions() + find_separation_options.num_threads = 1 + find_separation_options.verbose = True + find_separation_options.solver_id = ScsSolver.id() + find_separation_options.terminate_at_failure = False + find_separation_options.solver_options = solver_options + + self.assertEqual(find_separation_options.num_threads, 1) + self.assertTrue(find_separation_options.verbose) + self.assertEqual(find_separation_options.solver_id, ScsSolver.id()) + self.assertFalse(find_separation_options.terminate_at_failure) + self.assertEqual( + find_separation_options.solver_options.common_solver_options()[ + CommonSolverOption.kPrintToConsole], 1) + + # FindSeparationCertificateGivenPolytopeOptions + lagrangian_options = \ + dut.FindSeparationCertificateGivenPolytopeOptions() + self.assertEqual( + lagrangian_options.num_threads, -1) + self.assertFalse( + lagrangian_options.verbose) + self.assertEqual( + lagrangian_options.solver_id, + MosekSolver.id()) + self.assertTrue( + lagrangian_options.terminate_at_failure) + self.assertIsNone( + lagrangian_options.solver_options) + self.assertFalse( + lagrangian_options.ignore_redundant_C) + num_threads = 1 + lagrangian_options.num_threads = num_threads + lagrangian_options.verbose = True + lagrangian_options.solver_id = ScsSolver.id() + lagrangian_options.terminate_at_failure = False + lagrangian_options.solver_options = solver_options + lagrangian_options.ignore_redundant_C = True + self.assertEqual( + lagrangian_options.num_threads, + num_threads) + self.assertTrue( + lagrangian_options.verbose) + self.assertEqual( + lagrangian_options.solver_id, + ScsSolver.id()) + self.assertFalse( + lagrangian_options.terminate_at_failure) + self.assertEqual( + lagrangian_options.solver_options.common_solver_options()[ + CommonSolverOption.kPrintToConsole], 1) + self.assertTrue( + lagrangian_options.ignore_redundant_C) + + # EllipsoidMarginCost + margin_cost = [dut.EllipsoidMarginCost.kGeometricMean, + dut.EllipsoidMarginCost.kSum] + + # FindPolytopeGivenLagrangianOptions + polytope_options = dut.FindPolytopeGivenLagrangianOptions() + self.assertIsNone(polytope_options.backoff_scale) + self.assertEqual( + polytope_options.ellipsoid_margin_epsilon, 1e-5) + self.assertEqual( + polytope_options.solver_id, + MosekSolver.id()) + self.assertIsNone(polytope_options.solver_options) + self.assertIsNone(polytope_options.s_inner_pts) + self.assertTrue( + polytope_options.search_s_bounds_lagrangians) + self.assertEqual( + polytope_options.ellipsoid_margin_cost, + dut.EllipsoidMarginCost.kGeometricMean) + polytope_options.backoff_scale = 1e-3 + polytope_options.ellipsoid_margin_epsilon = 1e-6 + polytope_options.solver_id = ScsSolver.id() + polytope_options.solver_options = solver_options + polytope_options.s_inner_pts = np.zeros((2, 1)) + polytope_options.search_s_bounds_lagrangians = False + polytope_options.ellipsoid_margin_cost = dut.EllipsoidMarginCost.kSum + self.assertEqual( + polytope_options.backoff_scale, 1e-3) + self.assertEqual( + polytope_options.ellipsoid_margin_epsilon, 1e-6) + self.assertEqual( + polytope_options.solver_id, + ScsSolver.id()) + self.assertEqual( + polytope_options.solver_options.common_solver_options()[ + CommonSolverOption.kPrintToConsole], 1) + np.testing.assert_array_almost_equal( + polytope_options.s_inner_pts, np.zeros( + (2, 1)), 1e-5) + self.assertFalse( + polytope_options.search_s_bounds_lagrangians) + self.assertEqual( + polytope_options.ellipsoid_margin_cost, + dut.EllipsoidMarginCost.kSum) + + # BilinearAlternationOptions + bilinear_alternation_options = dut.BilinearAlternationOptions() + self.assertEqual(bilinear_alternation_options.max_iter, 10) + self.assertAlmostEqual(bilinear_alternation_options.convergence_tol, + 1e-3) + self.assertAlmostEqual(bilinear_alternation_options.ellipsoid_scaling, + 0.99) + self.assertTrue(bilinear_alternation_options. + find_polytope_options.search_s_bounds_lagrangians) + self.assertFalse( + bilinear_alternation_options.find_lagrangian_options.verbose) + bilinear_alternation_options.max_iter = 4 + bilinear_alternation_options.convergence_tol = 1e-2 + bilinear_alternation_options.find_polytope_options = polytope_options + bilinear_alternation_options.find_lagrangian_options.verbose = \ + True + bilinear_alternation_options.ellipsoid_scaling = 0.5 + self.assertEqual(bilinear_alternation_options.max_iter, 4) + self.assertAlmostEqual(bilinear_alternation_options.convergence_tol, + 1e-2) + self.assertAlmostEqual(bilinear_alternation_options.ellipsoid_scaling, + 0.5) + self.assertFalse(bilinear_alternation_options. + find_polytope_options.search_s_bounds_lagrangians) + self.assertTrue(bilinear_alternation_options. + find_lagrangian_options.verbose) + + # BinarySearchOptions + binary_search_options = dut.BinarySearchOptions() + self.assertAlmostEqual(binary_search_options.scale_max, 1) + self.assertAlmostEqual(binary_search_options.scale_min, 0.01) + self.assertEqual(binary_search_options.max_iter, 10) + self.assertAlmostEqual( + binary_search_options.convergence_tol, 1e-3) + self.assertFalse( + binary_search_options.find_lagrangian_options.verbose) + + binary_search_options.scale_max = 2 + binary_search_options.scale_min = 1 + binary_search_options.max_iter = 2 + binary_search_options.convergence_tol = 1e-5 + binary_search_options.find_lagrangian_options.verbose = True + self.assertAlmostEqual(binary_search_options.scale_max, 2) + self.assertAlmostEqual(binary_search_options.scale_min, 1) + self.assertEqual(binary_search_options.max_iter, 2) + self.assertAlmostEqual( + binary_search_options.convergence_tol, 1e-5) + self.assertTrue( + binary_search_options.find_lagrangian_options.verbose) + + options = dut.Options() + self.assertFalse(options.with_cross_y) + options.with_cross_y = True + self.assertTrue(options.with_cross_y) + + def test_CspaceFreePolytope_getters_and_auxillary_structs(self): + dut = self.cspace_free_polytope + + # TODO(Alexandre.Amice): Insert the suggested test from #20025 once the + # issue is resolved. + + self.assertGreaterEqual( + len(dut.map_geometries_to_separating_planes().keys()), 1) + self.assertGreaterEqual( + len(dut.separating_planes()), 1) + self.assertEqual(len(dut.y_slack()), 3) + + # Test all bindings of objects in c_iris_collision_geometry.h/cc, + # cspace_separating_plane.h/cc, cspace_free_structs.h/cc. + # Many of these objects require an instantiation of + # CspaceFreePolytopeBase (either CspaceFreePolytope or CspaceFreeBox) + # to access which is why they are tested here. + + # Test CSpaceSeparatingPlane and CIrisCollisionGeometry + geom_type_possible_values = [ + mut.CIrisGeometryType.kPolytope, + mut.CIrisGeometryType.kSphere, + mut.CIrisGeometryType.kCylinder, + mut.CIrisGeometryType.kCapsule] + geom_shape_possible_values = [ + Capsule, Sphere, Cylinder, Box, Convex + ] + for plane_idx in dut.map_geometries_to_separating_planes().values(): + plane = dut.separating_planes()[plane_idx] + self.assertIsInstance(plane.a[0], Polynomial) + self.assertIsInstance(plane.b, Polynomial) + self.assertIsInstance(plane.expressed_body, BodyIndex) + self.assertEqual(plane.plane_degree, 1) + self.assertIsInstance(plane.decision_variables[0], Variable) + for geom in [plane.positive_side_geometry, + plane.negative_side_geometry]: + self.assertIn(geom.type(), geom_type_possible_values) + self.assertIn( + type( + geom.geometry()), + geom_shape_possible_values) + + self.assertIsInstance(geom.body_index(), BodyIndex) + self.assertGreater(geom.num_rationals(), 0) + self.assertIsInstance(geom.X_BG(), RigidTransform) + self.assertIsInstance(geom.id(), GeometryId) + # SeparationCertificateProgramBase, SeparationCertificateResultBase are + # not tested as they cannot be instantiated. They are bound only to + # provide subclassing to other bindings. The class + # FindSeparationCertificateOptions is tested in + # test_CspaceFreePolytope_Options along with all the other options. + + @unittest.skipUnless(MosekSolver().available(), "Requires Mosek") + def test_CspaceFreePolytopeMethods(self): + # These tests take very long using any solver besides Mosek so only run + # them if Mosek is available. + mosek_solver = MosekSolver() + C_init = np.vstack([np.atleast_2d(np.eye(self.plant.num_positions( + ))), -np.atleast_2d(np.eye(self.plant.num_positions()))]) + d_init = 1e-3 * np.ones((C_init.shape[0], 1)) + + lagrangian_options = \ + mut.CspaceFreePolytope.\ + FindSeparationCertificateGivenPolytopeOptions() + + binary_search_options = \ + mut.CspaceFreePolytope.BinarySearchOptions() + binary_search_options.scale_min = 1e-2 + binary_search_options.scale_max = 1e7 + binary_search_options.max_iter = 2 + + bilinear_alternation_options = \ + mut.CspaceFreePolytope.BilinearAlternationOptions() + bilinear_alternation_options.max_iter = 2 + + (success, certificates) = self.cspace_free_polytope.\ + FindSeparationCertificateGivenPolytope( + C=C_init, d=d_init, ignored_collision_pairs=set(), + options=lagrangian_options) + self.assertTrue(success) + geom_pair = list(self.cspace_free_polytope. + map_geometries_to_separating_planes().keys())[0] + self.assertIn(geom_pair, certificates.keys()) + self.assertIsInstance( + certificates[geom_pair], + mut.CspaceFreePolytope.SeparationCertificateResult) + + result = self.cspace_free_polytope.BinarySearch( + ignored_collision_pairs=set(), + C=C_init, + d=d_init, + s_center=np.zeros(self.plant.num_positions()), + options=binary_search_options + ) + + # Accesses all members of SearchResult. + self.assertGreaterEqual(result.num_iter(), 1) + self.assertGreaterEqual(len(result.C()), 1) + self.assertGreaterEqual(len(result.d()), 1) + self.assertIsInstance(result.certified_polytope(), mut.HPolyhedron) + self.assertEqual(len(result.a()), 1) + self.assertEqual(len(result.b()), 1) + self.assertIsInstance(result.a()[0][0], Polynomial) + + result = self.cspace_free_polytope.SearchWithBilinearAlternation( + ignored_collision_pairs=set(), + C_init=C_init, + d_init=d_init, + options=bilinear_alternation_options) + self.assertIsNotNone(result) + self.assertGreaterEqual(len(result), 1) + self.assertIsInstance(result[0], + mut.CspaceFreePolytope.SearchResult) + + # Make and Solve GeometrySeparableProgram. + pair = list(self.cspace_free_polytope. + map_geometries_to_separating_planes().keys())[0] + cert_prog = \ + self.cspace_free_polytope.MakeIsGeometrySeparableProgram( + geometry_pair=pair, C=C_init, d=d_init) + # Call all CspaceFreePolytope.SeparationCertificateProgram methods. + certificates = cert_prog.certificate + self.assertIsInstance(certificates, + mut.CspaceFreePolytope.SeparationCertificate) + self.assertIsInstance(cert_prog.prog(), MathematicalProgram) + self.assertGreaterEqual(cert_prog.plane_index, 0) + + self.assertIsInstance( + certificates.positive_side_rational_lagrangians[0], + mut.CspaceFreePolytope.SeparatingPlaneLagrangians) + self.assertIsInstance( + certificates.negative_side_rational_lagrangians[0], + mut.CspaceFreePolytope.SeparatingPlaneLagrangians) + + cert_prog_sol = \ + self.cspace_free_polytope.SolveSeparationCertificateProgram( + certificate_program=cert_prog, options=lagrangian_options) + + # Call all CspaceFreePolytope.SeparationCertificateResult + # methods. + self.assertEqual(cert_prog_sol.a.shape, (3,)) + self.assertIsInstance(cert_prog_sol.a[0], Polynomial) + self.assertIsInstance(cert_prog_sol.b, Polynomial) + self.assertIsInstance( + cert_prog_sol.plane_decision_var_vals[0], float) + self.assertIsInstance( + cert_prog_sol.result, MathematicalProgramResult) + + # Bindings for SeparatingPlaneLagrangians. + positive_side_lagrangians = \ + cert_prog_sol.positive_side_rational_lagrangians + positive_test_lagrangian = positive_side_lagrangians[0] + negative_side_lagrangians = \ + cert_prog_sol.negative_side_rational_lagrangians + negative_test_lagrangian = negative_side_lagrangians[0] + + positive_test_lagrangian.GetSolution(cert_prog_sol.result) + negative_test_lagrangian.GetSolution(cert_prog_sol.result) + + self.assertEqual(len(positive_test_lagrangian.polytope()), + C_init.shape[0]) + self.assertEqual(len(positive_test_lagrangian.s_lower()), 1) + self.assertEqual(len(positive_test_lagrangian.s_upper()), 1) + self.assertEqual(len(negative_test_lagrangian.polytope()), + C_init.shape[0]) + self.assertEqual(len(negative_test_lagrangian.s_lower()), 1) + self.assertEqual(len(negative_test_lagrangian.s_upper()), 1) diff --git a/geometry/optimization/cspace_free_polytope.h b/geometry/optimization/cspace_free_polytope.h index 5dce61759794..872afc0f9429 100644 --- a/geometry/optimization/cspace_free_polytope.h +++ b/geometry/optimization/cspace_free_polytope.h @@ -284,16 +284,19 @@ class CspaceFreePolytope : public CspaceFreePolytopeBase { return certified_polytope_; } + /** Each plane index is mapped to a vector of polynomials. */ [[nodiscard]] const std::unordered_map>& a() const { return a_; } + /** Each plane index is mapped to a polynomial, */ [[nodiscard]] const std::unordered_map& b() const { return b_; } + /** The number of iterations taken to search for the result. */ [[nodiscard]] int num_iter() const { return num_iter_; } private: diff --git a/geometry/optimization/cspace_free_structs.h b/geometry/optimization/cspace_free_structs.h index 1a089e320179..0e00f8bad71f 100644 --- a/geometry/optimization/cspace_free_structs.h +++ b/geometry/optimization/cspace_free_structs.h @@ -19,6 +19,8 @@ namespace optimization { plane. The conditions are that certain rational functions should be always positive. */ +// TODO(Alexandre.Amice) consider moving to internal since this is never +// accessed publically by cspace_free_polytope struct PlaneSeparatesGeometries { PlaneSeparatesGeometries( std::vector m_positive_side_rationals,