def make_plant(model_directive_path):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
InsertCorrectLoadingMethodHere(model_directive_path, plant)
plant.Finalize()
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
return plant, scene_graph, diagram
def simple_mbp():
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
Parser(plant).AddModelFromFile("common/test/models/double_pendulum.sdf")
plant.Finalize()
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
return plant, scene_graph, diagram
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("--simulation_time",
type=float,
default=10.0,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--time_step",
type=float,
default=0.,
help="If greater than zero, the plant is modeled as a system with "
"discrete updates and period equal to this time_step. "
"If 0, the plant is modeled as a continuous system.")
args = parser.parse_args()
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant(time_step=args.time_step))
cart_pole.RegisterAsSourceForSceneGraph(scene_graph)
Parser(plant=cart_pole).AddModelFromFile(file_name)
cart_pole.AddForceElement(UniformGravityFieldElement())
cart_pole.Finalize()
assert cart_pole.geometry_source_is_registered()
builder.Connect(scene_graph.get_query_output_port(),
cart_pole.get_geometry_query_input_port())
builder.Connect(
cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
cart_pole_context = diagram.GetMutableSubsystemContext(
cart_pole, diagram_context)
cart_pole_context.FixInputPort(
cart_pole.get_actuation_input_port().get_index(), [0])
cart_slider = cart_pole.GetJointByName("CartSlider")
pole_pin = cart_pole.GetJointByName("PolePin")
cart_slider.set_translation(context=cart_pole_context, translation=0.)
pole_pin.set_angle(context=cart_pole_context, angle=2.)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.Initialize()
simulator.StepTo(args.simulation_time)
def main():
args_parser = argparse.ArgumentParser()
args_parser.add_argument("--simulation_sec", type=float, default=np.inf)
args_parser.add_argument("--sim_dt", type=float, default=0.1)
args_parser.add_argument(
"--single_shot",
action="store_true",
help="Test workflow of visulaization through Simulator.Initialize")
args_parser.add_argument("--realtime_rate", type=float, default=1.)
args_parser.add_argument("--num_models", type=int, default=3)
args = args_parser.parse_args()
sdf_file = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/iiwa7/"
"iiwa7_no_collision.sdf")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.01)
parser = Parser(plant)
models = []
for i in range(args.num_models):
model_name = f"iiwa{i}"
# TODO(eric.cousineau): This warns about mutating the package path
# multiple times :(
model = parser.AddModelFromFile(sdf_file, model_name)
models.append(model)
base_frame = plant.GetFrameByName("iiwa_link_0", model)
# Translate along x-axis by 1m to separate.
X_WB = RigidTransform([i, 0, 0])
plant.WeldFrames(plant.world_frame(), base_frame, X_WB)
plant.Finalize()
for model in models:
no_control(plant, builder, model)
ConnectDrakeVisualizer(builder, scene_graph)
ConnectRvizVisualizer(builder, scene_graph)
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
simulator.set_target_realtime_rate(args.realtime_rate)
# Wait for ROS publishers to wake up :(
time.sleep(0.3)
if args.single_shot:
# To see what 'preview' scripts look like.
# TODO(eric.cousineau): Make this work *robustly* with Rviz. Markers
# still don't always show up :(
simulator.Initialize()
diagram.Publish(context)
else:
while context.get_time() < args.simulation_sec:
# Use increments to permit Ctrl+C to be caught.
simulator.AdvanceTo(context.get_time() + args.sim_dt)
def test_copying(self):
"""Ensures that index ordering is generally the same when copying a
plant using a subgraph."""
time_step = 0.01
builder_a = DiagramBuilder()
plant_a, scene_graph_a = AddMultibodyPlantSceneGraph(
builder_a, time_step)
util.add_arbitrary_multibody_stuff(plant_a)
# Ensure that this is "physically" valid.
plant_a.Finalize()
if VISUALIZE:
for model in me.get_model_instances(plant_a):
util.no_control(builder_a, plant_a, model)
print("test_composition_array_with_scene_graph")
ConnectDrakeVisualizer(builder_a, scene_graph_a)
diagram = builder_a.Build()
simulator = Simulator(diagram)
simulator.Initialize()
diagram.Publish(simulator.get_context())
simulator.AdvanceTo(1.)
# Checking for determinism, making a slight change to trigger an error.
for slight_difference in [False, True]:
builder_b = DiagramBuilder()
plant_b, scene_graph_b = AddMultibodyPlantSceneGraph(
builder_b, time_step)
util.add_arbitrary_multibody_stuff(
plant_b, slight_difference=slight_difference)
plant_b.Finalize()
if not slight_difference:
util.assert_plant_equals(plant_a, scene_graph_a, plant_b,
scene_graph_b)
else:
with self.assertRaises(AssertionError):
util.assert_plant_equals(plant_a, scene_graph_a, plant_b,
scene_graph_b)
# Check for general ordering with full subgraph "cloning".
builder_b = DiagramBuilder()
plant_b, scene_graph_b = AddMultibodyPlantSceneGraph(
builder_b, time_step)
subgraph_a = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(plant_a, scene_graph_a))
subgraph_a.add_to(plant_b, scene_graph_b)
plant_b.Finalize()
util.assert_plant_equals(plant_a, scene_graph_a, plant_b,
scene_graph_b)
def connect_visualizers(builder, plant, scene_graph):
# 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))
# Connect to PyPlot.
if args.pyplot:
pyplot = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
pyplot.get_input_port(0))
def main():
parser = argparse.ArgumentParser(
description=__doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument(
"filename", type=str,
help="Path to an SDF or URDF file.")
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
filename = args.filename
if not os.path.isfile(filename):
parser.error("File does not exist: {}".format(filename))
# Construct Plant + SceneGraph.
builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant())
scene_graph = builder.AddSystem(SceneGraph())
plant.RegisterAsSourceForSceneGraph(scene_graph)
builder.Connect(
scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
builder.Connect(
plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
# Load the model file.
Parser(plant).AddModelFromFile(filename)
plant.Finalize()
# Publish to Drake Visualizer.
drake_viz_pub = ConnectDrakeVisualizer(builder, scene_graph)
# Publish 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))
diagram = builder.Build()
context = diagram.CreateDefaultContext()
# Use Simulator to dispatch initialization events.
# TODO(eric.cousineau): Simplify as part of #10015.
Simulator(diagram).Initialize()
# Publish draw messages with current state.
diagram.Publish(context)
def test_composition_array_with_scene_graph(self):
"""Tests subgraphs (post-finalize) with a scene graph. This time, using
sugar from parse_as_multibody_plant_subgraph."""
# Create IIWA.
iiwa_file = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/urdf/"
"iiwa14_spheres_dense_elbow_collision.urdf")
iiwa_subgraph, iiwa_model = mut.parse_as_multibody_plant_subgraph(
iiwa_file)
self.assertIsInstance(iiwa_subgraph, mut.MultibodyPlantSubgraph)
self.assertIsInstance(iiwa_model, ModelInstanceIndex)
# Make 10 copies of the IIWA in a line.
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=0.01)
models = []
for i in range(10):
sub_to_full = iiwa_subgraph.add_to(
plant, scene_graph, model_instance_remap=f"iiwa_{i}")
self.assertIsInstance(sub_to_full, mut.MultibodyPlantElementsMap)
X_WB = RigidTransform(p=[i * 0.5, 0, 0])
model = sub_to_full.model_instances[iiwa_model]
base_frame = plant.GetFrameByName("base", model)
plant.WeldFrames(plant.world_frame(), base_frame, X_WB)
models.append(model)
plant.Finalize()
if VISUALIZE:
print("test_composition_array_with_scene_graph")
ConnectDrakeVisualizer(builder, scene_graph)
diagram = builder.Build()
d_context = diagram.CreateDefaultContext()
for i, model in enumerate(models):
self.assertEqual(plant.GetModelInstanceName(model), f"iiwa_{i}")
if VISUALIZE:
print(" Visualize composite")
Simulator(diagram, d_context.Clone()).Initialize()
input(" Press enter...")
def main():
builder = DiagramBuilder()
sim_plant, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(mbp_time_step))
load_atlas(sim_plant, add_ground=True)
sim_plant_context = sim_plant.CreateDefaultContext()
controller_plant = MultibodyPlant(mbp_time_step)
load_atlas(controller_plant)
controller = builder.AddSystem(
HumanoidController(controller_plant,
Atlas.CONTACTS_PER_FRAME,
is_wbc=False))
controller.set_name("HumanoidController")
disturber = builder.AddSystem(
ForceDisturber(sim_plant.GetBodyByName("utorso").index(), 4, 0.1, 2))
builder.Connect(disturber.get_output_port(0),
sim_plant.get_applied_spatial_force_input_port())
builder.Connect(sim_plant.get_state_output_port(),
controller.GetInputPort("q_v"))
builder.Connect(controller.GetOutputPort("tau"),
sim_plant.get_actuation_input_port())
ConnectContactResultsToDrakeVisualizer(builder=builder, plant=sim_plant)
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
sim_plant_context = diagram.GetMutableSubsystemContext(
sim_plant, diagram_context)
set_atlas_initial_pose(sim_plant, sim_plant_context)
controller_context = diagram.GetMutableSubsystemContext(
controller, diagram_context)
controller.GetInputPort("y_des").FixValue(controller_context,
np.array([0.1, 0.0]))
simulator = Simulator(diagram, diagram_context)
simulator.set_target_realtime_rate(0.04)
simulator.AdvanceTo(20.0)
"user input.",
default=False)
args = parser.parse_args()
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant())
cart_pole.RegisterAsSourceForSceneGraph(scene_graph)
AddModelFromSdfFile(file_name=file_name, plant=cart_pole)
cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
cart_pole.Finalize()
assert cart_pole.geometry_source_is_registered()
builder.Connect(scene_graph.get_query_output_port(),
cart_pole.get_geometry_query_input_port())
builder.Connect(cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
builder.ExportInput(cart_pole.get_actuation_input_port())
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
diagram.set_name("graphviz_example")
plot_system_graphviz(diagram, max_depth=2)
if not args.test:
plt.show()
def do_exploding_iiwa_sim(self, plant_src, scene_graph_src, context_src):
# Show a simulation which "changes state" by being rebuilt.
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=1e-3)
subgraph_src = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(plant_src, scene_graph_src))
to_plant = subgraph_src.add_to(plant, scene_graph)
# 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.no_control(builder, plant, model)
if VISUALIZE:
print("do_exploding_iiwa_sim")
role = Role.kIllustration
ConnectDrakeVisualizer(builder, scene_graph, role=role)
ConnectContactResultsToDrakeVisualizer(builder, plant)
diagram = builder.Build()
# Set up context.
d_context = diagram.CreateDefaultContext()
context = plant.GetMyContextFromRoot(d_context)
to_plant.copy_state(context_src, 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 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:
ConnectDrakeVisualizer(builder_new, scene_graph_new, role=role)
ConnectContactResultsToDrakeVisualizer(builder_new, plant_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...")
# file.
camera_ids = ["805212060544"]
station = builder.AddSystem(
ManipulationStationHardwareInterface(camera_ids))
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
station.AddCupboard()
object = AddModelFromSdfFile(
FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf"),
"object", station.get_mutable_multibody_plant(),
station.get_mutable_scene_graph())
station.Finalize()
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
teleop = builder.AddSystem(
JointSliders(station.get_controller_plant(), length=800))
if args.test:
teleop.window.withdraw() # Don't display the window when testing.
builder.Connect(teleop.get_output_port(0),
station.GetInputPort("iiwa_position"))
wsg_buttons = builder.AddSystem(SchunkWsgButtons(teleop.window))
builder.Connect(wsg_buttons.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(wsg_buttons.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
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.")
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.")
# 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)
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 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)
def test_decomposition_controller_like_workflow(self):
"""Tests subgraphs (post-finalize) for decomposition, with a
scene-graph. Also shows a workflow of replacing joints / welding
joints."""
builder = DiagramBuilder()
# N.B. I (Eric) am using ManipulationStation because it's currently
# the simplest way to get a complex scene in pydrake.
station = ManipulationStation(time_step=0.001)
station.SetupManipulationClassStation()
station.Finalize()
builder.AddSystem(station)
plant = station.get_multibody_plant()
scene_graph = station.get_scene_graph()
iiwa = plant.GetModelInstanceByName("iiwa")
wsg = plant.GetModelInstanceByName("gripper")
if VISUALIZE:
print("test_decomposition_controller_like_workflow")
ConnectDrakeVisualizer(builder, scene_graph,
station.GetOutputPort("pose_bundle"))
diagram = builder.Build()
# Set the context with which things should be computed.
d_context = diagram.CreateDefaultContext()
context = plant.GetMyContextFromRoot(d_context)
q_iiwa = [0.3, 0.7, 0.3, 0.6, 0.5, 0.6, 0.7]
q_wsg = [-0.03, 0.03]
plant.SetPositions(context, iiwa, q_iiwa)
plant.SetPositions(context, wsg, q_wsg)
# Build and visualize.
control_builder = DiagramBuilder()
control_plant, control_scene_graph = AddMultibodyPlantSceneGraph(
control_builder, time_step=0.)
if VISUALIZE:
ConnectDrakeVisualizer(control_builder, control_scene_graph)
# N.B. This has the same scene, but with all joints outside of the
# IIWA frozen.
to_control = mut.add_plant_with_articulated_subset_to(
plant_src=plant,
scene_graph_src=scene_graph,
articulated_models_src=[iiwa],
context_src=context,
plant_dest=control_plant,
scene_graph_dest=control_scene_graph)
self.assertIsInstance(to_control, mut.MultibodyPlantElementsMap)
control_plant.Finalize()
self.assertEqual(control_plant.num_positions(),
plant.num_positions(iiwa))
control_diagram = control_builder.Build()
control_d_context = control_diagram.CreateDefaultContext()
control_context = control_plant.GetMyContextFromRoot(control_d_context)
to_control.copy_state(context, control_context)
util.compare_frames(plant, context, control_plant, control_context,
"iiwa_link_0", "iiwa_link_7")
util.compare_frames(plant, context, control_plant, control_context,
"body", "left_finger")
# Visualize.
if VISUALIZE:
print(" Visualize original plant")
Simulator(diagram, d_context.Clone()).Initialize()
input(" Press enter...")
print(" Visualize control plant")
Simulator(control_diagram, control_d_context.Clone()).Initialize()
input(" Press enter...")
# For grins, ensure that we can copy everything, including world weld.
control_plant_copy = MultibodyPlant(time_step=0.)
subgraph = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(control_plant))
subgraph.add_to(control_plant_copy)
control_plant_copy.Finalize()
self.assertEqual(control_plant_copy.num_positions(),
control_plant.num_positions())
util.assert_plant_equals(control_plant, None, control_plant_copy, None)
def add_drake_visualizer(scene_graph, builder):
lcm = DrakeLcm()
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph, lcm=lcm)
DispatchLoadMessage(scene_graph,
lcm) # TODO: only update viewer after a plan is found
return lcm # Important that variable is saved
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 = builder.AddSystem(
MeshcatVisualizer(station.get_scene_graph(),
zmq_url=args.meshcat,
open_browser=args.open_browser))
builder.Connect(station.GetOutputPort("pose_bundle"),
meshcat.get_input_port(0))
if args.setup == 'planar':
meshcat.set_planar_viewpoint()
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:
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
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 visualize(theta1, theta2, theta3, theta4=0.0, phi=0.0):
file_name = "res/stair_climb.sdf"
builder = DiagramBuilder()
stair_climb, scene_graph = AddMultibodyPlantSceneGraph(builder)
# stair_climb.RegisterAsSourceForSceneGraph(scene_graph)
Parser(plant=stair_climb).AddModelFromFile(file_name)
# stair_climb.AddForceElement(UniformGravityFieldElement())
front_wheel_x, front_wheel_y = eom.findFrontWheelPosition(
theta1, theta2, theta3)
front_wheel_x = front_wheel_x.Evaluate()
front_wheel_y = front_wheel_y.Evaluate()
step = Box(STEP_DEPTH, STEP_WIDTH, STEP_HEIGHT)
step_pos = RigidTransform(
[STEP_POSITION + STEP_DEPTH / 2.0, 0.0, STEP_HEIGHT / 2.0])
stair_climb.RegisterCollisionGeometry(
stair_climb.world_body(), RigidTransform([0.0, 0.0, 0.0]), HalfSpace(),
"GroundCollision", CoulombFriction(COEFF_FRICTION, COEFF_FRICTION))
stair_climb.RegisterVisualGeometry(stair_climb.world_body(),
RigidTransform([0.0, 0.0, 0.0]),
HalfSpace(), "GroundVisual",
np.array([0.5, 0.5, 0.5, 0.5])) # Color
stair_climb.RegisterCollisionGeometry(
stair_climb.world_body(), step_pos, step, "StepCollision",
CoulombFriction(COEFF_FRICTION, COEFF_FRICTION))
stair_climb.RegisterVisualGeometry(stair_climb.world_body(), step_pos,
step, "StepVisual",
np.array([1.0, 1.0, 0.0, 1.0])) # Color
stair_climb.Finalize()
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
stair_climb_context = diagram.GetMutableSubsystemContext(
stair_climb, diagram_context)
stair_climb_context.FixInputPort(
stair_climb.get_actuation_input_port().get_index(), [0, 0, 0, 0, 0])
theta1_joint = stair_climb.GetJointByName("theta1")
theta2_joint = stair_climb.GetJointByName("theta2")
theta3_joint = stair_climb.GetJointByName("theta3")
theta4_joint = stair_climb.GetJointByName("theta4")
phi_joint = stair_climb.GetJointByName("phi")
theta1_joint.set_angle(context=stair_climb_context, angle=theta1)
theta2_joint.set_angle(context=stair_climb_context, angle=theta2)
theta3_joint.set_angle(context=stair_climb_context, angle=theta3)
theta4_joint.set_angle(context=stair_climb_context, angle=theta4)
phi_joint.set_angle(context=stair_climb_context, angle=phi)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(0.001)
simulator.Initialize()
simulator.AdvanceTo(0.0)
def LittleDogSimulationDiagram2(lcm, rb_tree, dt, drake_visualizer):
builder = DiagramBuilder()
num_pos = rb_tree.get_num_positions()
num_actuators = rb_tree.get_num_actuators()
zero_config = np.zeros(
(rb_tree.get_num_actuators() * 2, 1)) # just for test
# zero_config = np.concatenate((np.array([0, 1, 1, 1, 1.7, 0.5, 0, 0, 0]),np.zeros(9)), axis=0)
# zero_config = np.concatenate((rb_tree.getZeroConfiguration(),np.zeros(9)), axis=0)
zero_config = np.concatenate(
(np.array([1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]), np.zeros(12)), axis=0)
# zero_config[0] = 1.7
# zero_config[1] = 1.7
# zero_config[2] = 1.7
# zero_config[3] = 1.7
# 1.SceneGraph system
scene_graph = builder.AddSystem(SceneGraph())
scene_graph.RegisterSource('scene_graph_n')
plant = builder.AddSystem(MultibodyPlant())
plant_parser = Parser(plant, scene_graph)
plant_parser.AddModelFromFile(file_name=model_path, model_name="littledog")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("body"))
# Add gravity to the model.
plant.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
# Model is completed
plant.Finalize()
builder.Connect(scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
builder.Connect(plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
# Robot State Encoder
robot_state_encoder = builder.AddSystem(
RobotStateEncoder(rb_tree)) # force_sensor_info
robot_state_encoder.set_name('robot_state_encoder')
builder.Connect(plant.get_continuous_state_output_port(),
robot_state_encoder.joint_state_results_input_port())
# Robot command to Plant Converter
robot_command_to_rigidbodyplant_converter = builder.AddSystem(
RobotCommandToRigidBodyPlantConverter(
rb_tree.actuators,
motor_gain=None)) # input argu: rigidbody actuators
robot_command_to_rigidbodyplant_converter.set_name(
'robot_command_to_rigidbodyplant_converter')
builder.Connect(
robot_command_to_rigidbodyplant_converter.desired_effort_output_port(),
plant.get_actuation_input_port())
# PID controller
kp, ki, kd = joints_PID_params(rb_tree)
# PIDcontroller = builder.AddSystem(RobotPDAndFeedForwardController(rb_tree, kp, ki, kd))
# PIDcontroller.set_name('PID_controller')
rb = RigidBodyTree()
robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0],
[0, 0, 0])
# insert a robot from urdf files
pmap = PackageMap()
pmap.PopulateFromFolder(os.path.dirname(model_path))
AddModelInstanceFromUrdfStringSearchingInRosPackages(
open(model_path, 'r').read(),
pmap,
os.path.dirname(model_path),
FloatingBaseType.kFixed, # FloatingBaseType.kRollPitchYaw,
robot_frame,
rb)
PIDcontroller = builder.AddSystem(
RbtInverseDynamicsController(rb, kp, ki, kd, False))
PIDcontroller.set_name('PID_controller')
builder.Connect(robot_state_encoder.joint_state_outport_port(),
PIDcontroller.get_input_port(0))
builder.Connect(
PIDcontroller.get_output_port(0),
robot_command_to_rigidbodyplant_converter.robot_command_input_port())
# Ref trajectory
traj_src = builder.AddSystem(ConstantVectorSource(zero_config))
builder.Connect(traj_src.get_output_port(0),
PIDcontroller.get_input_port(1))
# Signal logger
logger = builder.AddSystem(SignalLogger(num_pos * 2))
builder.Connect(plant.get_continuous_state_output_port(),
logger.get_input_port(0))
return builder.Build(), logger, plant
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate",
type=float,
default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument("--duration",
type=float,
default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware",
action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument("--test",
action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
"--filter_time_const",
type=float,
default=0.005,
help="Time constant for the first order low pass filter applied to"
"the teleop commands")
parser.add_argument(
"--velocity_limit_factor",
type=float,
default=1.0,
help="This value, typically between 0 and 1, further limits the "
"iiwa14 joint velocities. It multiplies each of the seven "
"pre-defined joint velocity limits. "
"Note: The pre-defined velocity limits are specified by "
"iiwa14_velocity_limits, found in this python file.")
parser.add_argument('--setup',
type=str,
default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
if args.test:
# Don't grab mouse focus during testing.
grab_focus = False
# See: https://stackoverflow.com/a/52528832/7829525
os.environ["SDL_VIDEODRIVER"] = "dummy"
else:
grab_focus = True
builder = DiagramBuilder()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation()
station.AddManipulandFromFile(
("drake/examples/manipulation_station/models/"
"061_foam_brick.sdf"),
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation()
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
station.Finalize()
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
if args.meshcat:
meshcat = builder.AddSystem(
MeshcatVisualizer(station.get_scene_graph(),
zmq_url=args.meshcat))
builder.Connect(station.GetOutputPort("pose_bundle"),
meshcat.get_input_port(0))
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
time_step = 0.005
params.set_timestep(time_step)
# True velocity limits for the IIWA14 (in rad, rounded down to the first
# decimal)
iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])
# Stay within a small fraction of those limits for this teleop demo.
factor = args.velocity_limit_factor
params.set_joint_velocity_limits(
(-factor * iiwa14_velocity_limits, factor * iiwa14_velocity_limits))
differential_ik = builder.AddSystem(
DifferentialIK(robot, robot.GetFrameByName("iiwa_link_7"), params,
time_step))
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
teleop = builder.AddSystem(MouseKeyboardTeleop(grab_focus=grab_focus))
filter_ = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6))
builder.Connect(teleop.get_output_port(0), filter_.get_input_port(0))
builder.Connect(filter_.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
builder.Connect(teleop.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(teleop.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(7))
simulator.AdvanceTo(1e-6)
q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context)
differential_ik.parameters.set_nominal_joint_position(q0)
teleop.SetPose(differential_ik.ForwardKinematics(q0))
filter_.set_initial_output_value(
diagram.GetMutableSubsystemContext(filter_,
simulator.get_mutable_context()),
teleop.get_output_port(0).Eval(
diagram.GetMutableSubsystemContext(
teleop, simulator.get_mutable_context())))
differential_ik.SetPositions(
diagram.GetMutableSubsystemContext(differential_ik,
simulator.get_mutable_context()),
q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
print_instructions()
simulator.AdvanceTo(args.duration)
def test_composition_gripper_workflow(self):
"""Tests subgraphs (pre-finalize) for composition, with a scene graph,
welding bodies together across different subgraphs."""
# 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_subgraph = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(iiwa_plant, iiwa_scene_graph))
self.assertIsInstance(iiwa_subgraph, mut.MultibodyPlantSubgraph)
mut.disconnect_subgraph_from_world(iiwa_subgraph)
# 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_subgraph = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(wsg_plant, wsg_scene_graph))
self.assertIsInstance(wsg_subgraph, mut.MultibodyPlantSubgraph)
mut.disconnect_subgraph_from_world(wsg_subgraph)
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:
print("test_composition")
ConnectDrakeVisualizer(iiwa_builder, iiwa_scene_graph)
ConnectDrakeVisualizer(wsg_builder, wsg_scene_graph)
ConnectDrakeVisualizer(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_plant.Finalize()
iiwa_diagram = iiwa_builder.Build()
iiwa_d_context = iiwa_diagram.CreateDefaultContext()
iiwa_context = iiwa_plant.GetMyContextFromRoot(iiwa_d_context)
iiwa_plant.SetPositions(iiwa_context, iiwa_model, q_iiwa)
wsg_plant.Finalize()
wsg_diagram = wsg_builder.Build()
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_frames(plant, context, iiwa_plant, iiwa_context, "base",
"iiwa_link_7")
util.compare_frames(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...")
self.do_exploding_iiwa_sim(plant, scene_graph, context)
def main():
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation())
station.SetupClutterClearingStation()
station.Finalize()
package_map = PackageMap()
package_map.PopulateFromEnvironment('AMENT_PREFIX_PATH')
sdf_file_path = os.path.join(package_map.GetPath('iiwa14_description'),
'iiwa14_no_collision.sdf')
joint_names = [
'iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4',
'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7'
]
joints = []
for name in joint_names:
joints.append(station.get_controller_plant().GetJointByName(name))
rclpy.init()
node = rclpy.create_node('interactive_demo')
# Publish SDF content on robot_description topic
latching_qos = QoSProfile(depth=1,
durability=DurabilityPolicy.TRANSIENT_LOCAL)
description_publisher = node.create_publisher(StringMsg,
'robot_description',
qos_profile=latching_qos)
msg = StringMsg()
with open(sdf_file_path, 'r') as sdf_file:
msg.data = sdf_file.read()
description_publisher.publish(msg)
clock_system = SystemClock()
builder.AddSystem(clock_system)
# Transform broadcaster for TF frames
tf_broadcaster = TransformBroadcaster(node)
# Connect to system that publishes TF transforms
tf_system = builder.AddSystem(
TFPublisher(tf_broadcaster=tf_broadcaster, joints=joints))
tf_buffer = Buffer(node=node)
tf_listener = TransformListener(tf_buffer, node)
server = InteractiveMarkerServer(node, 'joint_targets')
joint_target_system = builder.AddSystem(
MovableJoints(server, tf_buffer, joints))
fk_system = builder.AddSystem(
ForwardKinematics(station.get_controller_plant()))
builder.Connect(station.GetOutputPort('iiwa_position_measured'),
fk_system.GetInputPort('joint_positions'))
builder.Connect(fk_system.GetOutputPort('transforms'),
tf_system.GetInputPort('body_poses'))
builder.Connect(clock_system.GetOutputPort('clock'),
tf_system.GetInputPort('clock'))
builder.Connect(joint_target_system.GetOutputPort('joint_states'),
station.GetInputPort('iiwa_position'))
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
constant_sys = builder.AddSystem(ConstantVectorSource(numpy.array([0.107
])))
builder.Connect(constant_sys.get_output_port(0),
station.GetInputPort("wsg_position"))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator_context = simulator.get_mutable_context()
simulator.set_target_realtime_rate(1.0)
station_context = station.GetMyMutableContextFromRoot(simulator_context)
num_iiwa_joints = station.num_iiwa_joints()
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, numpy.zeros(num_iiwa_joints))
while simulator_context.get_time() < 12345:
simulator.AdvanceTo(simulator_context.get_time() + 0.01)
# TODO(sloretz) really need a spin_some in rclpy
rclpy.spin_once(node, timeout_sec=0)
rclpy.shutdown()
def connect_to_drake_visualizer(self):
builder = self.builder
sg = self.sg
ConnectDrakeVisualizer(builder, sg, sg.get_pose_bundle_output_port())
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate", type=float, default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument(
"--duration", type=float, default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware", action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument(
"--test", action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
'--setup', type=str, default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing', 'planar'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
if args.hardware:
# TODO(russt): Replace this hard-coded camera serial number with a
# config file.
camera_ids = ["805212060544"]
station = builder.AddSystem(ManipulationStationHardwareInterface(
camera_ids))
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation()
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/"
+ "061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation()
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
elif args.setup == 'planar':
station.SetupPlanarIiwaStation()
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/"
+ "061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
station.Finalize()
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
if args.meshcat:
meshcat = builder.AddSystem(MeshcatVisualizer(
station.get_scene_graph(), zmq_url=args.meshcat,
open_browser=args.open_browser))
builder.Connect(station.GetOutputPort("pose_bundle"),
meshcat.get_input_port(0))
if args.setup == 'planar':
pyplot_visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
station.get_scene_graph()))
builder.Connect(station.GetOutputPort("pose_bundle"),
pyplot_visualizer.get_input_port(0))
teleop = builder.AddSystem(JointSliders(station.get_controller_plant(),
length=800))
if args.test:
teleop.window.withdraw() # Don't display the window when testing.
num_iiwa_joints = station.num_iiwa_joints()
filter = builder.AddSystem(FirstOrderLowPassFilter(
time_constant=2.0, size=num_iiwa_joints))
builder.Connect(teleop.get_output_port(0), filter.get_input_port(0))
builder.Connect(filter.get_output_port(0),
station.GetInputPort("iiwa_position"))
wsg_buttons = builder.AddSystem(SchunkWsgButtons(teleop.window))
builder.Connect(wsg_buttons.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(wsg_buttons.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
# When in regression test mode, log our joint velocities to later check
# that they were sufficiently quiet.
if args.test:
iiwa_velocities = builder.AddSystem(SignalLogger(num_iiwa_joints))
builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"),
iiwa_velocities.get_input_port(0))
else:
iiwa_velocities = None
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(num_iiwa_joints))
# If the diagram is only the hardware interface, then we must advance it a
# little bit so that first LCM messages get processed. A simulated plant is
# already publishing correct positions even without advancing, and indeed
# we must not advance a simulated plant until the sliders and filters have
# been initialized to match the plant.
if args.hardware:
simulator.AdvanceTo(1e-6)
# Eval the output port once to read the initial positions of the IIWA.
q0 = station.GetOutputPort("iiwa_position_measured").Eval(
station_context)
teleop.set_position(q0)
filter.set_initial_output_value(diagram.GetMutableSubsystemContext(
filter, simulator.get_mutable_context()), q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.AdvanceTo(args.duration)
# Ensure that our initialization logic was correct, by inspecting our
# logged joint velocities.
if args.test:
for time, qdot in zip(iiwa_velocities.sample_times(),
iiwa_velocities.data().transpose()):
# TODO(jwnimmer-tri) We should be able to do better than a 40
# rad/sec limit, but that's the best we can enforce for now.
if qdot.max() > 0.1:
print(f"ERROR: large qdot {qdot} at time {time}")
sys.exit(1)
def generate(
model_manifest,
seed_value,
get_num_model_instances,
use_height_heuristic=False,
min_realtime_rate=1.,
max_sim_time=1.,
visualize=False,
visualize_height_heuristic=False,
sim_rate=0.,
sim_dt=0.05,
):
"""Generates poses for a given scene."""
# TOOD(eric.cousineau): This may not be fully deterministic? Random state
# may be leaking in from somewhere else?
seed(seed_value)
timing = pd.DataFrame(np.zeros(1, timing_dtype))
time_construction = make_timer(timing, 'construction')
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=5e-4)
frame_O_list, frame_Sink = add_sink_scene(model_manifest, plant,
scene_graph,
get_num_model_instances)
num_manipulands = len(frame_O_list)
if num_manipulands == 0:
raise RetryError(time_construction(), "No manipulands (empty sink)")
plant.Finalize()
if visualize:
ConnectDrakeVisualizer(builder, scene_graph)
diagram = builder.Build()
d_context = diagram.CreateDefaultContext()
context = plant.GetMyContextFromRoot(d_context)
time_construction()
time_posturing = make_timer(timing, 'posturing')
# Try to sample different configurations that are collision-free.
X_SinkO_list = generate_manipuland_sink_poses(num_manipulands)
set_manipuland_poses(plant, context, frame_Sink, frame_O_list,
X_SinkO_list)
# If there are any existing collisions, reject this sample.
query_object = plant.get_geometry_query_input_port().Eval(context)
if query_object.HasCollisions():
raise RetryError(time_posturing(), f"Collision in initial config")
time_posturing()
# Initilialize simulator here so that the visualization can be initialized
# (if enabled).
simulator = Simulator(diagram, d_context)
simulator.Initialize()
time_projection = make_timer(timing, 'projection')
if use_height_heuristic:
def height_heuristic_show(debug_name):
if visualize_height_heuristic:
print(f" height_heuristic_show: {debug_name}")
diagram.Publish(d_context)
input(f" Press Enter...\n")
# All objects in this pose should be collision free. Record their
# heights.
zs_free = get_frame_heights(plant, context, frame_O_list)
# Specify a configuration that should have collisions. We use
# something slightly below zero because if there are only plates, their
# frame is defined such that z=0 may not collide with the sink.
# We purposely make things collide so that we can briefly search for
# the closest collision free configuration.
zs_colliding = np.full(num_manipulands, -0.01)
zs = height_heuristic(plant,
context,
frame_O_list,
zs_colliding,
zs_free,
show=height_heuristic_show)
# Use returned heights in our context.
set_frame_heights(plant, context, frame_O_list, zs)
diagram.Publish(d_context)
time_projection()
time_simulation = make_timer(timing, 'simulation')
simulator.set_target_realtime_rate(sim_rate)
try:
# TODO(eric.cousineau): Also use settling velocities / height as
# terminating criteria (to reject or use a simulation)?
realtime_rates = []
while d_context.get_time() < max_sim_time:
t_real_start = time.time()
simulator.AdvanceTo(d_context.get_time() + sim_dt)
dt_real = time.time() - t_real_start
realtime_rates.append(sim_dt / dt_real)
if np.mean(realtime_rates) < min_realtime_rate:
raise RetryError(time_simulation(), "Sim too slow")
zs = get_frame_heights(plant, context, frame_O_list)
if np.any(zs < 0.):
raise RetryError(time_simulation(), "Manipuland(s) fell")
except RuntimeError as e:
# Try to mitigate solver failures.
is_solver_failure = ("discrete update solver failed to converge"
in str(e))
if is_solver_failure:
raise RetryError(time_simulation(), "Solver failure")
else:
raise
time_simulation()
# Compute poses w.r.t. sink and return.
X_SinkO_list = []
for i, frame_O in enumerate(frame_O_list):
X_SinkO = plant.CalcRelativeTransform(context, frame_Sink, frame_O)
X_SinkO_list.append(X_SinkO)
return X_SinkO_list, timing