def test_v_polytope(self): vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]]) vpoly = mut.VPolytope(vertices=vertices) self.assertEqual(vpoly.ambient_dimension(), 2) np.testing.assert_array_equal(vpoly.vertices(), vertices) self.assertTrue(vpoly.PointInSet(x=[1.0, 5.0], tol=1e-8)) vpoly.AddPointInSetConstraints(self.prog, self.x[0:2]) constraints = vpoly.AddPointInNonnegativeScalingConstraints( prog=self.prog, x=self.x[:2], t=self.t) self.assertGreaterEqual(len(constraints), 2) self.assertIsInstance(constraints[0], Binding[Constraint]) constraints = vpoly.AddPointInNonnegativeScalingConstraints( prog=self.prog, A=self.Ay[:2], b=self.by[:2], c=self.cz, d=self.dz, x=self.y, t=self.z) self.assertGreaterEqual(len(constraints), 2) self.assertIsInstance(constraints[0], Binding[Constraint]) assert_pickle(self, vpoly, lambda S: S.vertices()) v_box = mut.VPolytope.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1]) self.assertTrue(v_box.PointInSet([0, 0, 0])) self.assertAlmostEqual(v_box.CalcVolume(), 8, 1E-10) v_unit_box = mut.VPolytope.MakeUnitBox(dim=3) self.assertTrue(v_unit_box.PointInSet([0, 0, 0])) v_from_h = mut.VPolytope(H=mut.HPolyhedron.MakeUnitBox(dim=3)) self.assertTrue(v_from_h.PointInSet([0, 0, 0])) # Test creating a vpolytope from a non-minimal set of vertices # 2D: Random points inside a circle r = 2.0 n = 400 vertices = np.zeros((2, n + 4)) theta = np.linspace(0, 2 * np.pi, n, endpoint=False) vertices[0, 0:n] = r * np.cos(theta) vertices[1, 0:n] = r * np.sin(theta) vertices[:, n:] = np.array([[r / 2, r / 3, r / 4, r / 5], [r / 2, r / 3, r / 4, r / 5]]) vpoly = mut.VPolytope(vertices=vertices).GetMinimalRepresentation() self.assertAlmostEqual(vpoly.CalcVolume(), np.pi * r * r, delta=1e-3) self.assertEqual(vpoly.vertices().shape[1], n) # Calculate the length of the path that visits all the vertices # sequentially. # If the vertices are in clockwise/counter-clockwise order, # the length of the path will coincide with the perimeter of a # circle. self.assertAlmostEqual(self._calculate_path_length(vpoly.vertices()), 2 * np.pi * r, delta=1e-3) # 3D: Random points inside a box a = 2.0 vertices = np.array( [[0, a, 0, a, 0, a, 0, a, a / 2, a / 3, a / 4, a / 5], [0, 0, a, a, 0, 0, a, a, a / 2, a / 3, a / 4, a / 5], [0, 0, 0, 0, a, a, a, a, a / 2, a / 3, a / 4, a / 5]]) vpoly = mut.VPolytope(vertices=vertices).GetMinimalRepresentation() self.assertAlmostEqual(vpoly.CalcVolume(), a * a * a) self.assertEqual(vpoly.vertices().shape[1], 8)
def test_v_polytope(self): vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]]) vpoly = mut.VPolytope(vertices=vertices) self.assertEqual(vpoly.ambient_dimension(), 2) np.testing.assert_array_equal(vpoly.vertices(), vertices) self.assertTrue(vpoly.PointInSet(x=[1.0, 5.0], tol=1e-8)) vpoly.AddPointInSetConstraints(self.prog, self.x[0:2]) constraints = vpoly.AddPointInNonnegativeScalingConstraints( prog=self.prog, x=self.x[:2], t=self.t) self.assertGreaterEqual(len(constraints), 2) self.assertIsInstance(constraints[0], Binding[Constraint]) constraints = vpoly.AddPointInNonnegativeScalingConstraints( prog=self.prog, A=self.Ay[:2], b=self.by[:2], c=self.cz, d=self.dz, x=self.y, t=self.z) self.assertGreaterEqual(len(constraints), 2) self.assertIsInstance(constraints[0], Binding[Constraint]) assert_pickle(self, vpoly, lambda S: S.vertices()) v_box = mut.VPolytope.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1]) self.assertTrue(v_box.PointInSet([0, 0, 0])) self.assertAlmostEqual(v_box.CalcVolume(), 8, 1E-10) v_unit_box = mut.VPolytope.MakeUnitBox(dim=3) self.assertTrue(v_unit_box.PointInSet([0, 0, 0])) v_from_h = mut.VPolytope(H=mut.HPolyhedron.MakeUnitBox(dim=3)) self.assertTrue(v_from_h.PointInSet([0, 0, 0]))
def test_v_polytope(self): vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]]) vpoly = mut.VPolytope(vertices=vertices) self.assertEqual(vpoly.ambient_dimension(), 2) np.testing.assert_array_equal(vpoly.vertices(), vertices) self.assertTrue(vpoly.PointInSet(x=[1.0, 5.0], tol=1e-8)) vpoly.AddPointInSetConstraints(self.prog, self.x[0:2]) v_box = mut.VPolytope.MakeBox( lb=[-1, -1, -1], ub=[1, 1, 1]) self.assertTrue(v_box.PointInSet([0, 0, 0])) self.assertAlmostEqual(v_box.CalcVolume(), 8, 1E-10) v_unit_box = mut.VPolytope.MakeUnitBox(dim=3) self.assertTrue(v_unit_box.PointInSet([0, 0, 0])) v_from_h = mut.VPolytope(H=mut.HPolyhedron.MakeUnitBox(dim=3)) self.assertTrue(v_from_h.PointInSet([0, 0, 0]))
def _update_body_fill_verts(self, body_fill, patch_V): """ Takes a convex hull if necessary and uses in-place replacement of vertices to update the fill. """ # Take a convex hull to get an accurate shape for drawing, with verts # coming out in ccw order. if patch_V.shape[1] > 3: vpoly = optimization.VPolytope(patch_V) patch_V = vpoly.GetMinimalRepresentation().vertices() # Update the verts, padding out to the appropriate full # of verts by # replicating the final vertex. n_verts = body_fill.get_path().vertices.shape[0] patch_V = np.pad(patch_V, ((0, 0), (0, n_verts - patch_V.shape[1])), mode="edge") body_fill.get_path().vertices[:, :] = patch_V.T
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)
def _build_body_patches(self, use_random_colors, substitute_collocated_mesh_files, inspector): """ Generates body patches. self._patch_Blist stores a list of patches for each body (starting at body id 1). A body patch is a list of all 3D vertices of a piece of visual geometry. """ self._patch_Blist = {} self._patch_Blist_colors = {} for frame_id in inspector.GetAllFrameIds(): count = inspector.NumGeometriesForFrameWithRole( frame_id, Role.kIllustration) if count == 0: continue frame_name = self.frame_name(frame_id, inspector) this_body_patches = [] this_body_colors = [] for g_id in inspector.GetGeometries(frame_id, Role.kIllustration): X_BG = inspector.GetPoseInFrame(g_id) shape = inspector.GetShape(g_id) if isinstance(shape, Box): # Draw a bounding box. patch_G = np.vstack( (shape.width() / 2. * np.array([-1, -1, 1, 1, -1, -1, 1, 1]), shape.depth() / 2. * np.array([-1, 1, -1, 1, -1, 1, -1, 1]), shape.height() / 2. * np.array([-1, -1, -1, -1, 1, 1, 1, 1]))) elif isinstance(shape, Sphere): # Sphere is the only shape that allows a zero-measure. A # zero-radius sphere is a point, and we skip it. if shape.radius() == 0: continue lati, longi = np.meshgrid(np.arange(0., 2. * math.pi, 0.5), np.arange(0., 2. * math.pi, 0.5)) lati = lati.ravel() longi = longi.ravel() patch_G = np.vstack([ np.sin(lati) * np.cos(longi), np.sin(lati) * np.sin(longi), np.cos(lati) ]) patch_G *= shape.radius() elif isinstance(shape, Cylinder): radius = shape.radius() length = shape.length() # In the lcm geometry, cylinders are along +z # https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/matlab/systems/plants/RigidBodyCylinder.m # Two circles: one at bottom, one at top. sample_pts = np.arange(0., 2. * math.pi, 0.25) patch_G = np.hstack([ np.array([[ radius * math.cos(pt), radius * math.sin(pt), -length / 2. ], [ radius * math.cos(pt), radius * math.sin(pt), length / 2. ]]).T for pt in sample_pts ]) elif isinstance(shape, (Mesh, Convex)): filename = shape.filename() base, ext = os.path.splitext(filename) if (ext.lower() != ".obj" and substitute_collocated_mesh_files): # Check for a co-located .obj file (case insensitive). for f in glob.glob(base + '.*'): if f[-4:].lower() == '.obj': filename = f break if filename[-4:].lower() != '.obj': raise RuntimeError( f"The given file {filename} is not " f"supported and no alternate {base}" ".obj could be found.") if not os.path.exists(filename): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), filename) # Get mesh scaling. scale = shape.scale() mesh = ReadObjToTriangleSurfaceMesh(filename, scale) patch_G = np.vstack(mesh.vertices()) # Only store the vertices of the (3D) convex hull of the # mesh, as any interior vertices will still be interior # vertices after projection, and will therefore be removed # in _update_body_fill_verts(). vpoly = optimization.VPolytope(patch_G.T) patch_G = vpoly.GetMinimalRepresentation().vertices() elif isinstance(shape, HalfSpace): # For a half space, we'll simply create a large box with # the top face at z = 0, the bottom face at z = -1 and the # far corners at +/- 50 in the x- and y- directions. x = 50 y = 50 z = -1 patch_G = np.vstack( (x * np.array([-1, -1, 1, 1, -1, -1, 1, 1]), y * np.array([-1, 1, -1, 1, -1, 1, -1, 1]), z * np.array([-1, -1, -1, -1, 0, 0, 0, 0]))) # TODO(SeanCurtis-TRI): Provide support for capsule and # ellipsoid. else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( type(shape))) continue # Compute pose in body. patch_B = X_BG @ patch_G # Close path if not closed. if (patch_B[:, -1] != patch_B[:, 0]).any(): patch_B = np.hstack((patch_B, patch_B[:, 0][np.newaxis].T)) this_body_patches.append(patch_B) if not use_random_colors: # If we need to use random colors, we apply them after the # fact. See below. props = inspector.GetIllustrationProperties(g_id) assert props is not None rgba = props.GetPropertyOrDefault("phong", "diffuse", Rgba(0.9, 0.9, 0.9, 1.0)) color = np.array((rgba.r(), rgba.g(), rgba.b(), rgba.a())) this_body_colors.append(color) self._patch_Blist[frame_name] = this_body_patches self._patch_Blist_colors[frame_name] = this_body_colors # Spawn a random color generator. Each body will be given a unique # color when using this random generator, with each visual element of # the body colored the same. if use_random_colors: color = iter( plt_cm.rainbow(np.linspace(0, 1, len(self._patch_Blist_colors)))) for name in self._patch_Blist_colors.keys(): this_color = next(color) patch_count = len(self._patch_Blist[name]) self._patch_Blist_colors[name] = [this_color] * patch_count