def load(self): """ Loads all visualization elements in the Blender server. @pre The `scene_graph` used to construct this object must be part of a fully constructed diagram (e.g. via `DiagramBuilder.Build()`). """ self.bsi.send_remote_call("initialize_scene") # Keeps track of registered bounding boxes, to keep us from # having to register + delete brand new objects every cycle. # If more than this number is needed in a given cycle, # more are registered and this number is increased. # If less are needed, the unused ones are made invisible. # The attributes of all bounding boxes are updated every cycle. self.num_registered_bounding_boxes = 0 self.num_visible_bounding_boxes = 0 # Intercept load message via mock LCM. mock_lcm = DrakeMockLcm() mock_lcm_subscriber = Subscriber(lcm=mock_lcm, channel="DRAKE_VIEWER_LOAD_ROBOT", lcm_type=lcmt_viewer_load_robot) DispatchLoadMessage(self._scene_graph, mock_lcm) mock_lcm.HandleSubscriptions(0) assert mock_lcm_subscriber.count > 0 load_robot_msg = mock_lcm_subscriber.message # Load all the elements over on the Blender side. self.num_link_geometries_by_link_name = {} self.link_subgeometry_local_tfs_by_link_name = {} for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] [source_name, frame_name] = self._parse_name(link.name) self.num_link_geometries_by_link_name[link.name] = link.num_geom tfs = [] for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currently sets alpha=0 to make collision # geometry "invisible". Ignore those geometries here. if geom.color[3] == 0: continue geom_name = self._format_geom_name(source_name, frame_name, link.robot_num, j) tfs.append( RigidTransform(RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsMatrix4()) # It will have a material with this key. # We will set it up after deciding whether it's # a mesh with a texture or not... material_key = "material_" + geom_name material_key_assigned = False # Check overrides for override in self.material_overrides: if override[0].match(geom_name): print("Using override ", override[0].pattern, " on name ", geom_name, " with applied args ", override[1]) self.bsi.send_remote_call("register_material", name=material_key, **(override[1])) material_key_assigned = True if geom.type == geom.BOX: assert geom.num_float_data == 3 # Blender cubes are 2x2x2 by default do_load_geom = lambda: self.bsi.send_remote_call( "register_object", name="obj_" + geom_name, type="cube", scale=[x * 0.5 for x in geom.float_data[:3]], location=geom.position, quaternion=geom.quaternion, material=material_key) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 do_load_geom = lambda: self.bsi.send_remote_call( "register_object", name="obj_" + geom_name, type="sphere", scale=geom.float_data[0], location=geom.position, quaternion=geom.quaternion, material=material_key) elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 # Blender cylinders are r=1, h=2 by default do_load_geom = lambda: self.bsi.send_remote_call( "register_object", name="obj_" + geom_name, type="cylinder", scale=[ geom.float_data[0], geom.float_data[0], 0.5 * geom. float_data[1] ], location=geom.position, quaternion=geom.quaternion, material=material_key) elif geom.type == geom.MESH: do_load_geom = lambda: self.bsi.send_remote_call( "register_object", name="obj_" + geom_name, type="obj", path=geom.string_data[0:-3] + "obj", scale=geom.float_data[:3], location=geom.position, quaternion=geom.quaternion, material=material_key) # Attempt to find a texture for the object by looking for an # identically-named *.png next to the model. # TODO(gizatt): In the long term, this kind of material information # should be gleaned from the SceneGraph constituents themselves, so # that we visualize what the simulation is *actually* reasoning about # rather than what files happen to be present. candidate_texture_path_png = geom.string_data[0:-3] + "png" if not material_key_assigned and os.path.exists( candidate_texture_path_png): material_key_assigned = self.bsi.send_remote_call( "register_material", name=material_key, material_type="color_texture", path=candidate_texture_path_png) else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( geom.type)) continue if not material_key_assigned: material_key_assigned = self.bsi.send_remote_call( "register_material", name=material_key, material_type="color", color=geom.color[:4]) # Finally actually load the geometry now that the material # is registered. do_load_geom() self.link_subgeometry_local_tfs_by_link_name[link.name] = tfs for i, camera_tf in enumerate(self.camera_tfs): camera_tf_post = self.global_transform.multiply(camera_tf) self.bsi.send_remote_call( "register_camera", name="cam_%d" % i, location=camera_tf_post.translation().tolist(), quaternion=camera_tf_post.quaternion().wxyz().tolist(), angle=90) self.bsi.send_remote_call("configure_rendering", camera_name='cam_%d' % i, resolution=[640 * 2, 480 * 2], file_format="JPEG")
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 options.random_seed = 1314 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 test_contact_force(self): """A block sitting on a table.""" object_file_path = FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf") table_file_path = FindResourceOrThrow( "drake/examples/kuka_iiwa_arm/models/table/" "extra_heavy_duty_table_surface_only_collision.sdf") # T: tabletop frame. X_TObject = RigidTransform([0, 0, 0.2]) builder = DiagramBuilder() plant = MultibodyPlant(0.002) _, scene_graph = AddMultibodyPlantSceneGraph(builder, plant) object_model = Parser(plant=plant).AddModelFromFile(object_file_path) table_model = Parser(plant=plant).AddModelFromFile(table_file_path) # Weld table to world. plant.WeldFrames(A=plant.world_frame(), B=plant.GetFrameByName("link", table_model)) plant.Finalize() # Add meshcat visualizer. viz = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, open_browser=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), viz.get_input_port(0)) # Add contact visualizer. contact_viz = builder.AddSystem( MeshcatContactVisualizer(meshcat_viz=viz, force_threshold=0, contact_force_scale=10, plant=plant)) contact_input_port = contact_viz.GetInputPort("contact_results") builder.Connect(plant.GetOutputPort("contact_results"), contact_input_port) builder.Connect(scene_graph.get_pose_bundle_output_port(), contact_viz.GetInputPort("pose_bundle")) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() mbp_context = diagram.GetMutableSubsystemContext( plant, diagram_context) X_WT = plant.CalcRelativeTransform(mbp_context, plant.world_frame(), plant.GetFrameByName("top_center")) plant.SetFreeBodyPose(mbp_context, plant.GetBodyByName("base_link", object_model), X_WT.multiply(X_TObject)) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(1.0) contact_viz_context = (diagram.GetMutableSubsystemContext( contact_viz, diagram_context)) contact_results = contact_viz.EvalAbstractInput( contact_viz_context, contact_input_port.get_index()).get_value() self.assertGreater(contact_results.num_point_pair_contacts(), 0) self.assertEqual(contact_viz._contact_key_counter, 4)
def test_rgbd_sensor(self): def check_ports(system): self.assertIsInstance(system.query_object_input_port(), InputPort) self.assertIsInstance(system.color_image_output_port(), OutputPort) self.assertIsInstance(system.depth_image_32F_output_port(), OutputPort) self.assertIsInstance(system.depth_image_16U_output_port(), OutputPort) self.assertIsInstance(system.label_image_output_port(), OutputPort) self.assertIsInstance(system.body_pose_in_world_output_port(), OutputPort) # Use HDTV size. width = 1280 height = 720 # There are *two* variants of the constructor for each camera # representation: one with color and depth explicitly specified and one # with only depth. We enumerate all four here. def construct(parent_id, X_PB): color_camera = ColorRenderCamera( RenderCameraCore("renderer", mut.CameraInfo(width, height, np.pi / 6), ClippingRange(0.1, 6.0), RigidTransform()), False) depth_camera = DepthRenderCamera(color_camera.core(), DepthRange(0.1, 5.5)) return mut.RgbdSensor(parent_id=parent_id, X_PB=X_PB, color_camera=color_camera, depth_camera=depth_camera) def construct_single(parent_id, X_PB): depth_camera = DepthRenderCamera( RenderCameraCore("renderer", mut.CameraInfo(width, height, np.pi / 6), ClippingRange(0.1, 6.0), RigidTransform()), DepthRange(0.1, 5.5)) return mut.RgbdSensor(parent_id=parent_id, X_PB=X_PB, depth_camera=depth_camera) # Put it at the origin. X_WB = RigidTransform() # This id would fail if we tried to render; no such id exists. parent_id = FrameId.get_new_id() def check_info(camera_info): self.assertIsInstance(camera_info, mut.CameraInfo) self.assertEqual(camera_info.width(), width) self.assertEqual(camera_info.height(), height) for constructor in [construct, construct_single]: sensor = constructor(parent_id, X_WB) check_info(sensor.color_camera_info()) check_info(sensor.depth_camera_info()) self.assertIsInstance(sensor.X_BC(), RigidTransform) self.assertIsInstance(sensor.X_BD(), RigidTransform) self.assertEqual(sensor.parent_frame_id(), parent_id) check_ports(sensor) # Test discrete camera. We'll simply use the last sensor constructed. period = mut.RgbdSensorDiscrete.kDefaultPeriod discrete = mut.RgbdSensorDiscrete(sensor=sensor, period=period, render_label_image=True) self.assertTrue(discrete.sensor() is sensor) self.assertEqual(discrete.period(), period) check_ports(discrete) # That we can access the state as images. context = discrete.CreateDefaultContext() values = context.get_abstract_state() self.assertIsInstance(values.get_value(0), Value[mut.ImageRgba8U]) self.assertIsInstance(values.get_value(1), Value[mut.ImageDepth32F]) self.assertIsInstance(values.get_value(2), Value[mut.ImageDepth16U]) self.assertIsInstance(values.get_value(3), Value[mut.ImageLabel16I])
def _build_body_patches(self, use_random_colors, substitute_collocated_mesh_files): """ 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 = {} memq_lcm = DrakeLcm("memq://") memq_lcm_subscriber = Subscriber(lcm=memq_lcm, channel="DRAKE_VIEWER_LOAD_ROBOT", lcm_type=lcmt_viewer_load_robot) # TODO(SeanCurtis-TRI): Use SceneGraph inspection instead of mocking # LCM and inspecting the generated message. DispatchLoadMessage(self._scene_graph, memq_lcm) memq_lcm.HandleSubscriptions(0) assert memq_lcm_subscriber.count > 0 load_robot_msg = memq_lcm_subscriber.message # Spawn a random color generator, in case we need to pick random colors # for some bodies. Each body will be given a unique color when using # this random generator, with each visual element of the body colored # the same. color = iter( plt.cm.rainbow(np.linspace(0, 1, load_robot_msg.num_links))) for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] this_body_patches = [] this_body_colors = [] this_color = next(color) for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currently sets alpha=0 to make collision # geometry "invisible". Ignore those geometries here. if geom.color[3] == 0: continue X_BG = RigidTransform( RotationMatrix(Quaternion(geom.quaternion)), geom.position) if geom.type == geom.BOX: assert geom.num_float_data == 3 # Draw a bounding box. patch_G = np.vstack( (geom.float_data[0] / 2. * np.array([-1, -1, 1, 1, -1, -1, 1, 1]), geom.float_data[1] / 2. * np.array([-1, 1, -1, 1, -1, 1, -1, 1]), geom.float_data[2] / 2. * np.array([-1, -1, -1, -1, 1, 1, 1, 1]))) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 radius = geom.float_data[0] 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 *= radius elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 radius = geom.float_data[0] length = geom.float_data[1] # 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 geom.type == geom.MESH: filename = geom.string_data 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("The given file " + filename + " is not " "supported and no alternate " + base + ".obj could be found.") if not os.path.exists(filename): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), filename) mesh = ReadObjToSurfaceMesh(filename) patch_G = np.vstack([v.r_MV() for v in mesh.vertices()]).T else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( geom.type)) 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 use_random_colors: this_body_colors.append(this_color) else: this_body_colors.append(geom.color) self._patch_Blist[link.name] = this_body_patches self._patch_Blist_colors[link.name] = this_body_colors
def test_query_object(self, T): RigidTransform = RigidTransform_[float] SceneGraph = mut.SceneGraph_[T] QueryObject = mut.QueryObject_[T] SceneGraphInspector = mut.SceneGraphInspector_[T] FramePoseVector = mut.FramePoseVector_[T] # First, ensure we can default-construct it. model = QueryObject() self.assertIsInstance(model, QueryObject) scene_graph = SceneGraph() source_id = scene_graph.RegisterSource("source") frame_id = scene_graph.RegisterFrame( source_id=source_id, frame=mut.GeometryFrame("frame")) 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")) render_params = mut.render.RenderEngineVtkParams() renderer_name = "test_renderer" scene_graph.AddRenderer(renderer_name, mut.render.MakeRenderEngineVtk(render_params)) context = scene_graph.CreateDefaultContext() pose_vector = FramePoseVector() pose_vector.set_value(frame_id, RigidTransform_[T]()) scene_graph.get_source_pose_port(source_id).FixValue( context, pose_vector) query_object = scene_graph.get_query_output_port().Eval(context) self.assertIsInstance(query_object.inspector(), SceneGraphInspector) with catch_drake_warnings(expected_count=3): self.assertIsInstance( query_object.X_WF(id=frame_id), RigidTransform_[T]) self.assertIsInstance( query_object.X_PF(id=frame_id), RigidTransform_[T]) self.assertIsInstance( query_object.X_WG(id=geometry_id), RigidTransform_[T]) self.assertIsInstance( query_object.GetPoseInWorld(frame_id=frame_id), RigidTransform_[T]) self.assertIsInstance( query_object.GetPoseInParent(frame_id=frame_id), RigidTransform_[T]) self.assertIsInstance( query_object.GetPoseInWorld(geometry_id=geometry_id), RigidTransform_[T]) # Proximity queries -- all of these will produce empty results. results = query_object.ComputeSignedDistancePairwiseClosestPoints() self.assertEqual(len(results), 0) results = query_object.ComputePointPairPenetration() self.assertEqual(len(results), 0) results = query_object.ComputeSignedDistanceToPoint(p_WQ=(1, 2, 3)) self.assertEqual(len(results), 0) results = query_object.FindCollisionCandidates() self.assertEqual(len(results), 0) self.assertFalse(query_object.HasCollisions()) # ComputeSignedDistancePairClosestPoints() requires two valid geometry # ids. There are none in this SceneGraph instance. Rather than # populating the SceneGraph, we look for the exception thrown in # response to invalid ids as evidence of correct binding. with self.assertRaisesRegex( RuntimeError, r"The geometry given by id \d+ does not reference a geometry" + " that can be used in a signed distance query"): query_object.ComputeSignedDistancePairClosestPoints( geometry_id_A=mut.GeometryId.get_new_id(), geometry_id_B=mut.GeometryId.get_new_id()) # TODO(SeanCurtis-TRI) Remove this call at the same time as deprecating # the subsequent deprecation test; it is only here to show that the # non-keyword call invokes the non-deprecated overload. self.assertRaisesRegex( RuntimeError, r"The geometry given by id \d+ does not reference a geometry" + " that can be used in a signed distance query", query_object.ComputeSignedDistancePairClosestPoints, mut.GeometryId.get_new_id(), mut.GeometryId.get_new_id()) with catch_drake_warnings(expected_count=1): with self.assertRaisesRegex( RuntimeError, r"The geometry given by id \d+ does not reference a geometry" + " that can be used in a signed distance query"): query_object.ComputeSignedDistancePairClosestPoints( id_A=mut.GeometryId.get_new_id(), id_B=mut.GeometryId.get_new_id()) # Confirm rendering API returns images of appropriate type. camera_core = mut.render.RenderCameraCore( renderer_name=renderer_name, intrinsics=CameraInfo(width=10, height=10, fov_y=pi/6), clipping=mut.render.ClippingRange(0.1, 10.0), X_BS=RigidTransform()) color_camera = mut.render.ColorRenderCamera( core=camera_core, show_window=False) depth_camera = mut.render.DepthRenderCamera( core=camera_core, depth_range=mut.render.DepthRange(0.1, 5.0)) image = query_object.RenderColorImage( camera=color_camera, parent_frame=SceneGraph.world_frame_id(), X_PC=RigidTransform()) self.assertIsInstance(image, ImageRgba8U) image = query_object.RenderDepthImage( camera=depth_camera, parent_frame=SceneGraph.world_frame_id(), X_PC=RigidTransform()) self.assertIsInstance(image, ImageDepth32F) image = query_object.RenderLabelImage( camera=color_camera, parent_frame=SceneGraph.world_frame_id(), X_PC=RigidTransform()) self.assertIsInstance(image, ImageLabel16I) with catch_drake_warnings(expected_count=4): # One deprecation warning for constructing DepthCameraProperties # and one for each invocation of Render*Image. depth_camera = mut.render.DepthCameraProperties( width=320, height=240, fov_y=pi/6, renderer_name=renderer_name, z_near=0.1, z_far=5.0) image = query_object.RenderColorImage( camera=depth_camera, parent_frame=SceneGraph.world_frame_id(), X_PC=RigidTransform()) self.assertIsInstance(image, ImageRgba8U) image = query_object.RenderDepthImage( camera=depth_camera, parent_frame=SceneGraph.world_frame_id(), X_PC=RigidTransform()) self.assertIsInstance(image, ImageDepth32F) image = query_object.RenderLabelImage( camera=depth_camera, parent_frame=SceneGraph.world_frame_id(), X_PC=RigidTransform()) self.assertIsInstance(image, ImageLabel16I)
with open(os.path.join(this_dir, 'ur10.sdf.in'), 'r') as file_in: with open(sdf_file_path, 'w') as file_out: file_out.write(file_in.read().replace('PWD_GOES_HERE', this_dir)) builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001) parser = Parser(plant) model_name = "ur10" model = parser.AddModelFromFile(sdf_file_path, model_name) # Weld to world so it doesn't fall through floor :D base_frame = plant.GetFrameByName("base", model) X_WB = RigidTransform([0, 0, 0]) plant.WeldFrames(plant.world_frame(), base_frame, X_WB) plant.Finalize() # Must happen after Finalize # RuntimeError: Pre-finalize calls to 'num_actuated_dofs()' are not allowed; you must call Finalize() first. no_control(plant, builder, model) joints = [] for i in range(plant.num_joints()): joints.append(plant.get_joint(JointIndex(i))) rclpy.init() node = rclpy.create_node('drake_demo')
def __init__(self, visible=Visible(), min_range=MinRange(), max_range=MaxRange(), value=Value()): """ Args: visible: An object with boolean elements for 'roll', 'pitch', 'yaw', 'x', 'y', 'z'; the intention is for this to be the PoseSliders.Visible() namedtuple. Defaults to all true. min_range, max_range, value: Objects with float values for 'roll', 'pitch', 'yaw', 'x', 'y', 'z'; the intention is for the caller to use the PoseSliders.MinRange, MaxRange, and Value namedtuples. See those tuples for default values. """ LeafSystem.__init__(self) self.DeclareAbstractOutputPort( "pose", lambda: AbstractValue.Make(RigidTransform()), self.DoCalcOutput) # Note: This timing affects the keyboard teleop performance. A larger # time step causes more lag in the response. self.DeclarePeriodicPublish(0.01, 0.0) self._roll = FloatSlider(min=min_range.roll, max=max_range.roll, value=value.roll, step=0.01, continuous_update=True, description="roll", layout=Layout(width='90%')) self._pitch = FloatSlider(min=min_range.pitch, max=max_range.pitch, value=value.pitch, step=0.01, continuous_update=True, description="pitch", layout=Layout(width='90%')) self._yaw = FloatSlider(min=min_range.yaw, max=max_range.yaw, value=value.yaw, step=0.01, continuous_update=True, description="yaw", layout=Layout(width='90%')) self._x = FloatSlider(min=min_range.x, max=max_range.x, value=value.x, step=0.01, continuous_update=True, description="x", orient='horizontal', layout=Layout(width='90%')) self._y = FloatSlider(min=min_range.y, max=max_range.y, value=value.y, step=0.01, continuous_update=True, description="y", layout=Layout(width='90%')) self._z = FloatSlider(min=min_range.z, max=max_range.z, value=value.z, step=0.01, continuous_update=True, description="z", layout=Layout(width='90%')) if visible.roll: display(self._roll) if visible.pitch: display(self._pitch) if visible.yaw: display(self._yaw) if visible.x: display(self._x) if visible.y: display(self._y) if visible.z: display(self._z)
def se3_from_xyz(xyz): return RigidTransform(p=xyz)
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--target_realtime_rate", type=float, default=1.0, help="Desired rate relative to real time. See documentation for " "Simulator::set_target_realtime_rate() for details.") parser.add_argument( "--duration", type=float, default=np.inf, help="Desired duration of the simulation in seconds.") parser.add_argument( "--hardware", action='store_true', help="Use the ManipulationStationHardwareInterface instead of an " "in-process simulation.") parser.add_argument( "--test", action='store_true', help="Disable opening the gui window for testing.") parser.add_argument( '--setup', type=str, default='manipulation_class', help="The manipulation station setup to simulate. ", choices=['manipulation_class', 'clutter_clearing', 'planar']) MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() builder = DiagramBuilder() if args.hardware: # TODO(russt): Replace this hard-coded camera serial number with a # config file. camera_ids = ["805212060544"] station = builder.AddSystem(ManipulationStationHardwareInterface( camera_ids)) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) # Initializes the chosen station type. if args.setup == 'manipulation_class': station.SetupManipulationClassStation() station.AddManipulandFromFile( "drake/examples/manipulation_station/models/" + "061_foam_brick.sdf", RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) elif args.setup == 'clutter_clearing': station.SetupClutterClearingStation() ycb_objects = CreateClutterClearingYcbObjectList() for model_file, X_WObject in ycb_objects: station.AddManipulandFromFile(model_file, X_WObject) elif args.setup == 'planar': station.SetupPlanarIiwaStation() station.AddManipulandFromFile( "drake/examples/manipulation_station/models/" + "061_foam_brick.sdf", RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) station.Finalize() ConnectDrakeVisualizer(builder, station.get_scene_graph(), station.GetOutputPort("pose_bundle")) if args.meshcat: meshcat = builder.AddSystem(MeshcatVisualizer( station.get_scene_graph(), zmq_url=args.meshcat, open_browser=args.open_browser)) builder.Connect(station.GetOutputPort("pose_bundle"), meshcat.get_input_port(0)) if args.setup == 'planar': meshcat.set_planar_viewpoint() if args.setup == 'planar': pyplot_visualizer = builder.AddSystem(PlanarSceneGraphVisualizer( station.get_scene_graph())) builder.Connect(station.GetOutputPort("pose_bundle"), pyplot_visualizer.get_input_port(0)) teleop = builder.AddSystem(JointSliders(station.get_controller_plant(), length=800)) if args.test: teleop.window.withdraw() # Don't display the window when testing. num_iiwa_joints = station.num_iiwa_joints() filter = builder.AddSystem(FirstOrderLowPassFilter( time_constant=2.0, size=num_iiwa_joints)) builder.Connect(teleop.get_output_port(0), filter.get_input_port(0)) builder.Connect(filter.get_output_port(0), station.GetInputPort("iiwa_position")) wsg_buttons = builder.AddSystem(SchunkWsgButtons(teleop.window)) builder.Connect(wsg_buttons.GetOutputPort("position"), station.GetInputPort("wsg_position")) builder.Connect(wsg_buttons.GetOutputPort("force_limit"), station.GetInputPort("wsg_force_limit")) # When in regression test mode, log our joint velocities to later check # that they were sufficiently quiet. if args.test: iiwa_velocities = builder.AddSystem(SignalLogger(num_iiwa_joints)) builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"), iiwa_velocities.get_input_port(0)) else: iiwa_velocities = None diagram = builder.Build() simulator = Simulator(diagram) # This is important to avoid duplicate publishes to the hardware interface: simulator.set_publish_every_time_step(False) station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros(num_iiwa_joints)) # If the diagram is only the hardware interface, then we must advance it a # little bit so that first LCM messages get processed. A simulated plant is # already publishing correct positions even without advancing, and indeed # we must not advance a simulated plant until the sliders and filters have # been initialized to match the plant. if args.hardware: simulator.AdvanceTo(1e-6) # Eval the output port once to read the initial positions of the IIWA. q0 = station.GetOutputPort("iiwa_position_measured").Eval( station_context) teleop.set_position(q0) filter.set_initial_output_value(diagram.GetMutableSubsystemContext( filter, simulator.get_mutable_context()), q0) simulator.set_target_realtime_rate(args.target_realtime_rate) simulator.AdvanceTo(args.duration) # Ensure that our initialization logic was correct, by inspecting our # logged joint velocities. if args.test: for time, qdot in zip(iiwa_velocities.sample_times(), iiwa_velocities.data().transpose()): # TODO(jwnimmer-tri) We should be able to do better than a 40 # rad/sec limit, but that's the best we can enforce for now. if qdot.max() > 0.1: print(f"ERROR: large qdot {qdot} at time {time}") sys.exit(1)
def value_to_pose(val): return RigidTransform(RollPitchYaw(val.roll, val.pitch, val.yaw), [val.x, val.y, val.z])
def add_triad( source_id, frame_id, scene_graph, *, length, radius, opacity, X_FT=RigidTransform(), name="frame", ): """ Adds illustration geometry representing the coordinate frame, with the x-axis drawn in red, the y-axis in green and the z-axis in blue. The axes point in +x, +y and +z directions, respectively. Based on [code permalink](https://github.com/RussTedrake/manipulation/blob/5e59811/manipulation/scenarios.py#L367-L414).# noqa Args: source_id: The source registered with SceneGraph. frame_id: A geometry::frame_id registered with scene_graph. scene_graph: The SceneGraph with which we will register the geometry. length: the length of each axis in meters. radius: the radius of each axis in meters. opacity: the opacity of the coordinate axes, between 0 and 1. X_FT: a RigidTransform from the triad frame T to the frame_id frame F name: the added geometry will have names name + " x-axis", etc. """ # x-axis X_TG = RigidTransform( RotationMatrix.MakeYRotation(np.pi / 2), [length / 2.0, 0, 0], ) geom = GeometryInstance( X_FT.multiply(X_TG), Cylinder(radius, length), name + " x-axis" ) geom.set_illustration_properties( MakePhongIllustrationProperties([1, 0, 0, opacity]) ) scene_graph.RegisterGeometry(source_id, frame_id, geom) # y-axis X_TG = RigidTransform( RotationMatrix.MakeXRotation(np.pi / 2), [0, length / 2.0, 0], ) geom = GeometryInstance( X_FT.multiply(X_TG), Cylinder(radius, length), name + " y-axis" ) geom.set_illustration_properties( MakePhongIllustrationProperties([0, 1, 0, opacity]) ) scene_graph.RegisterGeometry(source_id, frame_id, geom) # z-axis X_TG = RigidTransform([0, 0, length / 2.0]) geom = GeometryInstance( X_FT.multiply(X_TG), Cylinder(radius, length), name + " z-axis" ) geom.set_illustration_properties( MakePhongIllustrationProperties([0, 0, 1, opacity]) ) scene_graph.RegisterGeometry(source_id, frame_id, geom)
def DepthCameraDemoSystem(): builder = DiagramBuilder() # If you have trouble finding resources, you can enable trace logging # to see how `FindResource*` is searching. set_log_level("trace") # Create the physics engine + scene graph. plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) # Add a single object into it. Parser(plant, scene_graph).AddModelFromFile(FindResourceOrThrow( "drake/manipulation/models/ycb/sdf/006_mustard_bottle.sdf")) # Add a rendering engine renderer = "my_renderer" scene_graph.AddRenderer(renderer, MakeRenderEngineVtk(RenderEngineVtkParams())) plant.Finalize() # Add a visualizer just to help us see the object. use_meshcat = False if use_meshcat: meshcat = builder.AddSystem(MeshcatVisualizer(scene_graph)) builder.Connect(scene_graph.get_pose_bundle_output_port(), meshcat.get_input_port(0)) # Add a camera to the environment. pose = RigidTransform(RollPitchYaw(-0.2, 0.2, 0), [-.1, -0.1, -.5]) properties = DepthCameraProperties( width=640, height=480, fov_y=np.pi / 4.0, renderer_name=renderer, z_near=0.1, z_far=10.0) camera = builder.AddSystem(RgbdSensor( parent_id=scene_graph.world_frame_id(), X_PB=pose, properties=properties, show_window=False)) camera.set_name("rgbd_sensor") builder.Connect(scene_graph.get_query_output_port(), camera.query_object_input_port()) # Export the camera outputs builder.ExportOutput(camera.color_image_output_port(), "color_image") builder.ExportOutput(camera.depth_image_32F_output_port(), "depth_image") # Add a system to convert the camera output into a point cloud to_point_cloud = builder.AddSystem( DepthImageToPointCloud( camera_info=camera.depth_camera_info(), fields=BaseField.kXYZs | BaseField.kRGBs)) builder.Connect( camera.depth_image_32F_output_port(), to_point_cloud.depth_image_input_port()) builder.Connect( camera.color_image_output_port(), to_point_cloud.color_image_input_port()) # Export the point cloud output. builder.ExportOutput( to_point_cloud.point_cloud_output_port(), "point_cloud") diagram = builder.Build() diagram.set_name("depth_camera_demo_system") return diagram
def buildViewPatches(self, use_random_colors): ''' Generates view patches. self.viewPatches stores a list of viewPatches for each body (starting at body id 1). A viewPatch is a list of all 3D vertices of a piece of visual geometry. ''' self.viewPatches = {} self.viewPatchColors = {} mock_lcm = DrakeMockLcm() mock_lcm_subscriber = Subscriber(lcm=mock_lcm, channel="DRAKE_VIEWER_LOAD_ROBOT", lcm_type=lcmt_viewer_load_robot) DispatchLoadMessage(self._scene_graph, mock_lcm) mock_lcm.HandleSubscriptions(0) assert mock_lcm_subscriber.count > 0 load_robot_msg = mock_lcm_subscriber.message # Spawn a random color generator, in case we need to pick # random colors for some bodies. Each body will be given # a unique color when using this random generator, with # each visual element of the body colored the same. color = iter(plt.cm.rainbow(np.linspace(0, 1, load_robot_msg.num_links))) for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] this_body_patches = [] this_body_colors = [] this_color = next(color) for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currently sets alpha=0 to make collision # geometry "invisible". Ignore those geometries here. if geom.color[3] == 0: continue element_local_tf = RigidTransform( RotationMatrix(Quaternion(geom.quaternion)), geom.position) if geom.type == geom.BOX: assert geom.num_float_data == 3 # Draw a bounding box. patch = np.vstack(( geom.float_data[0]/2.*np.array( [-1, -1, 1, 1, -1, -1, 1, 1]), geom.float_data[1]/2.*np.array( [-1, 1, -1, 1, -1, 1, -1, 1]), geom.float_data[2]/2.*np.array( [-1, -1, -1, -1, 1, 1, 1, 1]))) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 radius = geom.float_data[0] 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 = np.vstack([ np.sin(longi)*np.cos(lati), np.sin(longi)*np.sin(lati), np.cos(lati)]) patch *= radius elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 radius = geom.float_data[0] length = geom.float_data[1] # 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 = 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 geom.type == geom.MESH: # TODO(gizatt): Remove trimesh and shapely dependency when # vertex information is accessible from the SceneGraph # interface. mesh = trimesh.load(geom.string_data) patch = mesh.vertices.T # Apply scaling for i in range(3): patch[i, :] *= geom.float_data[i] else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( geom.type)) continue patch = np.vstack((patch, np.ones((1, patch.shape[1])))) patch = np.dot(element_local_tf.GetAsMatrix4(), patch) # Close path if not closed if (patch[:, -1] != patch[:, 0]).any(): patch = np.hstack((patch, patch[:, 0][np.newaxis].T)) this_body_patches.append(patch) if use_random_colors: this_body_colors.append(this_color) else: this_body_colors.append(geom.color) self.viewPatches[link.name] = this_body_patches self.viewPatchColors[link.name] = this_body_colors
def set_atlas_initial_pose(plant, plant_context): pelvis = plant.GetBodyByName("pelvis") X_WP = RigidTransform(RollPitchYaw(0.0, 0.0, 0.0), np.array([0.0, 0.0, 0.94])) plant.SetFreeBodyPose(plant_context, pelvis, X_WP)
def construct_environment(masses: typing.List, box_sizes: typing.List): """ Construct an environment with many free boxes. @param masses masses[i] is the mass of box i. @param box_sizes box_sizes[i] is the size of box i. """ builder = DiagramBuilder_[AutoDiffXd]() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) # Add the ground as a big box. ground_box = plant.AddRigidBody( "ground", SpatialInertia(1, np.array([0, 0, 0]), UnitInertia(1, 1, 1))) X_WG = RigidTransform([0, 0, -0.05]) ground_geometry_id = plant.RegisterCollisionGeometry( ground_box, RigidTransform(), Box(10, 10, 0.1), "ground", CoulombFriction(0.9, 0.8)) plant.RegisterVisualGeometry(ground_box, RigidTransform(), Box(10, 10, 0.1), "ground", [0.5, 0.5, 0.5, 1.]) plant.WeldFrames(plant.world_frame(), ground_box.body_frame(), X_WG) # Add boxes assert isinstance(masses, list) assert isinstance(box_sizes, list) assert len(masses) == len(box_sizes) num_boxes = len(masses) boxes = [] boxes_geometry_id = [] for i in range(num_boxes): box_name = f"box{i}" boxes.append( plant.AddRigidBody( box_name, SpatialInertia(masses[i], np.array([0, 0, 0]), UnitInertia(1, 1, 1)))) box_shape = Box(box_sizes[i][0], box_sizes[i][1], box_sizes[i][2]) boxes_geometry_id.append( plant.RegisterCollisionGeometry(boxes[i], RigidTransform(), box_shape, f"{box_name}_box", CoulombFriction(0.9, 0.8))) plant.RegisterVisualGeometry(ground_box, RigidTransform(), box_shape, f"{box_name}_box", [0.5, 0.5, 0.5, 1.]) # Add small spheres at the corners of the box. vertices = np.array([ [1, 1, 1], [1, 1, -1], [1, -1, 1], [1, -1, -1], [-1, 1, 1], [-1, -1, 1], [-1, 1, -1], [-1, -1, -1]]) *\ np.tile(box_sizes[i]/2, (8, 1)) sphere_shape = Sphere(0.001) for j in range(8): plant.RegisterCollisionGeometry(boxes[i], RigidTransform(vertices[j]), sphere_shape, f"{box_name}_sphere{j}", CoulombFriction(0.9, 0.8)) plant.RegisterVisualGeometry(boxes[i], RigidTransform(vertices[j]), sphere_shape, f"{box_name}_sphere{j}", [0.5, 0.5, 0.5, 1]) plant.Finalize() diagram = builder.Build() return Environment(plant=plant, scene_graph=scene_graph, diagram=diagram, boxes=boxes, ground_geometry_id=ground_geometry_id, boxes_geometry_id=boxes_geometry_id)
def load(self): """ Loads `meshcat` visualization elements. @pre The `scene_graph` used to construct this object must be part of a fully constructed diagram (e.g. via `DiagramBuilder.Build()`). """ self.vis[self.prefix].delete() # Intercept load message via mock LCM. mock_lcm = DrakeMockLcm() DispatchLoadMessage(self._scene_graph, mock_lcm) load_robot_msg = lcmt_viewer_load_robot.decode( mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT")) # Translate elements to `meshcat`. for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] [source_name, frame_name] = link.name.split("::") for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currenly sets alpha=0 to make collision # geometry "invisible". Ignore those geometries here. if geom.color[3] == 0: continue element_local_tf = RigidTransform( RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsMatrix4() if geom.type == geom.BOX: assert geom.num_float_data == 3 meshcat_geom = meshcat.geometry.Box(geom.float_data) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0]) elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 meshcat_geom = meshcat.geometry.Cylinder( geom.float_data[1], geom.float_data[0]) # In Drake, cylinders are along +z # In meshcat, cylinders are along +y # Rotate to fix this misalignment extra_rotation = tf.rotation_matrix( math.pi / 2., [1, 0, 0]) element_local_tf[0:3, 0:3] = \ element_local_tf[0:3, 0:3].dot( extra_rotation[0:3, 0:3]) elif geom.type == geom.MESH: meshcat_geom = \ meshcat.geometry.ObjMeshGeometry.from_file( geom.string_data[0:-3] + "obj") else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( geom.type)) continue # Turn a list of R,G,B elements (any indexable list of >= 3 # elements will work), where each element is specified on range # [0., 1.], into the equivalent 24-bit value 0xRRGGBB. def Rgb2Hex(rgb): val = 0 for i in range(3): val += (256**(2 - i)) * int(255 * rgb[i]) return val self.vis[self.prefix][source_name][str(link.robot_num)][ frame_name][str(j)]\ .set_object(meshcat_geom, meshcat.geometry .MeshLambertMaterial( color=Rgb2Hex(geom.color))) self.vis[self.prefix][source_name][str( link.robot_num)][frame_name][str(j)].set_transform( element_local_tf)
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--target_realtime_rate", type=float, default=1.0, help="Desired rate relative to real time. See documentation for " "Simulator::set_target_realtime_rate() for details.") parser.add_argument("--duration", type=float, default=np.inf, help="Desired duration of the simulation in seconds.") parser.add_argument( "--hardware", action='store_true', help="Use the ManipulationStationHardwareInterface instead of an " "in-process simulation.") parser.add_argument("--test", action='store_true', help="Disable opening the gui window for testing.") parser.add_argument( "--filter_time_const", type=float, default=0.005, help="Time constant for the first order low pass filter applied to" "the teleop commands") parser.add_argument( "--velocity_limit_factor", type=float, default=1.0, help="This value, typically between 0 and 1, further limits the " "iiwa14 joint velocities. It multiplies each of the seven " "pre-defined joint velocity limits. " "Note: The pre-defined velocity limits are specified by " "iiwa14_velocity_limits, found in this python file.") parser.add_argument('--setup', type=str, default='manipulation_class', help="The manipulation station setup to simulate. ", choices=['manipulation_class', 'clutter_clearing']) MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() if args.test: # Don't grab mouse focus during testing. grab_focus = False # See: https://stackoverflow.com/a/52528832/7829525 os.environ["SDL_VIDEODRIVER"] = "dummy" else: grab_focus = True builder = DiagramBuilder() if args.hardware: station = builder.AddSystem(ManipulationStationHardwareInterface()) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) # Initializes the chosen station type. if args.setup == 'manipulation_class': station.SetupManipulationClassStation() station.AddManipulandFromFile( ("drake/examples/manipulation_station/models/" "061_foam_brick.sdf"), RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) elif args.setup == 'clutter_clearing': station.SetupClutterClearingStation() ycb_objects = CreateDefaultYcbObjectList() for model_file, X_WObject in ycb_objects: station.AddManipulandFromFile(model_file, X_WObject) station.Finalize() ConnectDrakeVisualizer(builder, station.get_scene_graph(), station.GetOutputPort("pose_bundle")) if args.meshcat: meshcat = builder.AddSystem( MeshcatVisualizer(station.get_scene_graph(), zmq_url=args.meshcat)) builder.Connect(station.GetOutputPort("pose_bundle"), meshcat.get_input_port(0)) robot = station.get_controller_plant() params = DifferentialInverseKinematicsParameters(robot.num_positions(), robot.num_velocities()) time_step = 0.005 params.set_timestep(time_step) # True velocity limits for the IIWA14 (in rad, rounded down to the first # decimal) iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3]) # Stay within a small fraction of those limits for this teleop demo. factor = args.velocity_limit_factor params.set_joint_velocity_limits( (-factor * iiwa14_velocity_limits, factor * iiwa14_velocity_limits)) differential_ik = builder.AddSystem( DifferentialIK(robot, robot.GetFrameByName("iiwa_link_7"), params, time_step)) builder.Connect(differential_ik.GetOutputPort("joint_position_desired"), station.GetInputPort("iiwa_position")) teleop = builder.AddSystem(MouseKeyboardTeleop(grab_focus=grab_focus)) filter_ = builder.AddSystem( FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6)) builder.Connect(teleop.get_output_port(0), filter_.get_input_port(0)) builder.Connect(filter_.get_output_port(0), differential_ik.GetInputPort("rpy_xyz_desired")) builder.Connect(teleop.GetOutputPort("position"), station.GetInputPort("wsg_position")) builder.Connect(teleop.GetOutputPort("force_limit"), station.GetInputPort("wsg_force_limit")) diagram = builder.Build() simulator = Simulator(diagram) # This is important to avoid duplicate publishes to the hardware interface: simulator.set_publish_every_time_step(False) station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros(7)) simulator.AdvanceTo(1e-6) q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context) differential_ik.parameters.set_nominal_joint_position(q0) teleop.SetPose(differential_ik.ForwardKinematics(q0)) filter_.set_initial_output_value( diagram.GetMutableSubsystemContext(filter_, simulator.get_mutable_context()), teleop.get_output_port(0).Eval( diagram.GetMutableSubsystemContext( teleop, simulator.get_mutable_context()))) differential_ik.SetPositions( diagram.GetMutableSubsystemContext(differential_ik, simulator.get_mutable_context()), q0) simulator.set_target_realtime_rate(args.target_realtime_rate) print_instructions() simulator.AdvanceTo(args.duration)
def test_render_engine_api(self): class DummyRenderEngine(mut.render.RenderEngine): """Mirror of C++ DummyRenderEngine.""" # See comment below about `rgbd_sensor_test.cc`. latest_instance = None def __init__(self, render_label=None): mut.render.RenderEngine.__init__(self) # N.B. We do not hide these because this is a test class. # Normally, you would want to hide this. self.force_accept = False self.registered_geometries = set() self.updated_ids = {} self.include_group_name = "in_test" self.X_WC = RigidTransform_[float]() self.color_count = 0 self.depth_count = 0 self.label_count = 0 self.color_camera = None self.depth_camera = None self.label_camera = None def UpdateViewpoint(self, X_WC): DummyRenderEngine.latest_instance = self self.X_WC = X_WC def ImplementGeometry(self, shape, user_data): DummyRenderEngine.latest_instance = self def DoRegisterVisual(self, id, shape, properties, X_WG): DummyRenderEngine.latest_instance = self mut.GetRenderLabelOrThrow(properties) if self.force_accept or properties.HasGroup( self.include_group_name ): self.registered_geometries.add(id) return True return False def DoUpdateVisualPose(self, id, X_WG): DummyRenderEngine.latest_instance = self self.updated_ids[id] = X_WG def DoRemoveGeometry(self, id): DummyRenderEngine.latest_instance = self self.registered_geometries.remove(id) def DoClone(self): DummyRenderEngine.latest_instance = self new = DummyRenderEngine() new.force_accept = copy.copy(self.force_accept) new.registered_geometries = copy.copy( self.registered_geometries) new.updated_ids = copy.copy(self.updated_ids) new.include_group_name = copy.copy(self.include_group_name) new.X_WC = copy.copy(self.X_WC) new.color_count = copy.copy(self.color_count) new.depth_count = copy.copy(self.depth_count) new.label_count = copy.copy(self.label_count) new.color_camera = copy.copy(self.color_camera) new.depth_camera = copy.copy(self.depth_camera) new.label_camera = copy.copy(self.label_camera) return new def DoRenderColorImage(self, camera, color_image_out): DummyRenderEngine.latest_instance = self self.color_count += 1 self.color_camera = camera def DoRenderDepthImage(self, camera, depth_image_out): DummyRenderEngine.latest_instance = self self.depth_count += 1 self.depth_camera = camera def DoRenderLabelImage(self, camera, label_image_out): DummyRenderEngine.latest_instance = self self.label_count += 1 self.label_camera = camera engine = DummyRenderEngine() self.assertIsInstance(engine, mut.render.RenderEngine) self.assertIsInstance(engine.Clone(), DummyRenderEngine) # Test implementation of C++ interface by using RgbdSensor. renderer_name = "renderer" builder = DiagramBuilder() scene_graph = builder.AddSystem(mut.SceneGraph()) # N.B. This passes ownership. scene_graph.AddRenderer(renderer_name, engine) sensor = builder.AddSystem(RgbdSensor( parent_id=scene_graph.world_frame_id(), X_PB=RigidTransform(), depth_camera=mut.render.DepthRenderCamera( mut.render.RenderCameraCore( renderer_name, CameraInfo(640, 480, np.pi/4), mut.render.ClippingRange(0.1, 5.0), RigidTransform()), mut.render.DepthRange(0.1, 5.0)))) builder.Connect( scene_graph.get_query_output_port(), sensor.query_object_input_port(), ) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() sensor_context = sensor.GetMyContextFromRoot(diagram_context) image = sensor.color_image_output_port().Eval(sensor_context) # N.B. Because there's context cloning going on under the hood, we # won't be interacting with our originally registered instance. # See `rgbd_sensor_test.cc` as well. current_engine = DummyRenderEngine.latest_instance self.assertIsNot(current_engine, engine) self.assertIsInstance(image, ImageRgba8U) self.assertEqual(current_engine.color_count, 1) image = sensor.depth_image_32F_output_port().Eval(sensor_context) self.assertIsInstance(image, ImageDepth32F) self.assertEqual(current_engine.depth_count, 1) image = sensor.depth_image_16U_output_port().Eval(sensor_context) self.assertIsInstance(image, ImageDepth16U) self.assertEqual(current_engine.depth_count, 2) image = sensor.label_image_output_port().Eval(sensor_context) self.assertIsInstance(image, ImageLabel16I) self.assertEqual(current_engine.label_count, 1) # Confirm that the CameraProperties APIs are *not* available. with catch_drake_warnings(expected_count=2): cam = mut.render.CameraProperties(10, 10, np.pi / 4, "") depth_cam = mut.render.DepthCameraProperties(10, 10, np.pi / 4, "", 0.1, 5) with self.assertRaises(TypeError): engine.RenderColorImage( cam, True, ImageRgba8U(cam.width, cam.height)) with self.assertRaises(TypeError): engine.RenderLabelImage( cam, True, ImageLabel16I(cam.width, cam.height)) with self.assertRaises(TypeError): engine.RenderDepthImage( depth_cam, True, ImageDepth32F(depth_cam.width, depth_cam.height))
def add_pusher(self): """ Adds a cylindrical pusher object :return: :rtype: """ mbp = self.mbp parser = self.parser pusher_model_idx = parser.AddModelFromFile(paths.xy_slide, "pusher") base_link = mbp.GetBodyByName("base", pusher_model_idx) # weld it to the world mbp.WeldFrames(mbp.world_frame(), base_link.body_frame()) self.models['pusher'] = pusher_model_idx radius = 0.01 length = 0.1 pusher_shape = Cylinder(radius, length) # This rigid body will be added to the pusher model instance world_body = mbp.world_body() pusher_body = mbp.AddRigidBody( "pusher_body", pusher_model_idx, SpatialInertia(mass=10.0, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(1.0, 1.0, 1.0))) self._rigid_bodies['pusher'] = pusher_body # weld it to EE frame at particular height translation = np.zeros(3) translation[ 2] = length / 2.0 + 0.001 # give it a little room for collision stuff T_EE_P = RigidTransform(p=translation) ee_link = mbp.GetBodyByName("end_effector", pusher_model_idx) mbp.WeldFrames(ee_link.body_frame(), pusher_body.body_frame(), T_EE_P) # color it green color = np.array([0., 1., 0., 1.]) mbp.RegisterVisualGeometry(pusher_body, RigidTransform.Identity(), pusher_shape, "pusher_vis", color) mbp.RegisterCollisionGeometry(pusher_body, RigidTransform.Identity(), pusher_shape, "pusher_collision", CoulombFriction(0.9, 0.8)) def export_port_func(): # export relevant ports actuation_input_port = mbp.get_actuation_input_port( pusher_model_idx) state_output_port = mbp.get_state_output_port(pusher_model_idx) self._port_names["pusher_state_output"] = "pusher_state_output" self._port_names[ "pusher_actuation_input"] = "pusher_actuation_input" self.builder.ExportOutput(state_output_port, self._port_names["pusher_state_output"]) self.builder.ExportInput( actuation_input_port, self._port_names["pusher_actuation_input"]) self._export_port_functions.append(export_port_func)
def __init__(self, visible=Visible(), min_range=MinRange(), max_range=MaxRange(), value=Value()): """ Args: visible: An object with boolean elements for 'roll', 'pitch', 'yaw', 'x', 'y', 'z'; the intention is for this to be the PoseSliders.Visible() namedtuple. Defaults to all true. min_range, max_range, value: Objects with float values for 'roll', 'pitch', 'yaw', 'x', 'y', 'z'; the intention is for the caller to use the PoseSliders.MinRange, MaxRange, and Value namedtuples. See those tuples for default values. """ LeafSystem.__init__(self) port = self.DeclareAbstractOutputPort( "pose", lambda: AbstractValue.Make(RigidTransform()), self.DoCalcOutput) # The widgets themselves have undeclared state. For now, we accept it, # and simply disable caching on the output port. # TODO(russt): consider implementing the more elaborate methods seen # in, e.g., LcmMessageSubscriber. port.disable_caching_by_default() # Note: This timing affects the keyboard teleop performance. A larger # time step causes more lag in the response. self.DeclarePeriodicEvent(0.01, 0.0, PublishEvent(self._process_event_queue)) self._roll = FloatSlider(min=min_range.roll, max=max_range.roll, value=value.roll, step=0.01, continuous_update=True, description="roll", layout=Layout(width='90%')) self._pitch = FloatSlider(min=min_range.pitch, max=max_range.pitch, value=value.pitch, step=0.01, continuous_update=True, description="pitch", layout=Layout(width='90%')) self._yaw = FloatSlider(min=min_range.yaw, max=max_range.yaw, value=value.yaw, step=0.01, continuous_update=True, description="yaw", layout=Layout(width='90%')) self._x = FloatSlider(min=min_range.x, max=max_range.x, value=value.x, step=0.01, continuous_update=True, description="x", orient='horizontal', layout=Layout(width='90%')) self._y = FloatSlider(min=min_range.y, max=max_range.y, value=value.y, step=0.01, continuous_update=True, description="y", layout=Layout(width='90%')) self._z = FloatSlider(min=min_range.z, max=max_range.z, value=value.z, step=0.01, continuous_update=True, description="z", layout=Layout(width='90%')) if visible.roll: display(self._roll) if visible.pitch: display(self._pitch) if visible.yaw: display(self._yaw) if visible.x: display(self._x) if visible.y: display(self._y) if visible.z: display(self._z)
builder = DiagramBuilder() if args.hardware: # TODO(russt): Replace this hard-coded camera serial number with a config # file. camera_ids = ["805212060544"] station = builder.AddSystem( ManipulationStationHardwareInterface(camera_ids)) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) station.SetupManipulationClassStation() station.AddManipulandFromFile( "drake/examples/manipulation_station/models/061_foam_brick.sdf", RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) station.Finalize() ConnectDrakeVisualizer(builder, station.get_scene_graph(), station.GetOutputPort("pose_bundle")) if args.meshcat: meshcat = builder.AddSystem( MeshcatVisualizer(station.get_scene_graph(), zmq_url=args.meshcat, open_browser=args.open_browser)) builder.Connect(station.GetOutputPort("pose_bundle"), meshcat.get_input_port(0)) teleop = builder.AddSystem( JointSliders(station.get_controller_plant(), length=800)) if args.test:
from pydrake.math import RigidTransform, RollPitchYaw import utilities as utils from scipy.special import erfinv import pickle import os from tempfile import TemporaryFile from pydrake.all import PiecewisePolynomial from systems.visualization import Visualizer # Create the block model with the default flat terrain _file = "systems/urdf/sliding_block.urdf" plant = TimeSteppingMultibodyPlant(file=_file) body_inds = plant.multibody.GetBodyIndices(plant.model_index) base_frame = plant.multibody.get_body(body_inds[0]).body_frame() plant.multibody.WeldFrames(plant.multibody.world_frame(), base_frame, RigidTransform()) plant.Finalize() # Get the default context context = plant.multibody.CreateDefaultContext() # set chance constraints parameters beta, theta, sigma = 0.6, 0.6, 0.3 chance_params = np.array([beta, theta, sigma]) # set friction ERM parameters friction_variance = sigma friction_bias = 0.01 friction_multiplier = 1e6 friction_erm_params = np.array( [friction_variance, friction_bias, friction_multiplier]) # set normal distance ERM parameters distance_variance = 0.1
def test_model_instance_state_access(self): # Create a MultibodyPlant with a kuka arm and a schunk gripper. # the arm is welded to the world, the gripper is welded to the # arm's end effector. wsg50_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "wsg_50_description/sdf/schunk_wsg_50.sdf") iiwa_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") plant = MultibodyPlant() parser = Parser(plant) iiwa_model = parser.AddModelFromFile( file_name=iiwa_sdf_path, model_name='robot') gripper_model = parser.AddModelFromFile( file_name=wsg50_sdf_path, model_name='gripper') # Weld the base of arm and gripper to reduce the number of states. X_EeGripper = RigidTransform( RollPitchYaw(np.pi / 2, 0, np.pi / 2), [0, 0, 0.081]) plant.WeldFrames(A=plant.world_frame(), B=plant.GetFrameByName("iiwa_link_0", iiwa_model)) plant.WeldFrames( A=plant.GetFrameByName("iiwa_link_7", iiwa_model), B=plant.GetFrameByName("body", gripper_model), X_AB=X_EeGripper) plant.Finalize() # Create a context of the MBP and set the state of the context # to desired values. context = plant.CreateDefaultContext() nq = plant.num_positions() nv = plant.num_velocities() nq_iiwa = 7 nv_iiwa = 7 nq_gripper = 2 nv_gripper = 2 q_iiwa_desired = np.zeros(nq_iiwa) v_iiwa_desired = np.zeros(nv_iiwa) q_gripper_desired = np.zeros(nq_gripper) v_gripper_desired = np.zeros(nv_gripper) q_iiwa_desired[2] = np.pi/3 q_gripper_desired[0] = 0.1 v_iiwa_desired[1] = 5.0 q_gripper_desired[0] = -0.3 x_iiwa_desired = np.zeros(nq_iiwa + nv_iiwa) x_iiwa_desired[0:nq_iiwa] = q_iiwa_desired x_iiwa_desired[nq_iiwa:nq_iiwa+nv_iiwa] = v_iiwa_desired x_gripper_desired = np.zeros(nq_gripper + nv_gripper) x_gripper_desired[0:nq_gripper] = q_gripper_desired x_gripper_desired[nq_gripper:nq_gripper+nv_gripper] = v_gripper_desired x_desired = np.zeros(nq + nv) x_desired[0:7] = q_iiwa_desired x_desired[7:9] = q_gripper_desired x_desired[nq:nq+7] = v_iiwa_desired x_desired[nq+7:nq+nv] = v_gripper_desired # Check SetPositionsAndVelocities() for each model instance. # Do the iiwa model first. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositionsAndVelocities(context, iiwa_model, x_iiwa_desired) self.assertTrue(np.allclose( plant.GetPositionsAndVelocities(context, iiwa_model), x_iiwa_desired)) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities( context, gripper_model), np.zeros(nq_gripper + nv_gripper))) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositionsAndVelocities( context, gripper_model, x_gripper_desired) self.assertTrue(np.allclose( plant.GetPositionsAndVelocities(context, gripper_model), x_gripper_desired)) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities( context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa))) # Check SetPositions() for each model instance. # Do the iiwa model first. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositions(context, iiwa_model, q_iiwa_desired) self.assertTrue(np.allclose( plant.GetPositions(context, iiwa_model), q_iiwa_desired)) self.assertTrue(np.allclose(plant.GetVelocities( context, iiwa_model), np.zeros(nv_iiwa))) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities( context, gripper_model), np.zeros(nq_gripper + nv_gripper))) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositions(context, gripper_model, q_gripper_desired) self.assertTrue(np.allclose( plant.GetPositions(context, gripper_model), q_gripper_desired)) self.assertTrue(np.allclose(plant.GetVelocities( context, gripper_model), np.zeros(nq_gripper))) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities( context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa))) # Check SetVelocities() for each model instance. # Do the iiwa model first. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetVelocities(context, iiwa_model, v_iiwa_desired) self.assertTrue(np.allclose( plant.GetVelocities(context, iiwa_model), v_iiwa_desired)) self.assertTrue(np.allclose(plant.GetPositions( context, iiwa_model), np.zeros(nq_iiwa))) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities( context, gripper_model), np.zeros(nq_gripper + nv_gripper))) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetVelocities(context, gripper_model, v_gripper_desired) self.assertTrue(np.allclose( plant.GetVelocities(context, gripper_model), v_gripper_desired)) self.assertTrue(np.allclose(plant.GetPositions( context, gripper_model), np.zeros(nv_gripper))) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities( context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa)))
def _convert_mesh(geom): # Given a LCM geometry message, forms a meshcat geometry and material # for that geometry, as well as a local tf to the meshcat geometry # that matches the LCM geometry. # For MESH geometry, this function checks if a texture file exists next # to the mesh file, and uses that to provide the material information if # present. For BOX, SPHERE, CYLINDER geometry as well as MESH geometry # not satisfying the above, this function uses the geom.color field to # create a flat color for the object. In the case of other geometry types, # both fields are returned as None. meshcat_geom = None material = None element_local_tf = RigidTransform( RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsMatrix4() if geom.type == geom.BOX: assert geom.num_float_data == 3 meshcat_geom = meshcat.geometry.Box(geom.float_data) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0]) elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 meshcat_geom = meshcat.geometry.Cylinder(geom.float_data[1], geom.float_data[0]) # In Drake, cylinders are along +z # In meshcat, cylinders are along +y # Rotate to fix this misalignment extra_rotation = tf.rotation_matrix(math.pi / 2., [1, 0, 0]) element_local_tf[0:3, 0:3] = (element_local_tf[0:3, 0:3].dot( extra_rotation[0:3, 0:3])) elif geom.type == geom.MESH: meshcat_geom = meshcat.geometry.ObjMeshGeometry.from_file( geom.string_data[0:-3] + "obj") # Handle scaling. # TODO(gizatt): See meshcat-python#40 for incorporating scale as a # field rather than a matrix multiplication. scale = geom.float_data[:3] element_local_tf[:3, :3] = element_local_tf[:3, :3].dot(np.diag(scale)) # Attempt to find a texture for the object by looking for an # identically-named *.png next to the model. # TODO(gizatt): Support .MTLs and prefer them over png, since they're # both more expressive and more standard. # TODO(gizatt): In the long term, this kind of material information # should be gleaned from the SceneGraph constituents themselves, so # that we visualize what the simulation is *actually* reasoning about # rather than what files happen to be present. candidate_texture_path_png = geom.string_data[0:-3] + "png" if os.path.exists(candidate_texture_path_png): material = meshcat.geometry.MeshLambertMaterial( map=meshcat.geometry.ImageTexture( image=meshcat.geometry.PngImage.from_file( candidate_texture_path_png))) else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(geom.type)) return meshcat_geom, material if material is None: def rgb_2_hex(rgb): # Turn a list of R,G,B elements (any indexable list # of >= 3 elements will work), where each element is # specified on range [0., 1.], into the equivalent # 24-bit value 0xRRGGBB. val = 0 for i in range(3): val += (256**(2 - i)) * int(255 * rgb[i]) return val material = meshcat.geometry.MeshLambertMaterial( color=rgb_2_hex(geom.color[:3]), transparent=geom.color[3] != 1., opacity=geom.color[3]) return meshcat_geom, material, element_local_tf
def test_model_instance_state_access_by_array(self): # Create a MultibodyPlant with a kuka arm and a schunk gripper. # the arm is welded to the world, the gripper is welded to the # arm's end effector. wsg50_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "wsg_50_description/sdf/schunk_wsg_50.sdf") iiwa_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") timestep = 0.0002 plant = MultibodyPlant(timestep) parser = Parser(plant) iiwa_model = parser.AddModelFromFile( file_name=iiwa_sdf_path, model_name='robot') gripper_model = parser.AddModelFromFile( file_name=wsg50_sdf_path, model_name='gripper') # Weld the base of arm and gripper to reduce the number of states. X_EeGripper = RigidTransform( RollPitchYaw(np.pi / 2, 0, np.pi / 2), [0, 0, 0.081]) plant.WeldFrames( A=plant.world_frame(), B=plant.GetFrameByName("iiwa_link_0", iiwa_model)) plant.WeldFrames( A=plant.GetFrameByName("iiwa_link_7", iiwa_model), B=plant.GetFrameByName("body", gripper_model), X_AB=X_EeGripper) plant.Finalize() # Create a context of the MBP and set the state of the context # to desired values. context = plant.CreateDefaultContext() nq = plant.num_positions() nq_iiwa = plant.num_positions(iiwa_model) nv = plant.num_velocities() nv_iiwa = plant.num_velocities(iiwa_model) q_iiwa_desired = np.linspace(0, 0.3, 7) v_iiwa_desired = q_iiwa_desired + 0.4 q_gripper_desired = [0.4, 0.5] v_gripper_desired = [-1., -2.] x_desired = np.zeros(nq + nv) x_desired[0:7] = q_iiwa_desired x_desired[7:9] = q_gripper_desired x_desired[nq:nq+7] = v_iiwa_desired x_desired[nq+7:nq+nv] = v_gripper_desired x = plant.GetMutablePositionsAndVelocities(context=context) x[:] = x_desired q = plant.GetPositions(context=context) v = plant.GetVelocities(context=context) # Get state from context. x = plant.GetPositionsAndVelocities(context=context) x_tmp = plant.GetMutablePositionsAndVelocities(context=context) self.assertTrue(np.allclose(x_desired, x_tmp)) # Get positions and velocities of specific model instances # from the position/velocity vector of the plant. q_iiwa = plant.GetPositions(context=context, model_instance=iiwa_model) q_iiwa_array = plant.GetPositionsFromArray( model_instance=iiwa_model, q=q) self.assertTrue(np.allclose(q_iiwa, q_iiwa_array)) q_gripper = plant.GetPositions( context=context, model_instance=gripper_model) v_iiwa = plant.GetVelocities( context=context, model_instance=iiwa_model) v_iiwa_array = plant.GetVelocitiesFromArray( model_instance=iiwa_model, v=v) self.assertTrue(np.allclose(v_iiwa, v_iiwa_array)) v_gripper = plant.GetVelocities( context=context, model_instance=gripper_model) # Assert that the `GetPositions` and `GetVelocities` return # the desired values set earlier. self.assertTrue(np.allclose(q_iiwa_desired, q_iiwa)) self.assertTrue(np.allclose(v_iiwa_desired, v_iiwa)) self.assertTrue(np.allclose(q_gripper_desired, q_gripper)) self.assertTrue(np.allclose(v_gripper_desired, v_gripper)) # Verify that SetPositionsInArray() and SetVelocitiesInArray() works. plant.SetPositionsInArray( model_instance=iiwa_model, q_instance=np.zeros(nq_iiwa), q=q) self.assertTrue(np.allclose( plant.GetPositionsFromArray(model_instance=iiwa_model, q=q), np.zeros(nq_iiwa))) plant.SetVelocitiesInArray( model_instance=iiwa_model, v_instance=np.zeros(nv_iiwa), v=v) self.assertTrue(np.allclose( plant.GetVelocitiesFromArray(model_instance=iiwa_model, v=v), np.zeros(nv_iiwa))) # Check actuation. nu = plant.num_actuated_dofs() u = np.zeros(nu) u_iiwa = np.arange(nv_iiwa) plant.SetActuationInArray( model_instance=iiwa_model, u_instance=u_iiwa, u=u) self.assertTrue(np.allclose(u[:7], u_iiwa))
def _xyz_rpy_deg(xyz, rpy_deg): return RigidTransform(RollPitchYaw(np.asarray(rpy_deg) * np.pi / 180), xyz)
def DoPublish(self, context, event): # TODO(SeanCurtis-TRI) We want to be able to use this visualizer to # draw without having it part of a Simulator. That means we'd like # vis.Publish(context) to work. Currently, pydrake offers no mechanism # to declare a forced event. However, by overriding DoPublish and # putting the forced event callback code in the override, we can # simulate it. # We need to bind a mechanism for declaring forced events so we don't # have to resort to overriding the dispatcher. LeafSystem.DoPublish(self, context, event) contact_results = self.EvalAbstractInput(context, 0).get_value() vis = self._meshcat_viz.vis[self._meshcat_viz.prefix]["contact_forces"] contacts = [] for i_contact in range(contact_results.num_point_pair_contacts()): contact_info = contact_results.point_pair_contact_info(i_contact) # Do not display small forces. force_norm = np.linalg.norm(contact_info.contact_force()) if force_norm < self._force_threshold: continue point_pair = contact_info.point_pair() key = (point_pair.id_A.get_value(), point_pair.id_B.get_value()) cvis = vis[str(key)] contacts.append(key) arrow_height = self._radius*2.0 if key not in self._published_contacts: # New key, so create the geometry. Note: the height of the # cylinder is 2 and gets scaled to twice the contact force # length, because I am drawing both (equal and opposite) # forces. Note also that meshcat (following three.js) puts # the height of the cylinder along the y axis. cvis["cylinder"].set_object(meshcat.geometry.Cylinder( height=2.0, radius=self._radius), meshcat.geometry.MeshLambertMaterial(color=0x33cc33)) cvis["head"].set_object(meshcat.geometry.Cylinder( height=arrow_height, radiusTop=0, radiusBottom=self._radius*2.0), meshcat.geometry.MeshLambertMaterial(color=0x00dd00)) cvis["tail"].set_object(meshcat.geometry.Cylinder( height=arrow_height, radiusTop=self._radius*2.0, radiusBottom=0), meshcat.geometry.MeshLambertMaterial(color=0x00dd00)) height = force_norm/self._contact_force_scale cvis["cylinder"].set_transform(tf.scale_matrix( height, direction=[0, 1, 0])) cvis["head"].set_transform(tf.translation_matrix( [0, height + arrow_height/2.0, 0.0])) cvis["tail"].set_transform(tf.translation_matrix( [0, -height - arrow_height/2.0, 0.0])) # The contact frame's origin is located at contact_point and is # oriented such that Cy is aligned with the contact force. p_WC = contact_info.contact_point() # documented as in world. if force_norm < 1e-6: # We cannot rely on self._force_threshold to determine if the # force can be normalized; that threshold can be zero. R_WC = RotationMatrix() else: fhat_C_W = contact_info.contact_force() / force_norm R_WC = RotationMatrix.MakeFromOneVector(b_A=fhat_C_W, axis_index=1) X_WC = RigidTransform(R=R_WC, p=p_WC) cvis.set_transform(X_WC.GetAsMatrix4()) # We only delete any contact vectors that did not persist into this # publish. It is tempting to just delete() the root branch at the # beginning of this publish, but this leads to visual artifacts # (flickering) in the browser. for key in set(self._published_contacts) - set(contacts): vis[str(key)].delete() self._published_contacts = contacts
def test_rgbd_sensor(self): def check_ports(system): self.assertIsInstance(system.query_object_input_port(), InputPort) self.assertIsInstance(system.color_image_output_port(), OutputPort) self.assertIsInstance(system.depth_image_32F_output_port(), OutputPort) self.assertIsInstance(system.depth_image_16U_output_port(), OutputPort) self.assertIsInstance(system.label_image_output_port(), OutputPort) # Use HDTV size. width = 1280 height = 720 color_properties = CameraProperties(width=width, height=height, fov_y=np.pi / 6, renderer_name="renderer") depth_properties = DepthCameraProperties(width=width, height=height, fov_y=np.pi / 6, renderer_name="renderer", z_near=0.1, z_far=5.5) # Put it at the origin. X_WB = RigidTransform() # This id would fail if we tried to render; no such id exists. parent_id = FrameId.get_new_id() camera_poses = mut.RgbdSensor.CameraPoses(X_BC=RigidTransform(), X_BD=RigidTransform()) sensor = mut.RgbdSensor(parent_id=parent_id, X_PB=X_WB, color_properties=color_properties, depth_properties=depth_properties, camera_poses=camera_poses, show_window=False) def check_info(camera_info): self.assertIsInstance(camera_info, mut.CameraInfo) self.assertEqual(camera_info.width(), width) self.assertEqual(camera_info.height(), height) check_info(sensor.color_camera_info()) check_info(sensor.depth_camera_info()) self.assertIsInstance(sensor.X_BC(), RigidTransform) self.assertIsInstance(sensor.X_BD(), RigidTransform) self.assertEqual(sensor.parent_frame_id(), parent_id) check_ports(sensor) # Test discrete camera, reconstructing using single-properties # constructor. color_and_depth_properties = depth_properties sensor = mut.RgbdSensor(parent_id=parent_id, X_PB=X_WB, properties=color_and_depth_properties, camera_poses=camera_poses, show_window=False) period = mut.RgbdSensorDiscrete.kDefaultPeriod discrete = mut.RgbdSensorDiscrete(sensor=sensor, period=period, render_label_image=True) self.assertTrue(discrete.sensor() is sensor) self.assertEqual(discrete.period(), period) check_ports(discrete) # That we can access the state as images. context = discrete.CreateDefaultContext() values = context.get_abstract_state() self.assertIsInstance(values.get_value(0), Value[mut.ImageRgba8U]) self.assertIsInstance(values.get_value(1), Value[mut.ImageDepth32F]) self.assertIsInstance(values.get_value(2), Value[mut.ImageDepth16U]) self.assertIsInstance(values.get_value(3), Value[mut.ImageLabel16I])
def __init__(self, scene_graph, zmq_url="default", draw_period=0.033333, camera_tfs=[RigidTransform()], material_overrides=[], global_transform=RigidTransform(), out_prefix=None, show_figure=False): """ Args: scene_graph: A SceneGraph object. draw_period: The rate at which this class publishes rendered images. zmq_url: Optionally set a url to connect to the blender server. Use zmp_url="default" to the value obtained by running a single blender server in another terminal. TODO(gizatt): Use zmp_url=None or zmq_url="new" to start a new server (as a child of this process); a new web browser will be opened (the url will also be printed to the console). Use e.g. zmq_url="tcp://127.0.0.1:5556" to specify a specific address. camera tfs: List of RigidTransform camera tfs. material_overrides: A list of tuples of regex rules and material override arguments to be passed into a a register material call, not including names. (Those are assigned automatically by this class.) global transform: RigidTransform that gets premultiplied to every object. Note: This call will not return until it connects to the Blender server. """ LeafSystem.__init__(self) if out_prefix is not None: self.out_prefix = out_prefix else: self.out_prefix = "/tmp/drake_blender_vis_" self.current_publish_num = 0 self.set_name('blender_color_camera') self.DeclarePeriodicPublish(draw_period, 0.0) self.draw_period = draw_period # Pose bundle (from SceneGraph) input port. self.DeclareAbstractInputPort("lcm_visualization", AbstractValue.Make(PoseBundle(0))) # Optional pose bundle of bounding boxes. self.DeclareAbstractInputPort("bounding_box_bundle", AbstractValue.Make(BoundingBoxBundle(0))) if zmq_url == "default": zmq_url = "tcp://127.0.0.1:5556" elif zmq_url == "new": raise NotImplementedError("TODO") if zmq_url is not None: print("Connecting to blender server at zmq_url=" + zmq_url + "...") self.bsi = BlenderServerInterface(zmq_url=zmq_url) print("Connected to Blender server.") self._scene_graph = scene_graph # Compile regex for the material overrides self.material_overrides = [(re.compile(x[0]), x[1]) for x in material_overrides] self.global_transform = global_transform self.camera_tfs = camera_tfs def on_initialize(context, event): self.load() self.DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize)) self.show_figure = show_figure if self.show_figure: plt.figure()