def test_hyper_ellipsoid(self): ellipsoid = mut.Hyperellipsoid(A=self.A, center=self.b) self.assertEqual(ellipsoid.ambient_dimension(), 3) np.testing.assert_array_equal(ellipsoid.A(), self.A) np.testing.assert_array_equal(ellipsoid.center(), self.b) self.assertTrue(ellipsoid.PointInSet(x=self.b, tol=0.0)) ellipsoid.AddPointInSetConstraints(self.prog, self.x) shape, pose = ellipsoid.ToShapeWithPose() self.assertIsInstance(shape, Ellipsoid) self.assertIsInstance(pose, RigidTransform) p = np.array([11.1, 12.2, 13.3]) point = mut.Point(p) scale, witness = ellipsoid.MinimumUniformScalingToTouch(point) self.assertTrue(scale > 0.0) np.testing.assert_array_almost_equal(witness, p) e_ball = mut.Hyperellipsoid.MakeAxisAligned( radius=[1, 1, 1], center=self.b) np.testing.assert_array_equal(e_ball.A(), self.A) np.testing.assert_array_equal(e_ball.center(), self.b) e_ball2 = mut.Hyperellipsoid.MakeHypersphere( radius=1, center=self.b) np.testing.assert_array_equal(e_ball2.A(), self.A) np.testing.assert_array_equal(e_ball2.center(), self.b) e_ball3 = mut.Hyperellipsoid.MakeUnitBall(dim=3) np.testing.assert_array_equal(e_ball3.A(), self.A) np.testing.assert_array_equal(e_ball3.center(), [0, 0, 0])
def test_point_convex_set(self): p = np.array([11.1, 12.2, 13.3]) point = mut.Point(p) self.assertEqual(point.ambient_dimension(), 3) np.testing.assert_array_equal(point.x(), p) point.set_x(x=2*p) np.testing.assert_array_equal(point.x(), 2*p) point.set_x(x=p)
def test_minkowski_sum(self): point = mut.Point(np.array([11.1, 12.2, 13.3])) hpoly = mut.HPolyhedron(A=self.A, b=self.b) sum = mut.MinkowskiSum(setA=point, setB=hpoly) self.assertEqual(sum.ambient_dimension(), 3) self.assertEqual(sum.num_terms(), 2) sum2 = mut.MinkowskiSum(sets=[point, hpoly]) self.assertEqual(sum2.ambient_dimension(), 3) self.assertEqual(sum2.num_terms(), 2) self.assertIsInstance(sum2.term(0), mut.Point)
def test_intersection(self): point = mut.Point(np.array([0.1, 0.2, 0.3])) h_box = mut.HPolyhedron.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1]) intersect = mut.Intersection(setA=point, setB=h_box) self.assertEqual(intersect.ambient_dimension(), 3) self.assertEqual(intersect.num_elements(), 2) intersect2 = mut.Intersection(sets=[point, h_box]) self.assertEqual(intersect2.ambient_dimension(), 3) self.assertEqual(intersect2.num_elements(), 2) self.assertIsInstance(intersect2.element(0), mut.Point)
def test_cartesian_product(self): point = mut.Point(np.array([11.1, 12.2, 13.3])) h_box = mut.HPolyhedron.MakeBox( lb=[-1, -1, -1], ub=[1, 1, 1]) sum = mut.CartesianProduct(setA=point, setB=h_box) self.assertEqual(sum.ambient_dimension(), 6) self.assertEqual(sum.num_factors(), 2) sum2 = mut.CartesianProduct(sets=[point, h_box]) self.assertEqual(sum2.ambient_dimension(), 6) self.assertEqual(sum2.num_factors(), 2) self.assertIsInstance(sum2.factor(0), mut.Point) sum2 = mut.CartesianProduct( sets=[point, h_box], A=np.eye(6, 3), b=[0, 1, 2, 3, 4, 5]) self.assertEqual(sum2.ambient_dimension(), 3) self.assertEqual(sum2.num_factors(), 2) self.assertIsInstance(sum2.factor(1), mut.HPolyhedron)
def test_hyper_ellipsoid(self): ellipsoid = mut.Hyperellipsoid(A=self.A, center=self.b) self.assertEqual(ellipsoid.ambient_dimension(), 3) np.testing.assert_array_equal(ellipsoid.A(), self.A) np.testing.assert_array_equal(ellipsoid.center(), self.b) self.assertTrue(ellipsoid.PointInSet(x=self.b, tol=0.0)) ellipsoid.AddPointInSetConstraints(self.prog, self.x) constraints = ellipsoid.AddPointInNonnegativeScalingConstraints( prog=self.prog, x=self.x, t=self.t) self.assertGreaterEqual(len(constraints), 2) self.assertIsInstance(constraints[0], Binding[Constraint]) constraints = ellipsoid.AddPointInNonnegativeScalingConstraints( prog=self.prog, A=self.Ay, b=self.by, c=self.cz, d=self.dz, x=self.y, t=self.z) self.assertGreaterEqual(len(constraints), 2) self.assertIsInstance(constraints[0], Binding[Constraint]) shape, pose = ellipsoid.ToShapeWithPose() self.assertIsInstance(shape, Ellipsoid) self.assertIsInstance(pose, RigidTransform) p = np.array([11.1, 12.2, 13.3]) point = mut.Point(p) scale, witness = ellipsoid.MinimumUniformScalingToTouch(point) self.assertTrue(scale > 0.0) np.testing.assert_array_almost_equal(witness, p) assert_pickle(self, ellipsoid, lambda S: np.vstack( (S.A(), S.center()))) e_ball = mut.Hyperellipsoid.MakeAxisAligned(radius=[1, 1, 1], center=self.b) np.testing.assert_array_equal(e_ball.A(), self.A) np.testing.assert_array_equal(e_ball.center(), self.b) e_ball2 = mut.Hyperellipsoid.MakeHypersphere(radius=1, center=self.b) np.testing.assert_array_equal(e_ball2.A(), self.A) np.testing.assert_array_equal(e_ball2.center(), self.b) e_ball3 = mut.Hyperellipsoid.MakeUnitBall(dim=3) np.testing.assert_array_equal(e_ball3.A(), self.A) np.testing.assert_array_equal(e_ball3.center(), [0, 0, 0])
def test_graph_of_convex_sets(self): spp = mut.GraphOfConvexSets() source = spp.AddVertex(set=mut.Point([0.1]), name="source") target = spp.AddVertex(set=mut.Point([0.2]), name="target") edge0 = spp.AddEdge(u=source, v=target, name="edge0") edge1 = spp.AddEdge(u_id=source.id(), v_id=target.id(), name="edge1") self.assertEqual(len(spp.Vertices()), 2) self.assertEqual(len(spp.Edges()), 2) result = spp.SolveShortestPath( source_id=source.id(), target_id=target.id(), convex_relaxation=True) self.assertIsInstance(result, MathematicalProgramResult) self.assertIsInstance(spp.SolveShortestPath( source=source, target=target, convex_relaxation=True), MathematicalProgramResult) self.assertIn("source", spp.GetGraphvizString( result=result, show_slacks=True, precision=2, scientific=False)) # Vertex self.assertIsInstance(source.id(), mut.GraphOfConvexSets.VertexId) self.assertEqual(source.ambient_dimension(), 1) self.assertEqual(source.name(), "source") self.assertIsInstance(source.x()[0], Variable) self.assertIsInstance(source.set(), mut.Point) np.testing.assert_array_almost_equal( source.GetSolution(result), [0.1], 1e-6) # Edge self.assertAlmostEqual(edge0.GetSolutionCost(result=result), 0.0, 1e-6) np.testing.assert_array_almost_equal( edge0.GetSolutionPhiXu(result=result), [0.1], 1e-6) np.testing.assert_array_almost_equal( edge0.GetSolutionPhiXv(result=result), [0.2], 1e-6) self.assertIsInstance(edge0.id(), mut.GraphOfConvexSets.EdgeId) self.assertEqual(edge0.name(), "edge0") self.assertEqual(edge0.u(), source) self.assertEqual(edge0.v(), target) self.assertIsInstance(edge0.phi(), Variable) self.assertIsInstance(edge0.xu()[0], Variable) self.assertIsInstance(edge0.xv()[0], Variable) var, binding = edge0.AddCost(e=1.0+edge0.xu()[0]) self.assertIsInstance(var, Variable) self.assertIsInstance(binding, Binding[Cost]) var, binding = edge0.AddCost(binding=binding) self.assertIsInstance(var, Variable) self.assertIsInstance(binding, Binding[Cost]) binding = edge0.AddConstraint(f=(edge0.xu()[0] == edge0.xv()[0])) self.assertIsInstance(binding, Binding[Constraint]) binding = edge0.AddConstraint(binding=binding) self.assertIsInstance(binding, Binding[Constraint]) edge0.AddPhiConstraint(phi_value=False) edge0.ClearPhiConstraints() # Remove Edges self.assertEqual(len(spp.Edges()), 2) spp.RemoveEdge(edge1.id()) self.assertEqual(len(spp.Edges()), 1) spp.RemoveEdge(edge0) self.assertEqual(len(spp.Edges()), 0) # Remove Vertices self.assertEqual(len(spp.Vertices()), 2) spp.RemoveVertex(source.id()) self.assertEqual(len(spp.Vertices()), 1) spp.RemoveVertex(target) self.assertEqual(len(spp.Vertices()), 0)
def test_make_from_scene_graph_and_iris(self): """ Tests the make from scene graph and iris functionality together as the Iris code makes obstacles from geometries registered in SceneGraph. """ scene_graph = SceneGraph() source_id = scene_graph.RegisterSource("source") frame_id = scene_graph.RegisterFrame( source_id=source_id, frame=GeometryFrame("frame")) box_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=GeometryInstance(X_PG=RigidTransform(), shape=Box(1., 1., 1.), name="box")) cylinder_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=GeometryInstance(X_PG=RigidTransform(), shape=Cylinder(1., 1.), name="cylinder")) sphere_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=GeometryInstance(X_PG=RigidTransform(), shape=Sphere(1.), name="sphere")) capsule_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=GeometryInstance(X_PG=RigidTransform(), shape=Capsule(1., 1.0), name="capsule")) context = scene_graph.CreateDefaultContext() pose_vector = FramePoseVector() pose_vector.set_value(frame_id, RigidTransform()) scene_graph.get_source_pose_port(source_id).FixValue( context, pose_vector) query_object = scene_graph.get_query_output_port().Eval(context) H = mut.HPolyhedron( query_object=query_object, geometry_id=box_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(H.ambient_dimension(), 3) C = mut.CartesianProduct( query_object=query_object, geometry_id=cylinder_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(C.ambient_dimension(), 3) E = mut.Hyperellipsoid( query_object=query_object, geometry_id=sphere_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(E.ambient_dimension(), 3) S = mut.MinkowskiSum( query_object=query_object, geometry_id=capsule_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(S.ambient_dimension(), 3) P = mut.Point( query_object=query_object, geometry_id=sphere_geometry_id, reference_frame=scene_graph.world_frame_id(), maximum_allowable_radius=1.5) self.assertEqual(P.ambient_dimension(), 3) V = mut.VPolytope( query_object=query_object, geometry_id=box_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(V.ambient_dimension(), 3) obstacles = mut.MakeIrisObstacles( query_object=query_object, reference_frame=scene_graph.world_frame_id()) options = mut.IrisOptions() options.require_sample_point_is_contained = True options.iteration_limit = 1 options.termination_threshold = 0.1 options.relative_termination_threshold = 0.01 self.assertNotIn("object at 0x", repr(options)) region = mut.Iris( obstacles=obstacles, sample=[2, 3.4, 5], domain=mut.HPolyhedron.MakeBox( lb=[-5, -5, -5], ub=[5, 5, 5]), options=options) self.assertIsInstance(region, mut.HPolyhedron) obstacles = [ mut.HPolyhedron.MakeUnitBox(3), mut.Hyperellipsoid.MakeUnitBall(3), mut.Point([0, 0, 0]), mut.VPolytope.MakeUnitBox(3)] region = mut.Iris( obstacles=obstacles, sample=[2, 3.4, 5], domain=mut.HPolyhedron.MakeBox( lb=[-5, -5, -5], ub=[5, 5, 5]), options=options) self.assertIsInstance(region, mut.HPolyhedron)