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()])