def add_triad( source_id, frame_id, scene_graph, *, length, radius, opacity, X_FT=RigidTransform(), name="frame", ): """ Adds illustration geometry representing the coordinate frame, with the x-axis drawn in red, the y-axis in green and the z-axis in blue. The axes point in +x, +y and +z directions, respectively. Based on [code permalink](https://github.com/RussTedrake/manipulation/blob/5e59811/manipulation/scenarios.py#L367-L414).# noqa Args: source_id: The source registered with SceneGraph. frame_id: A geometry::frame_id registered with scene_graph. scene_graph: The SceneGraph with which we will register the geometry. length: the length of each axis in meters. radius: the radius of each axis in meters. opacity: the opacity of the coordinate axes, between 0 and 1. X_FT: a RigidTransform from the triad frame T to the frame_id frame F name: the added geometry will have names name + " x-axis", etc. """ # x-axis X_TG = RigidTransform( RotationMatrix.MakeYRotation(np.pi / 2), [length / 2.0, 0, 0], ) geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length), name + " x-axis") geom.set_illustration_properties( MakePhongIllustrationProperties([1, 0, 0, opacity])) scene_graph.RegisterGeometry(source_id, frame_id, geom) # y-axis X_TG = RigidTransform( RotationMatrix.MakeXRotation(np.pi / 2), [0, length / 2.0, 0], ) geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length), name + " y-axis") geom.set_illustration_properties( MakePhongIllustrationProperties([0, 1, 0, opacity])) scene_graph.RegisterGeometry(source_id, frame_id, geom) # z-axis X_TG = RigidTransform([0, 0, length / 2.0]) geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length), name + " z-axis") geom.set_illustration_properties( MakePhongIllustrationProperties([0, 0, 1, opacity])) scene_graph.RegisterGeometry(source_id, frame_id, geom)
def AddTriad(source_id, frame_id, scene_graph, length=.25, radius=0.01, opacity=1., X_FT=RigidTransform(), name="frame"): """ Adds illustration geometry representing the coordinate frame, with the x-axis drawn in red, the y-axis in green and the z-axis in blue. The axes point in +x, +y and +z directions, respectively. Args: source_id: The source registered with SceneGraph. frame_id: A geometry::frame_id registered with scene_graph. scene_graph: The SceneGraph with which we will register the geometry. length: the length of each axis in meters. radius: the radius of each axis in meters. opacity: the opacity of the coordinate axes, between 0 and 1. X_FT: a RigidTransform from the triad frame T to the frame_id frame F name: the added geometry will have names name + " x-axis", etc. """ # x-axis X_TG = RigidTransform(RotationMatrix.MakeYRotation(np.pi / 2), [length / 2., 0, 0]) geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length), name + " x-axis") geom.set_illustration_properties( MakePhongIllustrationProperties([1, 0, 0, opacity])) scene_graph.RegisterGeometry(source_id, frame_id, geom) # y-axis X_TG = RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2), [0, length / 2., 0]) geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length), name + " y-axis") geom.set_illustration_properties( MakePhongIllustrationProperties([0, 1, 0, opacity])) scene_graph.RegisterGeometry(source_id, frame_id, geom) # z-axis X_TG = RigidTransform([0, 0, length / 2.]) geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length), name + " z-axis") geom.set_illustration_properties( MakePhongIllustrationProperties([0, 0, 1, opacity])) scene_graph.RegisterGeometry(source_id, frame_id, geom)
def update_visualization(plant, scene_graph): if args.visualize_collisions: # Find all the geometries that have not already been marked as # 'illustration' (visual) and assume they are collision geometries. # Then add illustration properties to them that will draw them in # red and fifty percent translucent. source_id = plant.get_source_id() inspector = scene_graph.model_inspector() diffuse_rgba = [1, 0, 0, 0.5] red_illustration = MakePhongIllustrationProperties(diffuse_rgba) for geometry_id in inspector.GetAllGeometryIds(): if inspector.GetIllustrationProperties(geometry_id) is None: scene_graph.AssignRole(source_id, geometry_id, red_illustration) if args.visualize_frames: # Visualize frames # Find all the frames and draw them using add_triad(). # The frames are drawn using the parsed length. # The world frame is drawn thicker than the rest. inspector = scene_graph.model_inspector() for frame_id in inspector.GetAllFrameIds(): radius = args.triad_radius * ( 3 if frame_id == scene_graph.world_frame_id() else 1) add_triad( plant.get_source_id(), frame_id, scene_graph, length=args.triad_length, radius=radius, opacity=args.triad_opacity, )
def add_vis_object(self, name, color): source_id = self._sg.RegisterSource(name) frame_id = self._sg.RegisterFrame(source_id, GeometryFrame(f"{name}_frame")) geom = GeometryInstance(RigidTransform(), Sphere(0.014), f"{name}_geometry") geom.set_illustration_properties( MakePhongIllustrationProperties(color) ) goal_id = self._sg.RegisterGeometry(source_id, frame_id, geom) goal_vis = self._builder.AddSystem(VisObject(frame_id)) self._builder.Connect(goal_vis.get_output_port(0), self._sg.get_source_pose_port(source_id)) return goal_vis
def update_visualization(plant, scene_graph): if args.visualize_collisions: # Find all the geometries that have not already been marked as # 'illustration' (visual) and assume they are collision geometries. # Then add illustration properties to them that will draw them in # red and fifty percent translucent. source_id = plant.get_source_id() inspector = scene_graph.model_inspector() diffuse_rgba = [1, 0, 0, 0.5] red_illustration = MakePhongIllustrationProperties(diffuse_rgba) for geometry_id in inspector.GetAllGeometryIds(): if inspector.GetIllustrationProperties(geometry_id) is None: scene_graph.AssignRole(source_id, geometry_id, red_illustration)
def main(): args_parser = argparse.ArgumentParser( description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) args_parser.add_argument("filename", type=str, help="Path to an SDF or URDF file.") args_parser.add_argument( "--package_path", type=str, default=None, help="Full path to the root package for reading in SDF resources.") args_parser.add_argument("--pyplot", action="store_true", help="Opens a pyplot figure for rendering using " "PlanarSceneGraphVisualizer.") # TODO(russt): Consider supporting the PlanarSceneGraphVisualizer # options as additional arguments. position_group = args_parser.add_mutually_exclusive_group() position_group.add_argument( "--position", type=float, nargs="+", default=[], help="A list of positions which must be the same length as the number " "of positions in the sdf model. Note that most models have a " "floating-base joint by default (unless the sdf explicitly welds " "the base to the world, and so have 7 positions corresponding to " "the quaternion representation of that floating-base position.") position_group.add_argument( "--joint_position", type=float, nargs="+", default=[], help="A list of positions which must be the same length as the number " "of positions ASSOCIATED WITH JOINTS in the sdf model. This " "does not include, e.g., floating-base coordinates, which will " "be assigned a default value.") args_parser.add_argument( "--test", action='store_true', help="Disable opening the gui window for testing.") args_parser.add_argument( "--visualize_collisions", action="store_true", help="Visualize the collision geometry in the visualizer. The " "collision geometries will be shown in red to differentiate " "them from the visual geometries.") # TODO(russt): Add option to weld the base to the world pending the # availability of GetUniqueBaseBody requested in #9747. MeshcatVisualizer.add_argparse_argument(args_parser) args = args_parser.parse_args() filename = args.filename if not os.path.isfile(filename): args_parser.error("File does not exist: {}".format(filename)) builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) # Construct a MultibodyPlant. plant = MultibodyPlant(0.0) plant.RegisterAsSourceForSceneGraph(scene_graph) # Create the parser. parser = Parser(plant) # Get the package pathname. if args.package_path: # Verify that package.xml is found in the designated path. package_path = os.path.abspath(args.package_path) if not os.path.isfile(os.path.join(package_path, "package.xml")): parser.error("package.xml not found at: {}".format(package_path)) # Get the package map and populate it using the package path. package_map = parser.package_map() package_map.PopulateFromFolder(package_path) # Add the model from the file and finalize the plant. parser.AddModelFromFile(filename) # Find all the geometries that have not already been marked as # 'illustration' (visual) and assume they are collision geometries. # Then add illustration properties to them that will draw them in red # and fifty percent translucent. if args.visualize_collisions: source_id = plant.get_source_id() red_illustration = MakePhongIllustrationProperties([1, 0, 0, 0.5]) for geometry_id in scene_graph.model_inspector().GetAllGeometryIds(): if (scene_graph.model_inspector().GetIllustrationProperties( geometry_id) is None): scene_graph.AssignRole(source_id, geometry_id, red_illustration) plant.Finalize() # Add sliders to set positions of the joints. sliders = builder.AddSystem(JointSliders(robot=plant)) to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant)) builder.Connect(sliders.get_output_port(0), to_pose.get_input_port()) builder.Connect(to_pose.get_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) # Connect this to drake_visualizer. ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) # Connect to Meshcat. if args.meshcat is not None: meshcat_viz = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=args.meshcat)) builder.Connect(scene_graph.get_pose_bundle_output_port(), meshcat_viz.get_input_port(0)) if args.pyplot: pyplot = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph)) builder.Connect(scene_graph.get_pose_bundle_output_port(), pyplot.get_input_port(0)) if len(args.position): sliders.set_position(args.position) elif len(args.joint_position): sliders.set_joint_position(args.joint_position) # Make the diagram and run it. diagram = builder.Build() simulator = Simulator(diagram) simulator.set_publish_every_time_step(False) if args.test: sliders.window.withdraw() simulator.AdvanceTo(0.1) else: simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(np.inf)