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
