def test_shapes(self):
RigidTransform = RigidTransform_[float]
sphere = mut.Sphere(radius=1.0)
self.assertEqual(sphere.radius(), 1.0)
cylinder = mut.Cylinder(radius=1.0, length=2.0)
self.assertEqual(cylinder.radius(), 1.0)
self.assertEqual(cylinder.length(), 2.0)
box = mut.Box(width=1.0, depth=2.0, height=3.0)
self.assertEqual(box.width(), 1.0)
self.assertEqual(box.depth(), 2.0)
self.assertEqual(box.height(), 3.0)
numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0]))
capsule = mut.Capsule(radius=1.0, length=2.0)
self.assertEqual(capsule.radius(), 1.0)
self.assertEqual(capsule.length(), 2.0)
ellipsoid = mut.Ellipsoid(a=1.0, b=2.0, c=3.0)
self.assertEqual(ellipsoid.a(), 1.0)
self.assertEqual(ellipsoid.b(), 2.0)
self.assertEqual(ellipsoid.c(), 3.0)
X_FH = mut.HalfSpace.MakePose(Hz_dir_F=[0, 1, 0], p_FB=[1, 1, 1])
self.assertIsInstance(X_FH, RigidTransform)
box_mesh_path = FindResourceOrThrow(
"drake/systems/sensors/test/models/meshes/box.obj")
mesh = mut.Mesh(absolute_filename=box_mesh_path, scale=1.0)
self.assertEqual(mesh.filename(), box_mesh_path)
self.assertEqual(mesh.scale(), 1.0)
convex = mut.Convex(absolute_filename=box_mesh_path, scale=1.0)
self.assertEqual(convex.filename(), box_mesh_path)
self.assertEqual(convex.scale(), 1.0)
def __init__(self, tf):
geom = PhysicsGeometryInfo()
rgba = np.random.uniform([0.85, 0.85, 0.85, 1.0],
[0.95, 0.95, 0.95, 1.0])
geom.register_geometry(
tf=drake_tf_to_torch_tf(RigidTransform(p=[0., 0., .01 / 2.])),
geometry=pydrake_geom.Box(0.2159, 0.2794, 0.01), # 8.5" x 11"
color=rgba)
super().__init__(tf=tf, physics_geometry_info=geom, observed=True)
def test_shapes(self):
sphere = mut.Sphere(radius=1.0)
self.assertEqual(sphere.radius(), 1.0)
cylinder = mut.Cylinder(radius=1.0, length=2.0)
self.assertEqual(cylinder.radius(), 1.0)
self.assertEqual(cylinder.length(), 2.0)
box = mut.Box(width=1.0, depth=2.0, height=3.0)
self.assertEqual(box.width(), 1.0)
self.assertEqual(box.depth(), 2.0)
self.assertEqual(box.height(), 3.0)
numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0]))
def __init__(self, tf):
geom = PhysicsGeometryInfo(fixed=True)
geom.register_geometry(
drake_tf_to_torch_tf(RigidTransform(p=[0.0, 0., -1.0])),
pydrake_geom.Box(20., 20., 2.)
)
super().__init__(
tf=tf,
physics_geometry_info=geom,
observed=True
)
def __init__(self, tf):
geom = PhysicsGeometryInfo()
geom.register_geometry(
tf=drake_tf_to_torch_tf(RigidTransform(p=[0., 0., -0.05])),
geometry=pydrake_geom.Box(0.1, 0.1, 0.1),
color=np.array([0.2, 0.5, 0.8, 1.0])
)
super().__init__(
tf=tf,
physics_geometry_info=geom,
observed=True
)
def __init__(self, tf):
geom = PhysicsGeometryInfo()
geom.register_geometry(
tf=drake_tf_to_torch_tf(RigidTransform(p=[0., 0., -0.5])),
geometry=pydrake_geom.Box(self.desk_size[0], self.desk_size[1],
1.0),
color=np.array([0.3, 0.2, 0.2, 1.0]))
super().__init__(rule_probs=RepeatingSetNode.get_geometric_rule_probs(
p=0.2, max_children=6),
tf=tf,
physics_geometry_info=geom,
observed=True)
def test_shape_constructors(self):
box_mesh_path = FindResourceOrThrow(
"drake/systems/sensors/test/models/meshes/box.obj")
shapes = [
mut.Sphere(radius=1.0),
mut.Cylinder(radius=1.0, length=2.0),
mut.Box(width=1.0, depth=2.0, height=3.0),
mut.HalfSpace(),
mut.Mesh(absolute_filename=box_mesh_path, scale=1.0),
mut.Convex(absolute_filename=box_mesh_path, scale=1.0)
]
for shape in shapes:
self.assertIsInstance(shape, mut.Shape)
def __init__(self, tf):
geom = PhysicsGeometryInfo()
geom.register_geometry(tf=drake_tf_to_torch_tf(
RigidTransform(
p=[self.desk_size[0] / 2., self.desk_size[1] / 2., -0.5])),
geometry=pydrake_geom.Box(
self.desk_size[0], self.desk_size[1], 1.0),
color=np.array([0.3, 0.2, 0.2, 1.0]))
super().__init__(tf=tf,
p=0.2,
max_children=6,
physics_geometry_info=geom,
observed=True)
def test_shapes(self):
sphere = mut.Sphere(radius=1.0)
self.assertEqual(sphere.radius(), 1.0)
cylinder = mut.Cylinder(radius=1.0, length=2.0)
self.assertEqual(cylinder.radius(), 1.0)
self.assertEqual(cylinder.length(), 2.0)
box = mut.Box(width=1.0, depth=2.0, height=3.0)
self.assertEqual(box.width(), 1.0)
self.assertEqual(box.depth(), 2.0)
self.assertEqual(box.height(), 3.0)
numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0]))
# Test for existence of deprecated accessors.
with catch_drake_warnings(expected_count=3):
cylinder.get_radius()
cylinder.get_length()
sphere.get_radius()
def test_shape_constructors(self):
shapes = [
mut.Sphere(radius=1.0),
mut.Cylinder(radius=1.0, length=2.0),
mut.Box(width=1.0, depth=2.0, height=3.0),
mut.Capsule(radius=1.0, length=2.0),
mut.Ellipsoid(a=1.0, b=2.0, c=3.0),
mut.HalfSpace(),
mut.Mesh(absolute_filename="arbitrary/path", scale=1.0),
mut.Convex(absolute_filename="arbitrary/path", scale=1.0),
mut.MeshcatCone(height=1.23, a=3.45, b=6.78)
]
for shape in shapes:
self.assertIsInstance(shape, mut.Shape)
shape_cls = type(shape)
shape_copy = shape.Clone()
self.assertIsInstance(shape_copy, shape_cls)
self.assertIsNot(shape, shape_copy)
def test_shapes(self):
sphere = mut.Sphere(radius=1.0)
self.assertEqual(sphere.radius(), 1.0)
cylinder = mut.Cylinder(radius=1.0, length=2.0)
self.assertEqual(cylinder.radius(), 1.0)
self.assertEqual(cylinder.length(), 2.0)
box = mut.Box(width=1.0, depth=2.0, height=3.0)
self.assertEqual(box.width(), 1.0)
self.assertEqual(box.depth(), 2.0)
self.assertEqual(box.height(), 3.0)
numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0]))
box_mesh_path = FindResourceOrThrow(
"drake/systems/sensors/test/models/meshes/box.obj")
mesh = mut.Mesh(absolute_filename=box_mesh_path, scale=1.0)
self.assertEqual(mesh.filename(), box_mesh_path)
self.assertEqual(mesh.scale(), 1.0)
convex = mut.Convex(absolute_filename=box_mesh_path, scale=1.0)
self.assertEqual(convex.filename(), box_mesh_path)
self.assertEqual(convex.scale(), 1.0)
def test_optimization(self):
"""Tests geometry::optimization bindings"""
A = np.eye(3)
b = [1.0, 1.0, 1.0]
prog = MathematicalProgram()
x = prog.NewContinuousVariables(3, "x")
t = prog.NewContinuousVariables(1, "t")
# Test Point.
p = np.array([11.1, 12.2, 13.3])
point = mut.optimization.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)
# Test HPolyhedron.
hpoly = mut.optimization.HPolyhedron(A=A, b=b)
self.assertEqual(hpoly.ambient_dimension(), 3)
np.testing.assert_array_equal(hpoly.A(), A)
np.testing.assert_array_equal(hpoly.b(), b)
self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0))
hpoly.AddPointInSetConstraints(prog, x)
with self.assertRaisesRegex(
RuntimeError, ".*not implemented yet for HPolyhedron.*"):
hpoly.ToShapeWithPose()
h_box = mut.optimization.HPolyhedron.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
h_unit_box = mut.optimization.HPolyhedron.MakeUnitBox(dim=3)
np.testing.assert_array_equal(h_box.A(), h_unit_box.A())
np.testing.assert_array_equal(h_box.b(), h_unit_box.b())
self.assertIsInstance(
h_box.MaximumVolumeInscribedEllipsoid(),
mut.optimization.Hyperellipsoid)
np.testing.assert_array_almost_equal(
h_box.ChebyshevCenter(), [0, 0, 0])
# Test Hyperellipsoid.
ellipsoid = mut.optimization.Hyperellipsoid(A=A, center=b)
self.assertEqual(ellipsoid.ambient_dimension(), 3)
np.testing.assert_array_equal(ellipsoid.A(), A)
np.testing.assert_array_equal(ellipsoid.center(), b)
self.assertTrue(ellipsoid.PointInSet(x=b, tol=0.0))
ellipsoid.AddPointInSetConstraints(prog, x)
shape, pose = ellipsoid.ToShapeWithPose()
self.assertIsInstance(shape, mut.Ellipsoid)
self.assertIsInstance(pose, RigidTransform)
scale, witness = ellipsoid.MinimumUniformScalingToTouch(point)
self.assertTrue(scale > 0.0)
np.testing.assert_array_almost_equal(witness, p)
e_ball = mut.optimization.Hyperellipsoid.MakeAxisAligned(
radius=[1, 1, 1], center=b)
np.testing.assert_array_equal(e_ball.A(), A)
np.testing.assert_array_equal(e_ball.center(), b)
e_ball2 = mut.optimization.Hyperellipsoid.MakeHypersphere(
radius=1, center=b)
np.testing.assert_array_equal(e_ball2.A(), A)
np.testing.assert_array_equal(e_ball2.center(), b)
e_ball3 = mut.optimization.Hyperellipsoid.MakeUnitBall(dim=3)
np.testing.assert_array_equal(e_ball3.A(), A)
np.testing.assert_array_equal(e_ball3.center(), [0, 0, 0])
# Test VPolytope.
vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]])
vpoly = mut.optimization.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(prog, x[0:2])
v_box = mut.optimization.VPolytope.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(v_box.PointInSet([0, 0, 0]))
v_unit_box = mut.optimization.VPolytope.MakeUnitBox(dim=3)
self.assertTrue(v_unit_box.PointInSet([0, 0, 0]))
# Test remaining ConvexSet methods using these instances.
self.assertIsInstance(hpoly.Clone(), mut.optimization.HPolyhedron)
self.assertTrue(ellipsoid.IsBounded())
hpoly.AddPointInNonnegativeScalingConstraints(prog=prog, x=x, t=t[0])
# Test MakeFromSceneGraph methods.
scene_graph = mut.SceneGraph()
source_id = scene_graph.RegisterSource("source")
frame_id = scene_graph.RegisterFrame(
source_id=source_id, frame=mut.GeometryFrame("frame"))
box_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=mut.GeometryInstance(X_PG=RigidTransform(),
shape=mut.Box(1., 1., 1.),
name="sphere"))
sphere_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=mut.GeometryInstance(X_PG=RigidTransform(),
shape=mut.Sphere(1.), name="sphere"))
context = scene_graph.CreateDefaultContext()
pose_vector = mut.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.optimization.HPolyhedron(
query_object=query_object, geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(H.ambient_dimension(), 3)
E = mut.optimization.Hyperellipsoid(
query_object=query_object, geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(E.ambient_dimension(), 3)
P = mut.optimization.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.optimization.VPolytope(
query_object=query_object, geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(V.ambient_dimension(), 3)
# Test Iris.
obstacles = mut.optimization.MakeIrisObstacles(
query_object=query_object,
reference_frame=scene_graph.world_frame_id())
options = mut.optimization.IrisOptions()
options.require_sample_point_is_contained = True
options.iteration_limit = 1
options.termination_threshold = 0.1
region = mut.optimization.Iris(
obstacles=obstacles, sample=[2, 3.4, 5],
domain=mut.optimization.HPolyhedron.MakeBox(
lb=[-5, -5, -5], ub=[5, 5, 5]), options=options)
self.assertIsInstance(region, mut.optimization.HPolyhedron)
obstacles = [
mut.optimization.HPolyhedron.MakeUnitBox(3),
mut.optimization.Hyperellipsoid.MakeUnitBall(3),
mut.optimization.Point([0, 0, 0]),
mut.optimization.VPolytope.MakeUnitBox(3)]
region = mut.optimization.Iris(
obstacles=obstacles, sample=[2, 3.4, 5],
domain=mut.optimization.HPolyhedron.MakeBox(
lb=[-5, -5, -5], ub=[5, 5, 5]), options=options)
self.assertIsInstance(region, mut.optimization.HPolyhedron)
def test_meshcat(self):
port = 7051
params = mut.MeshcatParams(host="*",
port=port,
web_url_pattern="http://host:{port}")
meshcat = mut.Meshcat(params=params)
self.assertEqual(meshcat.port(), port)
with self.assertRaises(RuntimeError):
meshcat2 = mut.Meshcat(port=port)
self.assertIn("http", meshcat.web_url())
self.assertIn("ws", meshcat.ws_url())
meshcat.SetObject(path="/test/box",
shape=mut.Box(1, 1, 1),
rgba=mut.Rgba(.5, .5, .5))
meshcat.SetTransform(path="/test/box", X_ParentPath=RigidTransform())
meshcat.SetTransform(path="/test/box", matrix=np.eye(4))
self.assertTrue(meshcat.HasPath("/test/box"))
cloud = PointCloud(4)
cloud.mutable_xyzs()[:] = np.zeros((3, 4))
meshcat.SetObject(path="/test/cloud",
cloud=cloud,
point_size=0.01,
rgba=mut.Rgba(.5, .5, .5))
mesh = mut.TriangleSurfaceMesh(triangles=[
mut.SurfaceTriangle(0, 1, 2),
mut.SurfaceTriangle(3, 0, 2)
],
vertices=[[0, 0, 0], [1, 0, 0],
[1, 0, 1], [0, 0, 1]])
meshcat.SetObject(path="/test/triangle_surface_mesh",
mesh=mesh,
rgba=mut.Rgba(0.3, 0.3, 0.3),
wireframe=True,
wireframe_line_width=2.0)
meshcat.SetLine(path="/test/line",
vertices=np.eye(3),
line_width=2.0,
rgba=mut.Rgba(.3, .3, .3))
meshcat.SetLineSegments(path="/test/line_segments",
start=np.eye(3),
end=2 * np.eye(3),
line_width=2.0,
rgba=mut.Rgba(.3, .3, .3))
meshcat.SetTriangleMesh(path="/test/triangle_mesh",
vertices=np.array([[0, 0, 0], [1, 0, 0],
[1, 0, 1], [0, 0, 1]]).T,
faces=np.array([[0, 1, 2], [3, 0, 2]]).T,
rgba=mut.Rgba(0.3, 0.3, 0.3),
wireframe=True,
wireframe_line_width=2.0)
meshcat.SetProperty(path="/Background", property="visible", value=True)
meshcat.SetProperty(path="/Lights/DirectionalLight/<object>",
property="intensity",
value=1.0)
meshcat.SetProperty(path="/Background",
property="top_color",
value=[0, 0, 0])
meshcat.Set2dRenderMode(X_WC=RigidTransform(),
xmin=-1,
xmax=1,
ymin=-1,
ymax=1)
meshcat.ResetRenderMode()
meshcat.AddButton(name="button")
self.assertEqual(meshcat.GetButtonClicks(name="button"), 0)
meshcat.DeleteButton(name="button")
meshcat.AddSlider(name="slider", min=0, max=1, step=0.01, value=0.5)
meshcat.SetSliderValue(name="slider", value=0.7)
self.assertAlmostEqual(meshcat.GetSliderValue(name="slider"),
0.7,
delta=1e-14)
meshcat.DeleteSlider(name="slider")
meshcat.DeleteAddedControls()
self.assertIn("data:application/octet-binary;base64",
meshcat.StaticHtml())
meshcat.Flush()
# PerspectiveCamera
camera = mut.Meshcat.PerspectiveCamera(fov=80,
aspect=1.2,
near=0.2,
far=200,
zoom=1.3)
self.assertEqual(camera.fov, 80)
self.assertEqual(camera.aspect, 1.2)
self.assertEqual(camera.near, 0.2)
self.assertEqual(camera.far, 200)
self.assertEqual(camera.zoom, 1.3)
self.assertEqual(
repr(camera), "".join([
r"PerspectiveCamera(", r"fov=80.0, "
r"aspect=1.2, ", r"near=0.2, ", r"far=200.0, ", r"zoom=1.3)"
]))
meshcat.SetCamera(camera=camera, path="mypath")
# OrthographicCamera
camera = mut.Meshcat.OrthographicCamera(left=0.1,
right=1.3,
top=0.3,
bottom=1.4,
near=0.2,
far=200,
zoom=1.3)
self.assertEqual(camera.left, 0.1)
self.assertEqual(camera.right, 1.3)
self.assertEqual(camera.top, 0.3)
self.assertEqual(camera.bottom, 1.4)
self.assertEqual(camera.near, 0.2)
self.assertEqual(camera.far, 200)
self.assertEqual(camera.zoom, 1.3)
self.assertEqual(
repr(camera), "".join([
r"OrthographicCamera(", r"left=0.1, "
r"right=1.3, ", r"top=0.3, "
r"bottom=1.4, ", r"near=0.2, ", r"far=200.0, ", r"zoom=1.3)"
]))
meshcat.SetCamera(camera=camera, path="mypath")
def build_mbp(seed=0, verts_geom=False, convex_collision_geom=True):
# Make some random lumpy objects
trimeshes = []
np.random.seed(42)
for k in range(3):
# Make a small random number of triangles and chull it
# to get a lumpy object
mesh = trimesh.creation.random_soup(5)
mesh = trimesh.convex.convex_hull(mesh)
trimeshes.append(mesh)
# Create Drake geometry from those objects by adding a small
# sphere at each vertex
sphere_rad = 0.05
cmap = plt.cm.get_cmap('jet')
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.001))
# Add ground
friction = CoulombFriction(0.9, 0.8)
g = pydrake_geom.Box(100., 100., 0.5)
tf = RigidTransform(p=[0., 0., -0.25])
mbp.RegisterVisualGeometry(body=mbp.world_body(),
X_BG=tf,
shape=g,
name="ground",
diffuse_color=[1.0, 1.0, 1.0, 1.0])
mbp.RegisterCollisionGeometry(body=mbp.world_body(),
X_BG=tf,
shape=g,
name="ground",
coulomb_friction=friction)
for i, mesh in enumerate(trimeshes):
inertia = SpatialInertia(mass=1.0,
p_PScm_E=np.zeros(3),
G_SP_E=UnitInertia(0.01, 0.01, 0.01))
body = mbp.AddRigidBody(name="body_%d" % i, M_BBo_B=inertia)
color = cmap(np.random.random())
if verts_geom:
for j, vert in enumerate(mesh.vertices):
g = pydrake_geom.Sphere(radius=sphere_rad)
tf = RigidTransform(p=vert)
mbp.RegisterVisualGeometry(body=body,
X_BG=tf,
shape=g,
name="body_%d_color_%d" % (i, j),
diffuse_color=color)
mbp.RegisterCollisionGeometry(body=body,
X_BG=tf,
shape=g,
name="body_%d_collision_%d" %
(i, j),
coulomb_friction=friction)
# And add mesh itself for vis
path = "/tmp/part_%d.obj" % i
trimesh.exchange.export.export_mesh(mesh, path)
g = pydrake_geom.Convex(path)
mbp.RegisterVisualGeometry(body=body,
X_BG=RigidTransform(),
shape=g,
name="body_%d_base" % i,
diffuse_color=color)
if convex_collision_geom:
mbp.RegisterCollisionGeometry(body=body,
X_BG=RigidTransform(),
shape=g,
name="body_%d_base_col" % i,
coulomb_friction=friction)
mbp.SetDefaultFreeBodyPose(body, RigidTransform(p=[i % 3, i / 3., 1.]))
mbp.Finalize()
return builder, mbp, scene_graph
def test_meshcat(self):
meshcat = mut.Meshcat(port=7051)
self.assertEqual(meshcat.port(), 7051)
with self.assertRaises(RuntimeError):
meshcat2 = mut.Meshcat(port=7051)
self.assertIn("http", meshcat.web_url())
self.assertIn("ws", meshcat.ws_url())
meshcat.SetObject(path="/test/box",
shape=mut.Box(1, 1, 1),
rgba=mut.Rgba(.5, .5, .5))
meshcat.SetTransform(path="/test/box", X_ParentPath=RigidTransform())
meshcat.SetTransform(path="/test/box", matrix=np.eye(4))
self.assertTrue(meshcat.HasPath("/test/box"))
cloud = PointCloud(4)
cloud.mutable_xyzs()[:] = np.zeros((3, 4))
meshcat.SetObject(path="/test/cloud",
cloud=cloud,
point_size=0.01,
rgba=mut.Rgba(.5, .5, .5))
mesh = mut.TriangleSurfaceMesh(triangles=[
mut.SurfaceTriangle(0, 1, 2),
mut.SurfaceTriangle(3, 0, 2)
],
vertices=[[0, 0, 0], [1, 0, 0],
[1, 0, 1], [0, 0, 1]])
meshcat.SetObject(path="/test/triangle_surface_mesh",
mesh=mesh,
rgba=mut.Rgba(0.3, 0.3, 0.3),
wireframe=True,
wireframe_line_width=2.0)
meshcat.SetLine(path="/test/line",
vertices=np.eye(3),
line_width=2.0,
rgba=mut.Rgba(.3, .3, .3))
meshcat.SetLineSegments(path="/test/line_segments",
start=np.eye(3),
end=2 * np.eye(3),
line_width=2.0,
rgba=mut.Rgba(.3, .3, .3))
meshcat.SetTriangleMesh(path="/test/triangle_mesh",
vertices=np.array([[0, 0, 0], [1, 0, 0],
[1, 0, 1], [0, 0, 1]]).T,
faces=np.array([[0, 1, 2], [3, 0, 2]]).T,
rgba=mut.Rgba(0.3, 0.3, 0.3),
wireframe=True,
wireframe_line_width=2.0)
meshcat.SetProperty(path="/Background", property="visible", value=True)
meshcat.SetProperty(path="/Lights/DirectionalLight/<object>",
property="intensity",
value=1.0)
meshcat.SetProperty(path="/Background",
property="top_color",
value=[0, 0, 0])
meshcat.Set2dRenderMode(X_WC=RigidTransform(),
xmin=-1,
xmax=1,
ymin=-1,
ymax=1)
meshcat.ResetRenderMode()
meshcat.AddButton(name="button")
self.assertEqual(meshcat.GetButtonClicks(name="button"), 0)
meshcat.DeleteButton(name="button")
meshcat.AddSlider(name="slider", min=0, max=1, step=0.01, value=0.5)
meshcat.SetSliderValue(name="slider", value=0.7)
self.assertAlmostEqual(meshcat.GetSliderValue(name="slider"),
0.7,
delta=1e-14)
meshcat.DeleteSlider(name="slider")
meshcat.DeleteAddedControls()
self.assertIn("data:application/octet-binary;base64",
meshcat.StaticHtml())
meshcat.Flush()
def __init__(self, tf):
geom = PhysicsGeometryInfo()
geom.register_geometry(tf=torch.eye(4),
geometry=pydrake_geom.Box(0.025, 0.025, 0.1),
color=np.array([0.8, 0.5, 0.2, 1.0]))
super().__init__(tf=tf, physics_geometry_info=geom, observed=True)
def test_shapes(self):
# We'll test some invariants on all shapes as inherited from the Shape
# API.
def assert_shape_api(shape):
self.assertIsInstance(shape, mut.Shape)
shape_cls = type(shape)
shape_copy = shape.Clone()
self.assertIsInstance(shape_copy, shape_cls)
self.assertIsNot(shape, shape_copy)
# Note: these are ordered alphabetical order and not in the declared
# order in shape_specification.h
box = mut.Box(width=1.0, depth=2.0, height=3.0)
assert_shape_api(box)
self.assertEqual(box.width(), 1.0)
self.assertEqual(box.depth(), 2.0)
self.assertEqual(box.height(), 3.0)
assert_pickle(
self, box,
lambda shape: [shape.width(
), shape.depth(), shape.height()])
numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0]))
self.assertAlmostEqual(mut.CalcVolume(box), 6.0, 1e-14)
capsule = mut.Capsule(radius=1.0, length=2.0)
assert_shape_api(capsule)
self.assertEqual(capsule.radius(), 1.0)
self.assertEqual(capsule.length(), 2.0)
assert_pickle(
self, capsule,
lambda shape: [shape.radius(), shape.length()])
junk_path = "arbitrary/path"
convex = mut.Convex(absolute_filename=junk_path, scale=1.0)
assert_shape_api(convex)
self.assertEqual(convex.filename(), junk_path)
self.assertEqual(convex.scale(), 1.0)
assert_pickle(
self, convex,
lambda shape: [shape.filename(), shape.scale()])
cylinder = mut.Cylinder(radius=1.0, length=2.0)
assert_shape_api(cylinder)
self.assertEqual(cylinder.radius(), 1.0)
self.assertEqual(cylinder.length(), 2.0)
assert_pickle(
self, cylinder,
lambda shape: [shape.radius(), shape.length()])
ellipsoid = mut.Ellipsoid(a=1.0, b=2.0, c=3.0)
assert_shape_api(ellipsoid)
self.assertEqual(ellipsoid.a(), 1.0)
self.assertEqual(ellipsoid.b(), 2.0)
self.assertEqual(ellipsoid.c(), 3.0)
assert_pickle(self, ellipsoid, lambda shape:
[shape.a(), shape.b(), shape.c()])
X_FH = mut.HalfSpace.MakePose(Hz_dir_F=[0, 1, 0], p_FB=[1, 1, 1])
self.assertIsInstance(X_FH, RigidTransform)
mesh = mut.Mesh(absolute_filename=junk_path, scale=1.0)
assert_shape_api(mesh)
self.assertEqual(mesh.filename(), junk_path)
self.assertEqual(mesh.scale(), 1.0)
assert_pickle(
self, mesh,
lambda shape: [shape.filename(), shape.scale()])
sphere = mut.Sphere(radius=1.0)
assert_shape_api(sphere)
self.assertEqual(sphere.radius(), 1.0)
assert_pickle(self, sphere, mut.Sphere.radius)
cone = mut.MeshcatCone(height=1.2, a=3.4, b=5.6)
assert_shape_api(cone)
self.assertEqual(cone.height(), 1.2)
self.assertEqual(cone.a(), 3.4)
self.assertEqual(cone.b(), 5.6)
assert_pickle(self, cone,
lambda shape: [shape.height(
), shape.a(), shape.b()])
