Skip to content

Commit

Permalink
rebase again
Browse files Browse the repository at this point in the history
  • Loading branch information
@AlexandreAmice
AlexandreAmice committed Jul 29, 2023
1 parent c1b6b70 commit 1d9427f
Show file tree
Hide file tree
Showing 4 changed files with 119 additions and 119 deletions.
1 change: 0 additions & 1 deletion bindings/pydrake/geometry/BUILD.bazel
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,7 +112,6 @@ drake_py_unittest(

drake_py_unittest(
name = "optimization_test",
timeout = "long",
deps = [
":geometry",
"//bindings/pydrake:math_py",
Expand Down
8 changes: 4 additions & 4 deletions bindings/pydrake/geometry/geometry_py_optimization.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -846,10 +846,10 @@ void DefineGeometryOptimization(py::module m) {
cspace_free_polytope_base_cls
// TODO(Alexandre.Amice): Figure out how to bind rational_forward_kin.
// The naive method returns an "Unable to convert to Python type" error.
// .def("rational_forward_kin",
// &BaseClass::rational_forward_kin,
// py_rvp::reference_internal,
// base_cls_doc.rational_forward_kin.doc)
// .def("rational_forward_kin",
// &BaseClass::rational_forward_kin,
// py_rvp::reference_internal,
// base_cls_doc.rational_forward_kin.doc)
.def("map_geometries_to_separating_planes",
&BaseClass::map_geometries_to_separating_planes,
base_cls_doc.map_geometries_to_separating_planes.doc)
Expand Down
227 changes: 114 additions & 113 deletions bindings/pydrake/geometry/test/optimization_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,13 +10,14 @@
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,
Expand Down Expand Up @@ -961,118 +962,118 @@ def test_CspaceFreePolytope_getters_and_auxillary_structs(self):
# 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()
if mosek_solver.available():
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)
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)
2 changes: 1 addition & 1 deletion geometry/optimization/cspace_free_structs.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ 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
// TODO(Alexandre.Amice) consider moving to internal since this is never
// accessed publically by cspace_free_polytope
struct PlaneSeparatesGeometries {
PlaneSeparatesGeometries(
Expand Down

0 comments on commit 1d9427f

Please sign in to comment.