def SetPose(self, pose): """ @param pose is an Isometry3. """ tf = RigidTransform(pose) self.SetRPY(RollPitchYaw(tf.rotation())) self.SetXYZ(pose.translation())
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']) parser.add_argument( "-w", "--open-window", dest="browser_new", action="store_const", const=1, default=None, help="Open the MeshCat display in a new browser window.") args = parser.parse_args() builder = DiagramBuilder() # NOTE: the meshcat instance is always created in order to create the # teleop controls (joint sliders and open/close gripper button). When # args.hardware is True, the meshcat server will *not* display robot # geometry, but it will contain the joint sliders and open/close gripper # button in the "Open Controls" tab in the top-right of the viewing server. meshcat = Meshcat() 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() geometry_query_port = station.GetOutputPort("geometry_query") DrakeVisualizer.AddToBuilder(builder, geometry_query_port) meshcat_visualizer = MeshcatVisualizerCpp.AddToBuilder( builder=builder, query_object_port=geometry_query_port, meshcat=meshcat) if args.setup == 'planar': meshcat.Set2dRenderMode() pyplot_visualizer = ConnectPlanarSceneGraphVisualizer( builder, station.get_scene_graph(), geometry_query_port) if args.browser_new is not None: url = meshcat.web_url() webbrowser.open(url=url, new=args.browser_new) teleop = builder.AddSystem( JointSliders(meshcat=meshcat, plant=station.get_controller_plant())) 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(meshcat=meshcat)) 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(VectorLogSink(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.SetPositions(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: iiwa_velocities_log = iiwa_velocities.FindLog(simulator.get_context()) for time, qdot in zip(iiwa_velocities_log.sample_times(), iiwa_velocities_log.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 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 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_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) 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()) # 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)
def _xyz_rpy_deg(xyz, rpy_deg): return RigidTransform(RollPitchYaw(np.asarray(rpy_deg) * np.pi / 180), xyz)
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 = {} mock_lcm = DrakeMockLcm() mock_lcm_subscriber = Subscriber(lcm=mock_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, 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 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() is not ".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_exploding_iiwa_sim(self): """ Shows a simulation of a falling + exploding IIWA which "changes topology" by being rebuilt without joints. """ builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-3) iiwa_file = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/urdf/" "iiwa14_spheres_dense_elbow_collision.urdf") Parser(plant).AddModelFromFile(iiwa_file, "iiwa") # Add ground plane. X_FH = HalfSpace.MakePose([0, 0, 1], [0, 0, 0]) plant.RegisterCollisionGeometry(plant.world_body(), X_FH, HalfSpace(), "ground_plane_collision", CoulombFriction(0.8, 0.3)) plant.Finalize() # Loosey-goosey - no control. for model in me.get_model_instances(plant): util.build_with_no_control(builder, plant, model) if VISUALIZE: print("test_exploding_iiwa_sim") role = Role.kIllustration DrakeVisualizer.AddToBuilder( builder, scene_graph, params=DrakeVisualizerParams(role=role)) ConnectContactResultsToDrakeVisualizer(builder, plant, scene_graph) diagram = builder.Build() # Set up context. d_context = diagram.CreateDefaultContext() context = plant.GetMyContextFromRoot(d_context) # - Hoist IIWA up in the air. plant.SetFreeBodyPose(context, plant.GetBodyByName("base"), RigidTransform([0, 0, 1.])) # - Set joint velocities to "spin" it in the air. for joint in me.get_joints(plant): if isinstance(joint, RevoluteJoint): me.set_joint_positions(plant, context, joint, 0.7) me.set_joint_velocities(plant, context, joint, -5.) def monitor(d_context): # Stop the simulation once there's any contact. context = plant.GetMyContextFromRoot(d_context) query_object = plant.get_geometry_query_input_port().Eval(context) if query_object.HasCollisions(): return EventStatus.ReachedTermination(plant, "Contact") else: return EventStatus.DidNothing() # Forward simulate. simulator = Simulator(diagram, d_context) simulator.Initialize() simulator.set_monitor(monitor) if VISUALIZE: simulator.set_target_realtime_rate(1.) simulator.AdvanceTo(2.) # Try to push a bit further. simulator.clear_monitor() simulator.AdvanceTo(d_context.get_time() + 0.05) diagram.Publish(d_context) # Recreate simulator. builder_new = DiagramBuilder() plant_new, scene_graph_new = AddMultibodyPlantSceneGraph( builder_new, time_step=plant.time_step()) subgraph = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(plant, scene_graph)) # Remove all joints; make them floating bodies. for joint in me.get_joints(plant): subgraph.remove_joint(joint) # Remove massless bodies. # For more info, see: https://stackoverflow.com/a/62035705/7829525 for body in me.get_bodies(plant): if body is plant.world_body(): continue if body.default_mass() == 0.: subgraph.remove_body(body) # Finalize. to_new = subgraph.add_to(plant_new, scene_graph_new) plant_new.Finalize() if VISUALIZE: DrakeVisualizer.AddToBuilder( builder_new, scene_graph_new, params=DrakeVisualizerParams(role=role)) ConnectContactResultsToDrakeVisualizer(builder_new, plant_new, scene_graph_new) diagram_new = builder_new.Build() # Remap state. d_context_new = diagram_new.CreateDefaultContext() d_context_new.SetTime(d_context.get_time()) context_new = plant_new.GetMyContextFromRoot(d_context_new) to_new.copy_state(context, context_new) # Simulate. simulator_new = Simulator(diagram_new, d_context_new) simulator_new.Initialize() diagram_new.Publish(d_context_new) if VISUALIZE: simulator_new.set_target_realtime_rate(1.) simulator_new.AdvanceTo(context_new.get_time() + 2) if VISUALIZE: input(" Press enter...")
def main(): goal_position = np.array([0.5, 0., 0.025]) blue_box_clean_position = [0.4, 0., 0.05] red_box_clean_position = [0.6, 0., 0.05] goal_delta = 0.05 parser = argparse.ArgumentParser(description=__doc__) MeshcatVisualizer.add_argparse_argument(parser) parser.add_argument('--use_meshcat', action='store_true', help="Must be set for meshcat to be used.") parser.add_argument('--disable_planar_viz', action='store_true', help="Don't create a planar visualizer. Probably" " breaks something that assumes the planar" " vis exists.") parser.add_argument('--teleop', action='store_true', help="Enable teleop, so *don't* use the state machine" " and motion primitives.") args = parser.parse_args() builder = DiagramBuilder() # Set up the ManipulationStation station = builder.AddSystem(ManipulationStation(0.001)) mbp = station.get_multibody_plant() station.SetupManipulationClassStation() add_goal_region_visual_geometry(mbp, goal_position, goal_delta) add_box_at_location(mbp, name="blue_box", color=[0.25, 0.25, 1., 1.], pose=RigidTransform(p=[0.4, 0.0, 0.025])) add_box_at_location(mbp, name="red_box", color=[1., 0.25, 0.25, 1.], pose=RigidTransform(p=[0.6, 0.0, 0.025])) station.Finalize() iiwa_q0 = np.array([0.0, 0.6, 0.0, -1.75, 0., 1., np.pi / 2.]) # Attach a visualizer. if args.use_meshcat: meshcat = builder.AddSystem( MeshcatVisualizer(station.get_scene_graph(), zmq_url=args.meshcat)) builder.Connect(station.GetOutputPort("pose_bundle"), meshcat.get_input_port(0)) if not args.disable_planar_viz: plt.gca().clear() viz = builder.AddSystem( PlanarSceneGraphVisualizer(station.get_scene_graph(), xlim=[0.25, 0.8], ylim=[-0.1, 0.5], ax=plt.gca())) builder.Connect(station.GetOutputPort("pose_bundle"), viz.get_input_port(0)) plt.draw() # Hook up DifferentialIK, since both control modes use it. 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 = 1.0 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)) differential_ik.set_name("Differential IK") differential_ik.parameters.set_nominal_joint_position(iiwa_q0) builder.Connect(differential_ik.GetOutputPort("joint_position_desired"), station.GetInputPort("iiwa_position")) if not args.teleop: symbol_list = [ SymbolL2Close("blue_box_in_goal", "blue_box", goal_position, goal_delta), SymbolL2Close("red_box_in_goal", "red_box", goal_position, goal_delta), SymbolRelativePositionL2("blue_box_on_red_box", "blue_box", "red_box", l2_thresh=0.01, offset=np.array([0., 0., 0.05])), SymbolRelativePositionL2("red_box_on_blue_box", "red_box", "blue_box", l2_thresh=0.01, offset=np.array([0., 0., 0.05])), ] primitive_list = [ MoveBoxPrimitive("put_blue_box_in_goal", mbp, "blue_box", goal_position), MoveBoxPrimitive("put_red_box_in_goal", mbp, "red_box", goal_position), MoveBoxPrimitive("put_blue_box_away", mbp, "blue_box", blue_box_clean_position), MoveBoxPrimitive("put_red_box_away", mbp, "red_box", red_box_clean_position), MoveBoxPrimitive("put_red_box_on_blue_box", mbp, "red_box", np.array([0., 0., 0.05]), "blue_box"), MoveBoxPrimitive("put_blue_box_on_red_box", mbp, "blue_box", np.array([0., 0., 0.05]), "red_box"), ] task_execution_system = builder.AddSystem( TaskExectionSystem( mbp, symbol_list=symbol_list, primitive_list=primitive_list, dfa_json_file="specifications/red_and_blue_boxes_stacking.json" )) builder.Connect(station.GetOutputPort("plant_continuous_state"), task_execution_system.GetInputPort("mbp_state_vector")) builder.Connect(task_execution_system.get_output_port(0), differential_ik.GetInputPort("rpy_xyz_desired")) builder.Connect(task_execution_system.get_output_port(1), station.GetInputPort("wsg_position")) #movebox = MoveBoxPrimitive("test_move_box", mbp, "red_box", goal_position) #rpy_xyz_trajectory, gripper_traj = movebox.generate_rpyxyz_and_gripper_trajectory(mbp.CreateDefaultContext()) #rpy_xyz_trajectory_source = builder.AddSystem(TrajectorySource(rpy_xyz_trajectory)) #builder.Connect(rpy_xyz_trajectory_source.get_output_port(0), # differential_ik.GetInputPort("rpy_xyz_desired")) #wsg_position_source = builder.AddSystem(TrajectorySource(gripper_traj)) #builder.Connect(wsg_position_source.get_output_port(0), # station.GetInputPort("wsg_position")) # Target zero feedforward residual torque at all times. fft = builder.AddSystem(ConstantVectorSource(np.zeros(7))) builder.Connect(fft.get_output_port(0), station.GetInputPort("iiwa_feedforward_torque")) input_force_fix = builder.AddSystem(ConstantVectorSource([40.0])) builder.Connect(input_force_fix.get_output_port(0), station.GetInputPort("wsg_force_limit")) end_time = 10000 else: # Set up teleoperation. # Hook up a pygame-based keyboard+mouse interface for # teleoperation, and low pass its output to drive the EE target # for the differential IK. print_instructions() teleop = builder.AddSystem(MouseKeyboardTeleop(grab_focus=True)) filter_ = builder.AddSystem( FirstOrderLowPassFilter(time_constant=0.005, 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")) # Target zero feedforward residual torque at all times. fft = builder.AddSystem(ConstantVectorSource(np.zeros(7))) builder.Connect(fft.get_output_port(0), station.GetInputPort("iiwa_feedforward_torque")) # Simulate functionally forever. end_time = 10000 # Create symbol log #symbol_log = SymbolFromTransformLog( # [SymbolL2Close('at_goal', 'red_box', goal_position, .025), # SymbolL2Close('at_goal', 'blue_box', goal_position, .025)]) # #symbol_logger_system = builder.AddSystem( # SymbolLoggerSystem( # station.get_multibody_plant(), symbol_logger=symbol_log)) #builder.Connect( # station.GetOutputPort("plant_continuous_state"), # symbol_logger_system.GetInputPort("mbp_state_vector")) # Remaining input ports need to be tied up. diagram = builder.Build() g = pydot.graph_from_dot_data(diagram.GetGraphvizString())[0] g.write_png("system_diagram.png") diagram_context = diagram.CreateDefaultContext() station_context = diagram.GetMutableSubsystemContext( station, diagram_context) station.SetIiwaPosition(station_context, iiwa_q0) differential_ik.SetPositions( diagram.GetMutableSubsystemContext(differential_ik, diagram_context), iiwa_q0) if args.teleop: teleop.SetPose(differential_ik.ForwardKinematics(iiwa_q0)) filter_.set_initial_output_value( diagram.GetMutableSubsystemContext(filter_, diagram_context), teleop.get_output_port(0).Eval( diagram.GetMutableSubsystemContext(teleop, diagram_context))) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(end_time)
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 # 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_deprecated(parent_id, X_PB): # One deprecation warning for CameraPoses and one for RgbdSensor. with catch_drake_warnings(expected_count=4): 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) camera_poses = mut.RgbdSensor.CameraPoses( X_BC=RigidTransform(), X_BD=RigidTransform()) return mut.RgbdSensor(parent_id=parent_id, X_PB=X_PB, color_properties=color_properties, depth_properties=depth_properties, camera_poses=camera_poses, show_window=False) 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_deprecated(parent_id, X_PB): with catch_drake_warnings(expected_count=3): depth_properties = DepthCameraProperties( width=width, height=height, fov_y=np.pi / 6, renderer_name="renderer", z_near=0.1, z_far=5.5) return mut.RgbdSensor( parent_id=parent_id, X_PB=X_PB, properties=depth_properties, camera_poses=mut.RgbdSensor.CameraPoses(), show_window=False) 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_deprecated, construct, construct_single_deprecated, 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 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 plant, scene_graph, diagram, boxes, ground_geometry_id,\ boxes_geometry_id
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.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 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( "--time_step", type=float, default=0.005, help="Time constant for the differential IK solver and 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']) parser.add_argument( '--schunk_collision_model', type=str, default='box', help="The Schunk collision model to use for simulation. ", choices=['box', 'box_plus_fingertip_spheres']) MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() if args.test: # Don't grab mouse focus during testing. # See: https://stackoverflow.com/a/52528832/7829525 os.environ["SDL_VIDEODRIVER"] = "dummy" builder = DiagramBuilder() if args.hardware: station = builder.AddSystem(ManipulationStationHardwareInterface()) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) if args.schunk_collision_model == "box": schunk_model = SchunkCollisionModel.kBox elif args.schunk_collision_model == "box_plus_fingertip_spheres": schunk_model = SchunkCollisionModel.kBoxPlusFingertipSpheres # Initializes the chosen station type. if args.setup == 'manipulation_class': station.SetupManipulationClassStation( schunk_model=schunk_model) 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( schunk_model=schunk_model) ycb_objects = CreateClutterClearingYcbObjectList() for model_file, X_WObject in ycb_objects: station.AddManipulandFromFile(model_file, X_WObject) station.Finalize() DrakeVisualizer.AddToBuilder(builder, station.GetOutputPort("query_object")) if args.meshcat: meshcat = ConnectMeshcatVisualizer( builder, output_port=station.GetOutputPort("geometry_query"), zmq_url=args.meshcat, open_browser=args.open_browser) if args.setup == 'planar': meshcat.set_planar_viewpoint() robot = station.get_controller_plant() params = DifferentialInverseKinematicsParameters(robot.num_positions(), robot.num_velocities()) params.set_timestep(args.time_step) # True velocity limits for the IIWA14 (in rad/s, 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, args.time_step)) builder.Connect(differential_ik.GetOutputPort("joint_position_desired"), station.GetInputPort("iiwa_position")) teleop = builder.AddSystem(DualShock4Teleop(initialize_joystick())) filter_ = builder.AddSystem( FirstOrderLowPassFilter(time_constant=args.time_step, 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)) # 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) 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 plot_mathematical_program(meshcat, path, prog, X, Y, result=None, point_size=0.05): assert prog.num_vars() == 2 assert X.size == Y.size N = X.size values = np.vstack((X.reshape(-1), Y.reshape(-1))) costs = prog.GetAllCosts() # Vectorized multiply for the quadratic form. # Z = (D*np.matmul(Q,D)).sum(0).reshape(nx, ny) if costs: Z = prog.EvalBindingVectorized(costs[0], values) for b in costs[1:]: Z = Z + prog.EvalBindingVectorized(b, values) cv = f"{path}/constraints" for binding in prog.GetAllConstraints(): if isinstance(binding.evaluator(), BoundingBoxConstraint): c = binding.evaluator() var_indices = [ int(prog.decision_variable_index()[v.get_id()]) for v in binding.variables() ] satisfied = np.array( c.CheckSatisfiedVectorized(values[var_indices, :], 0.001)).reshape(1, -1) if costs: Z[~satisfied] = np.nan v = f"{cv}/{type(c).__name__}" Zc = np.zeros(Z.shape) Zc[satisfied] = np.nan plot_surface(meshcat, v, X, Y, Zc.reshape(X.shape), rgba=Rgba(1.0, .2, .2, 1.0), wireframe=True) else: Zc = prog.EvalBindingVectorized(binding, values) evaluator = binding.evaluator() low = evaluator.lower_bound() up = evaluator.upper_bound() cvb = f"{cv}/{type(evaluator).__name__}" for index in range(Zc.shape[0]): # TODO(russt): Plot infeasible points in a different color. infeasible = np.logical_or(Zc[index, :] < low[index], Zc[index, :] > up[index]) plot_surface(meshcat, f"{cvb}/{index}", X, Y, Zc[index, :].reshape(X.shape), rgba=Rgba(1.0, .3, 1.0, 1.0), wireframe=True) if costs: plot_surface(meshcat, f"{path}/objective", X, Y, Z.reshape(X.shape), rgba=Rgba(.3, 1.0, .3, 1.0), wireframe=True) if result: v = f"{path}/solution" meshcat.SetObject(v, Sphere(point_size), Rgba(.3, 1.0, .3, 1.0)) x_solution = result.get_x_val() meshcat.SetTransform( v, RigidTransform( [x_solution[0], x_solution[1], result.get_optimal_cost()]))
def _get_transform(self): return RigidTransform( RollPitchYaw(self._value[0], self._value[1], self._value[2]), self._value[3:])
def test_composition_gripper_workflow(self): """Tests subgraphs (pre-finalize) for composition, with a scene graph, welding bodies together across different subgraphs.""" # N.B. The frame-welding is done so that we can easily set the # positions of the IIWA / WSG without having to worry about / work # around the floating body coordinates. # Create IIWA. iiwa_builder = DiagramBuilder() iiwa_plant, iiwa_scene_graph = AddMultibodyPlantSceneGraph( iiwa_builder, time_step=0.) iiwa_file = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/urdf/" "iiwa14_spheres_dense_elbow_collision.urdf") iiwa_model = Parser(iiwa_plant).AddModelFromFile(iiwa_file, "iiwa") iiwa_plant.WeldFrames(iiwa_plant.world_frame(), iiwa_plant.GetFrameByName("base")) iiwa_plant.Finalize() if VISUALIZE: print("test_composition") DrakeVisualizer.AddToBuilder(iiwa_builder, iiwa_scene_graph) iiwa_diagram = iiwa_builder.Build() iiwa_subgraph = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(iiwa_plant, iiwa_scene_graph)) self.assertIsInstance(iiwa_subgraph, mut.MultibodyPlantSubgraph) iiwa_subgraph.remove_body(iiwa_plant.world_body()) # Create WSG. wsg_builder = DiagramBuilder() wsg_plant, wsg_scene_graph = AddMultibodyPlantSceneGraph(wsg_builder, time_step=0.) wsg_file = FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/sdf/" "schunk_wsg_50.sdf") wsg_model = Parser(wsg_plant).AddModelFromFile(wsg_file, "gripper_model") wsg_plant.WeldFrames(wsg_plant.world_frame(), wsg_plant.GetFrameByName("__model__")) wsg_plant.Finalize() if VISUALIZE: DrakeVisualizer.AddToBuilder(wsg_builder, wsg_scene_graph) wsg_diagram = wsg_builder.Build() wsg_subgraph = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(wsg_plant, wsg_scene_graph)) self.assertIsInstance(wsg_subgraph, mut.MultibodyPlantSubgraph) wsg_subgraph.remove_body(wsg_plant.world_body()) builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-3) iiwa_to_plant = iiwa_subgraph.add_to(plant, scene_graph) self.assertIsInstance(iiwa_to_plant, mut.MultibodyPlantElementsMap) wsg_to_plant = wsg_subgraph.add_to(plant, scene_graph) self.assertIsInstance(wsg_to_plant, mut.MultibodyPlantElementsMap) if VISUALIZE: DrakeVisualizer.AddToBuilder(builder, scene_graph) rpy_deg = np.array([90., 0., 90]) X_7G = RigidTransform(p=[0, 0, 0.114], rpy=RollPitchYaw(rpy_deg * np.pi / 180)) frame_7 = plant.GetFrameByName("iiwa_link_7") frame_G = plant.GetFrameByName("body") plant.WeldFrames(frame_7, frame_G, X_7G) plant.Finalize() q_iiwa = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7] q_wsg = [-0.03, 0.03] iiwa_d_context = iiwa_diagram.CreateDefaultContext() iiwa_context = iiwa_plant.GetMyContextFromRoot(iiwa_d_context) iiwa_plant.SetPositions(iiwa_context, iiwa_model, q_iiwa) wsg_d_context = wsg_diagram.CreateDefaultContext() wsg_context = wsg_plant.GetMyContextFromRoot(wsg_d_context) wsg_plant.SetPositions(wsg_context, wsg_model, q_wsg) # Transfer state and briefly compare. diagram = builder.Build() d_context = diagram.CreateDefaultContext() context = plant.GetMyContextFromRoot(d_context) iiwa_to_plant.copy_state(iiwa_context, context) wsg_to_plant.copy_state(wsg_context, context) # Compare frames from sub-plants. util.compare_frame_poses(plant, context, iiwa_plant, iiwa_context, "base", "iiwa_link_7") util.compare_frame_poses(plant, context, wsg_plant, wsg_context, "body", "left_finger") # Visualize. if VISUALIZE: print(" Visualize IIWA") Simulator(iiwa_diagram, iiwa_d_context.Clone()).Initialize() input(" Press enter...") print(" Visualize WSG") Simulator(wsg_diagram, wsg_d_context.Clone()).Initialize() input(" Press enter...") print(" Visualize Composite") Simulator(diagram, d_context.Clone()).Initialize() input(" Press enter...")
def _write_pose_msg(X_AB, p, q): # p - position message # q - quaternion message X_AB = RigidTransform(X_AB) p.x, p.y, p.z = X_AB.translation() q.w, q.x, q.y, q.z = X_AB.rotation().ToQuaternion().wxyz()
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 _read_pose_msg(p, q): # p - position message # q - quaternion message return RigidTransform(Quaternion(wxyz=[q.w, q.x, q.y, q.z]), [p.x, p.y, p.z])
def __init__(self, scene_graph, draw_period=1. / 30, T_VW=np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]), xlim=[-1., 1], ylim=[-1, 1], facecolor=[1, 1, 1], use_random_colors=False, substitute_collocated_mesh_files=True, ax=None, show=None): """ Args: scene_graph: A SceneGraph object. draw_period: The rate at which this class publishes to the visualizer. T_VW: The view projection matrix from world to view coordinates. xlim: View limit into the scene. ylim: View limit into the scene. facecolor: Passed through to figure() and sets background color. Both color name strings and RGB triplets are allowed. Defaults to white. use_random_colors: If set to True, will render each body with a different color. (Multiple visual elements on the same body will be the same color.) substitute_collocated_mesh_files: If True, then a mesh file specified with an unsupported filename extension may be replaced by a file of the same base name in the same directory, but with a supported filename extension. Currently only .obj files are supported. ax: If supplied, the visualizer will draw onto those axes instead of creating a new set of axes. The visualizer will still change the view range and figure size of those axes. show: Opens a window during initialization / publish iff True. Default is None, which implies show=True unless matplotlib.get_backend() is 'template'. """ default_size = matplotlib.rcParams['figure.figsize'] scalefactor = (ylim[1] - ylim[0]) / (xlim[1] - xlim[0]) figsize = (default_size[0], default_size[0] * scalefactor) PyPlotVisualizer.__init__(self, facecolor=facecolor, figsize=figsize, ax=ax, draw_period=draw_period, show=show) self.set_name('planar_multibody_visualizer') self._scene_graph = scene_graph self._T_VW = T_VW # Pose bundle (from SceneGraph) input port. # TODO(tehbelinda): Rename the `lcm_visualization` port to match # SceneGraph once its output port has been updated. See #12214. self._pose_bundle_port = self.DeclareAbstractInputPort( "lcm_visualization", AbstractValue.Make(PoseBundle(0))) self.ax.axis('equal') self.ax.axis('off') # Achieve the desired view limits. self.ax.set_xlim(xlim) self.ax.set_ylim(ylim) default_size = self.fig.get_size_inches() self.fig.set_size_inches(figsize[0], figsize[1]) # Populate body patches. self._build_body_patches(use_random_colors, substitute_collocated_mesh_files) # Populate the body fill list -- which requires doing most of a draw # pass, but with an ax.fill() command to initialize the draw patches. # After initialization, we can then use in-place replacement of vertex # positions. The body fill list stores the ax patch objects in the # order they were spawned (i.e. by body, and then by order of view_ # patches). Drawing the tree should update them by iterating over # bodies and patches in the same order. self._body_fill_dict = {} X_WB_initial = RigidTransform.Identity() for full_name in self._patch_Blist.keys(): patch_Wlist, view_colors = self._get_view_patches( full_name, X_WB_initial) self._body_fill_dict[full_name] = [] for patch_W, color in zip(patch_Wlist, view_colors): # Project the full patch the first time, to initialize a vertex # list with enough space for any possible convex hull of this # vertex set. patch_V = self._project_patch(patch_W) body_fill = self.ax.fill(patch_V[0, :], patch_V[1, :], zorder=0, edgecolor='k', facecolor=color, closed=True)[0] self._body_fill_dict[full_name].append(body_fill) # Then update the vertices for a more accurate initial draw. self._update_body_fill_verts(body_fill, patch_V)
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.1, 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', 'planar']) parser.add_argument( '--schunk_collision_model', type=str, default='box', help="The Schunk collision model to use for simulation. ", choices=['box', 'box_plus_fingertip_spheres']) MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() builder = DiagramBuilder() if args.hardware: station = builder.AddSystem(ManipulationStationHardwareInterface()) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) if args.schunk_collision_model == "box": schunk_model = SchunkCollisionModel.kBox elif args.schunk_collision_model == "box_plus_fingertip_spheres": schunk_model = SchunkCollisionModel.kBoxPlusFingertipSpheres # Initializes the chosen station type. if args.setup == 'manipulation_class': station.SetupManipulationClassStation( schunk_model=schunk_model) 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( schunk_model=schunk_model) ycb_objects = CreateClutterClearingYcbObjectList() for model_file, X_WObject in ycb_objects: station.AddManipulandFromFile(model_file, X_WObject) elif args.setup == 'planar': station.SetupPlanarIiwaStation( schunk_model=schunk_model) station.AddManipulandFromFile( "drake/examples/manipulation_station/models/" + "061_foam_brick.sdf", RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) station.Finalize() # If using meshcat, don't render the cameras, since RgbdCamera # rendering only works with drake-visualizer. Without this check, # running this code in a docker container produces libGL errors. if args.meshcat: meshcat = ConnectMeshcatVisualizer( builder, output_port=station.GetOutputPort("geometry_query"), zmq_url=args.meshcat, open_browser=args.open_browser) if args.setup == 'planar': meshcat.set_planar_viewpoint() elif args.setup == 'planar': pyplot_visualizer = builder.AddSystem(PlanarSceneGraphVisualizer( station.get_scene_graph())) builder.Connect(station.GetOutputPort("pose_bundle"), pyplot_visualizer.get_input_port(0)) else: DrakeVisualizer.AddToBuilder(builder, station.GetOutputPort("query_object")) image_to_lcm_image_array = builder.AddSystem( ImageToLcmImageArrayT()) image_to_lcm_image_array.set_name("converter") for name in station.get_camera_names(): cam_port = ( image_to_lcm_image_array .DeclareImageInputPort[PixelType.kRgba8U]( "camera_" + name)) builder.Connect( station.GetOutputPort("camera_" + name + "_rgb_image"), cam_port) image_array_lcm_publisher = builder.AddSystem( LcmPublisherSystem.Make( channel="DRAKE_RGBD_CAMERA_IMAGES", lcm_type=image_array_t, lcm=None, publish_period=0.1, use_cpp_serializer=True)) image_array_lcm_publisher.set_name("rgbd_publisher") builder.Connect( image_to_lcm_image_array.image_array_t_msg_output_port(), image_array_lcm_publisher.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]) if args.setup == 'planar': # Extract the 3 joints that are not welded in the planar version. iiwa14_velocity_limits = iiwa14_velocity_limits[1:6:2] # The below constant is in body frame. params.set_end_effector_velocity_gain([1, 0, 0, 0, 1, 1]) # 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(EndEffectorTeleop(args.setup == 'planar')) if args.test: teleop.window.withdraw() # Don't display the window when testing. 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")) 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. num_iiwa_joints = station.num_iiwa_joints() 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) 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) 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 __init__(self, config=None): if config is None: config_path = os.path.join(os.path.dirname(__file__), "config.yaml") config = yaml.safe_load(open(config_path, 'r')) self._config = config self._step_dt = config["step_dt"] self._model_name = config["model_name"] self._sim_diagram = DrakeSimDiagram(config) mbp = self._sim_diagram.mbp builder = self._sim_diagram.builder # === Add table ===== dims = config["table"]["size"] color = np.array(config["table"]["color"]) friction_params = config["table"]["coulomb_friction"] box_shape = Box(*dims) X_W_T = RigidTransform(p=np.array([0., 0., -dims[2] / 2.])) # This rigid body will be added to the world model instance since # the model instance is not specified. box_body = mbp.AddRigidBody( "table", SpatialInertia(mass=1.0, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(1.0, 1.0, 1.0))) mbp.WeldFrames(mbp.world_frame(), box_body.body_frame(), X_W_T) mbp.RegisterVisualGeometry(box_body, RigidTransform(), box_shape, "table_vis", color) mbp.RegisterCollisionGeometry(box_body, RigidTransform(), box_shape, "table_collision", CoulombFriction(*friction_params)) # === Add pusher ==== parser = Parser(mbp, self._sim_diagram.sg) self._pusher_model_id = parser.AddModelFromFile( path_util.pusher_peg, "pusher") base_link = mbp.GetBodyByName("base", self._pusher_model_id) mbp.WeldFrames(mbp.world_frame(), base_link.body_frame()) def pusher_port_func(): actuation_input_port = mbp.get_actuation_input_port( self._pusher_model_id) state_output_port = mbp.get_state_output_port( self._pusher_model_id) builder.ExportInput(actuation_input_port, "pusher_actuation_input") builder.ExportOutput(state_output_port, "pusher_state_output") self._sim_diagram.finalize_functions.append(pusher_port_func) # === Add slider ==== parser = Parser(mbp, self._sim_diagram.sg) self._slider_model_id = parser.AddModelFromFile( path_util.model_paths[self._model_name], self._model_name) def slider_port_func(): state_output_port = mbp.get_state_output_port( self._slider_model_id) builder.ExportOutput(state_output_port, "slider_state_output") self._sim_diagram.finalize_functions.append(slider_port_func) if "rgbd_sensors" in config and config["rgbd_sensors"]["enabled"]: self._sim_diagram.add_rgbd_sensors_from_config(config) if "visualization" in config: if not config["visualization"]: pass elif config["visualization"] == "meshcat": self._sim_diagram.connect_to_meshcat() elif config["visualization"] == "drake_viz": self._sim_diagram.connect_to_drake_visualizer() else: raise ValueError("Unknown visualization:", config["visualization"]) self._sim_diagram.finalize() builder = DiagramBuilder() builder.AddSystem(self._sim_diagram) pid = builder.AddSystem( PidController(kp=[0, 0], kd=[1000, 1000], ki=[0, 0])) builder.Connect(self._sim_diagram.GetOutputPort("pusher_state_output"), pid.get_input_port_estimated_state()) builder.Connect( pid.get_output_port_control(), self._sim_diagram.GetInputPort("pusher_actuation_input")) builder.ExportInput(pid.get_input_port_desired_state(), "pid_input_port_desired_state") self._diagram = builder.Build() self._pid_desired_state_port = self._diagram.get_input_port(0) self._simulator = Simulator(self._diagram) self.reset() self.action_space = spaces.Box(low=-1., high=1., shape=(2, ), dtype=np.float32) # TODO: Observation space for images is currently too loose self.observation_space = convert_observation_to_space( self.get_observation())
def test_make_from_scene_graph_and_iris(self): """ Tests the make from scene graph and iris functionality together as the Iris code makes obstacles from geometries registered in SceneGraph. """ scene_graph = SceneGraph() source_id = scene_graph.RegisterSource("source") frame_id = scene_graph.RegisterFrame( source_id=source_id, frame=GeometryFrame("frame")) box_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=GeometryInstance(X_PG=RigidTransform(), shape=Box(1., 1., 1.), name="box")) cylinder_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=GeometryInstance(X_PG=RigidTransform(), shape=Cylinder(1., 1.), name="cylinder")) sphere_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=GeometryInstance(X_PG=RigidTransform(), shape=Sphere(1.), name="sphere")) capsule_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=GeometryInstance(X_PG=RigidTransform(), shape=Capsule(1., 1.0), name="capsule")) context = scene_graph.CreateDefaultContext() pose_vector = FramePoseVector() pose_vector.set_value(frame_id, RigidTransform()) scene_graph.get_source_pose_port(source_id).FixValue( context, pose_vector) query_object = scene_graph.get_query_output_port().Eval(context) H = mut.HPolyhedron( query_object=query_object, geometry_id=box_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(H.ambient_dimension(), 3) C = mut.CartesianProduct( query_object=query_object, geometry_id=cylinder_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(C.ambient_dimension(), 3) E = mut.Hyperellipsoid( query_object=query_object, geometry_id=sphere_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(E.ambient_dimension(), 3) S = mut.MinkowskiSum( query_object=query_object, geometry_id=capsule_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(S.ambient_dimension(), 3) P = mut.Point( query_object=query_object, geometry_id=sphere_geometry_id, reference_frame=scene_graph.world_frame_id(), maximum_allowable_radius=1.5) self.assertEqual(P.ambient_dimension(), 3) V = mut.VPolytope( query_object=query_object, geometry_id=box_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(V.ambient_dimension(), 3) obstacles = mut.MakeIrisObstacles( query_object=query_object, reference_frame=scene_graph.world_frame_id()) options = mut.IrisOptions() options.require_sample_point_is_contained = True options.iteration_limit = 1 options.termination_threshold = 0.1 options.relative_termination_threshold = 0.01 self.assertNotIn("object at 0x", repr(options)) region = mut.Iris( obstacles=obstacles, sample=[2, 3.4, 5], domain=mut.HPolyhedron.MakeBox( lb=[-5, -5, -5], ub=[5, 5, 5]), options=options) self.assertIsInstance(region, mut.HPolyhedron) obstacles = [ mut.HPolyhedron.MakeUnitBox(3), mut.Hyperellipsoid.MakeUnitBall(3), mut.Point([0, 0, 0]), mut.VPolytope.MakeUnitBox(3)] region = mut.Iris( obstacles=obstacles, sample=[2, 3.4, 5], domain=mut.HPolyhedron.MakeBox( lb=[-5, -5, -5], ub=[5, 5, 5]), options=options) self.assertIsInstance(region, mut.HPolyhedron)
def 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)
def xyz_rpy_deg(xyz, rpy_deg): """Shorthand for defining a pose.""" rpy_deg = np.asarray(rpy_deg) return RigidTransform(RollPitchYaw(rpy_deg * np.pi / 180), xyz)
def test_pose_aggregator(self): aggregator = PoseAggregator() # - Set-up. instance_id1 = 5 # Supply a random instance id. port1 = aggregator.AddSingleInput("pose_only", instance_id1) self.assertEqual(port1.get_data_type(), PortDataType.kVectorValued) self.assertEqual(port1.size(), PoseVector.kSize) instance_id2 = 42 # Supply another random, but unique, id. ports2 = aggregator.AddSinglePoseAndVelocityInput( "pose_and_velocity", instance_id2) self.assertEqual(ports2.pose_input_port.get_data_type(), PortDataType.kVectorValued) self.assertEqual(ports2.pose_input_port.size(), PoseVector.kSize) self.assertEqual(ports2.velocity_input_port.get_data_type(), PortDataType.kVectorValued) self.assertEqual(ports2.velocity_input_port.size(), FrameVelocity.kSize) num_poses = 1 port3 = aggregator.AddBundleInput("pose_bundle", num_poses) self.assertEqual(port3.get_data_type(), PortDataType.kAbstractValued) # - CalcOutput. self.assertEqual( aggregator.get_output_port(0).get_data_type(), PortDataType.kAbstractValued) context = aggregator.CreateDefaultContext() output = aggregator.AllocateOutput() p1 = [0, 1, 2] pose1 = PoseVector() pose1.set_translation(p1) p2 = [5, 7, 9] pose2 = PoseVector() pose2.set_translation(p2) w = [0.3, 0.4, 0.5] v = [0.5, 0.6, 0.7] velocity = FrameVelocity() velocity.set_velocity(SpatialVelocity(w=w, v=v)) p3 = [50, 70, 90] q3 = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) bundle = PoseBundle(num_poses) bundle.set_transform(0, RigidTransform(quaternion=q3, p=p3)) bundle_value = AbstractValue.Make(bundle) aggregator.get_input_port(0).FixValue(context, pose1) aggregator.get_input_port(1).FixValue(context, pose2) aggregator.get_input_port(2).FixValue(context, velocity) aggregator.get_input_port(3).FixValue(context, bundle_value) aggregator.CalcOutput(context, output) value = output.get_data(0).get_value() self.assertEqual(value.get_num_poses(), 3) pose1_actual = RigidTransform(p=p1) self.assertTrue((value.get_transform(0).GetAsMatrix34() == pose1_actual.GetAsMatrix34()).all()) pose2_actual = RigidTransform(p=p2) self.assertTrue((value.get_transform(1).GetAsMatrix34() == pose2_actual.GetAsMatrix34()).all()) vel_actual = value.get_velocity(1).get_velocity() self.assertTrue(np.allclose(vel_actual.rotational(), w)) self.assertTrue(np.allclose(vel_actual.translational(), v)) pose3_actual = RigidTransform(quaternion=q3, p=p3) self.assertTrue((value.get_transform(2).GetAsMatrix34() == pose3_actual.GetAsMatrix34()).all())
def test_multibody_tree_kinematics(self): file_name = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") plant = MultibodyPlant() Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() world_frame = plant.world_frame() base = plant.GetBodyByName("base") base_frame = plant.GetFrameByName("base") X_WL = plant.CalcRelativeTransform(context, frame_A=world_frame, frame_B=base_frame) self.assertIsInstance(X_WL, RigidTransform) p_AQi = plant.CalcPointsPositions(context=context, frame_B=base_frame, p_BQi=np.array([[0, 1, 2], [10, 11, 12]]).T, frame_A=world_frame).T self.assertTupleEqual(p_AQi.shape, (2, 3)) Jv_WL = plant.CalcFrameGeometricJacobianExpressedInWorld( context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0]) self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities())) nq = plant.num_positions() nv = plant.num_velocities() wrt_list = [ (JacobianWrtVariable.kQDot, nq), (JacobianWrtVariable.kV, nv), ] for wrt, nw in wrt_list: Jw_ABp_E = plant.CalcJacobianSpatialVelocity(context=context, with_respect_to=wrt, frame_B=base_frame, p_BP=np.zeros(3), frame_A=world_frame, frame_E=world_frame) self.assert_sane(Jw_ABp_E) self.assertEqual(Jw_ABp_E.shape, (6, nw)) # Compute body pose. X_WBase = plant.EvalBodyPoseInWorld(context, base) self.assertIsInstance(X_WBase, RigidTransform) # Set pose for the base. X_WB_desired = RigidTransform.Identity() X_WB = plant.CalcRelativeTransform(context, world_frame, base_frame) plant.SetFreeBodyPose(context=context, body=base, X_WB=X_WB_desired) self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix())) # Set a spatial velocity for the base. v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6]) plant.SetFreeBodySpatialVelocity(context=context, body=base, V_WB=v_WB) v_base = plant.EvalBodySpatialVelocityInWorld(context, base) self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational())) self.assertTrue( np.allclose(v_base.translational(), v_WB.translational())) # Compute accelerations. vdot = np.zeros(nv) A_WB_array = plant.CalcSpatialAccelerationsFromVdot(context=context, known_vdot=vdot) self.assertEqual(len(A_WB_array), plant.num_bodies())
def setUp(self): self.points = np.array([[1, 1, 0], [2, 1, 0]]).T self.translation = RigidTransform(p=[1, 2, 3]) self.rotation = RigidTransform( RotationMatrix(RollPitchYaw(0, 0, np.pi / 2)))
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 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_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))