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)