def run_pendulum_example(duration=1.0, playback=True, show=True):
"""
Runs a simulation of a pendulum
Arguments:
duration: Simulation duration (sec).
playback: Enable pyplot animations to be produced.
"""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant)
parser.AddModelFromFile(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"))
plant.Finalize()
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
T_VW = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
T_VW=T_VW,
xlim=[-1.2, 1.2],
ylim=[-1.2, 1.2],
show=show))
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
if playback:
visualizer.start_recording()
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the input port to zero
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant.get_actuation_input_port().FixValue(plant_context,
np.zeros(plant.num_actuators()))
plant_context.SetContinuousState([0.5, 0.1])
simulator.AdvanceTo(duration)
if playback:
visualizer.stop_recording()
ani = visualizer.get_recording_as_animation()
return ani
else:
return None
def test_cart_pole(self):
"""Cart-Pole with simple geometry."""
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
cart_pole, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant=cart_pole).AddModelFromFile(file_name)
cart_pole.Finalize()
assert cart_pole.geometry_source_is_registered()
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL,
open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
cart_pole_context = diagram.GetMutableSubsystemContext(
cart_pole, diagram_context)
cart_pole.get_actuation_input_port().FixValue(cart_pole_context, 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)
visualizer.set_planar_viewpoint(camera_position=[0, -1, 0],
camera_focus=[0, 0, 0],
xmin=-2,
xmax=2,
ymin=-1,
ymax=2)
visualizer.start_recording()
simulator.AdvanceTo(.1)
visualizer.stop_recording()
# Should have animation "clips" for both bodies.
# See https://github.com/rdeits/meshcat-python/blob/c4ef22c84336d6a8eaab682f73bb47cfca5d5779/src/meshcat/animation.py#L100 # noqa
self.assertEqual(len(visualizer._animation.clips), 2)
# After .1 seconds, we should have had 4 publish events.
self.assertEqual(visualizer._recording_frame_num, 4)
visualizer.publish_recording(play=True, repetitions=1)
visualizer.reset_recording()
self.assertEqual(len(visualizer._animation.clips), 0)
def test_diagram_publisher(self):
"""Acceptance tests that a Python LeafSystem is able to output LCM
messages for LcmPublisherSystem to transmit.
"""
lcm = DrakeLcm()
builder = DiagramBuilder()
source = builder.AddSystem(TestSystemsLcm.PythonMessageSource())
publisher = builder.AddSystem(
mut.LcmPublisherSystem.Make(channel="LCMT_HEADER",
lcm_type=lcmt_header,
lcm=lcm,
publish_period=0.05))
builder.Connect(source.get_output_port(), publisher.get_input_port())
diagram = builder.Build()
diagram.Publish(diagram.CreateDefaultContext())
def build_diagram(mbp, scene_graph, meshcat=False):
builder = DiagramBuilder()
builder.AddSystem(scene_graph)
builder.AddSystem(mbp)
# Connect scene_graph to MBP for collision detection.
builder.Connect(
mbp.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(mbp.get_source_id()))
builder.Connect(
scene_graph.get_query_output_port(),
mbp.get_geometry_query_input_port())
if meshcat:
vis = add_meshcat_visualizer(scene_graph, builder)
else:
vis = add_drake_visualizer(scene_graph, builder)
state_log = builder.AddSystem(SignalLogger(mbp.get_continuous_state_output_port().size()))
state_log._DeclarePeriodicPublish(0.02)
builder.Connect(mbp.get_continuous_state_output_port(), state_log.get_input_port(0))
#return builder.Build()
return builder, vis
def create_log(self, vector_system):
builder = DiagramBuilder()
system = builder.AddSystem(vector_system)
logger = builder.AddSystem(VectorLogSink(vector_system.output_size))
builder.Connect(system.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
context = diagram.CreateDefaultContext()
simulator = Simulator(diagram, context)
# Get the log and make sure its original values are as expected.
log = logger.FindLog(context)
return simulator, log, context
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("sdf_path", help="path to sdf")
parser.add_argument("--interactive", action='store_true')
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
plant = MultibodyPlant(time_step=0.01)
plant.RegisterAsSourceForSceneGraph(scene_graph)
parser = Parser(plant)
model = parser.AddModelFromFile(args.sdf_path)
plant.Finalize()
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder, output_port=scene_graph.get_query_output_port(),
zmq_url=args.meshcat, open_browser=args.open_browser)
if args.interactive:
# Add sliders to set positions of the joints.
sliders = builder.AddSystem(JointSliders(robot=plant))
else:
q0 = plant.GetPositions(plant.CreateDefaultContext())
sliders = builder.AddSystem(ConstantVectorSource(q0))
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()))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(1E6)
def test_procedural_geometry(self):
"""
This test ensures we can draw procedurally added primitive
geometry that is added to the world model instance (which has
a slightly different naming scheme than geometry with a
non-default / non-world model instance).
"""
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
world_body = mbp.world_body()
box_shape = Box(1., 2., 3.)
# This rigid body will be added to the world model instance since
# the model instance is not specified.
box_body = mbp.AddRigidBody(
"box",
SpatialInertia(mass=1.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(world_body.body_frame(), box_body.body_frame(),
RigidTransform())
mbp.RegisterVisualGeometry(box_body, RigidTransform.Identity(),
box_shape, "ground_vis",
np.array([0.5, 0.5, 0.5, 1.]))
mbp.RegisterCollisionGeometry(box_body, RigidTransform.Identity(),
box_shape, "ground_col",
CoulombFriction(0.9, 0.8))
mbp.Finalize()
frames_to_draw = {"world": {"box"}}
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
vis_context = diagram.GetMutableSubsystemContext(
visualizer, diagram_context)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(.1)
visualizer.draw(vis_context)
self.assertEqual(
visualizer.ax.get_title(),
"t = 0.1",
)
def run_manipulation_example(args):
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation(time_step=0.005))
station.SetupManipulationClassStation()
station.Finalize()
plant = station.get_multibody_plant()
scene_graph = station.get_scene_graph()
pose_bundle_output_port = station.GetOutputPort("pose_bundle")
# Side-on view of the station.
T_VW = np.array([[1., 0., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]])
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
scene_graph, T_VW=T_VW, xlim=[-0.5, 1.0], ylim=[-1.2, 1.2],
draw_period=0.1))
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
if args.playback:
visualizer.start_recording()
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the control inputs to zero.
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_position").FixValue(
station_context, station.GetIiwaPosition(station_context))
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(7))
station.GetInputPort("wsg_position").FixValue(
station_context, np.zeros(1))
station.GetInputPort("wsg_force_limit").FixValue(
station_context, [40.0])
simulator.AdvanceTo(args.duration)
if args.playback:
visualizer.stop_recording()
ani = visualizer.get_recording_as_animation()
# Playback the recording and save the output.
ani.save("{}/manip_playback.mp4".format(temp_directory()), fps=30)
def test_procedural_geometry_deprecated_api(self):
"""
This test ensures we can draw procedurally added primitive
geometry that is added to the world model instance (which has
a slightly different naming scheme than geometry with a
non-default / non-world model instance).
"""
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
world_body = mbp.world_body()
box_shape = Box(1., 2., 3.)
# This rigid body will be added to the world model instance since
# the model instance is not specified.
box_body = mbp.AddRigidBody(
"box",
SpatialInertia(mass=1.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(world_body.body_frame(), box_body.body_frame(),
RigidTransform())
mbp.RegisterVisualGeometry(box_body, RigidTransform.Identity(),
box_shape, "ground_vis",
np.array([0.5, 0.5, 0.5, 1.]))
mbp.RegisterCollisionGeometry(box_body, RigidTransform.Identity(),
box_shape, "ground_col",
CoulombFriction(0.9, 0.8))
mbp.Finalize()
frames_to_draw = {"world": {"box"}}
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url=ZMQ_URL,
open_browser=False,
frames_to_draw=frames_to_draw))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_publish_every_time_step(False)
with catch_drake_warnings(expected_count=1):
simulator.AdvanceTo(.1)
def make_ball_paddle(contact_model, contact_surface_representation, time_step):
multibody_plant_config = \
MultibodyPlantConfig(
time_step=time_step,
contact_model=contact_model,
contact_surface_representation=contact_surface_representation)
# We pose the paddle, so that its top surface is on World's X-Y plane.
# Intuitively we push it down 1 cm because the box is 2 cm thick.
p_WPaddle_fixed = RigidTransform(RollPitchYaw(0, 0, 0),
np.array([0, 0, -0.01]))
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlant(multibody_plant_config, builder)
parser = Parser(plant)
paddle_sdf_file_name = \
FindResourceOrThrow("drake/examples/hydroelastic/python_ball_paddle"
"/paddle.sdf")
paddle = parser.AddModelFromFile(paddle_sdf_file_name, model_name="paddle")
plant.WeldFrames(frame_on_parent_F=plant.world_frame(),
frame_on_child_M=plant.GetFrameByName("paddle", paddle),
X_FM=p_WPaddle_fixed)
ball_sdf_file_name = \
FindResourceOrThrow("drake/examples/hydroelastic/python_ball_paddle"
"/ball.sdf")
parser.AddModelFromFile(ball_sdf_file_name)
# TODO(DamrongGuoy): Let users override hydroelastic modulus, dissipation,
# and resolution hint from the two SDF files above.
plant.Finalize()
DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph)
ConnectContactResultsToDrakeVisualizer(builder=builder,
plant=plant,
scene_graph=scene_graph)
nx = plant.num_positions() + plant.num_velocities()
state_logger = builder.AddSystem(VectorLogSink(nx))
builder.Connect(plant.get_state_output_port(),
state_logger.get_input_port())
diagram = builder.Build()
return diagram, plant, state_logger
def show_cloud(pc, pc2=None, use_native=False, **kwargs):
# kwargs go to ctor.
builder = DiagramBuilder()
# Add point cloud visualization.
if use_native:
viz = meshcat.Visualizer(zmq_url=ZMQ_URL)
else:
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
plant.Finalize()
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))
pc_viz = builder.AddSystem(
MeshcatPointCloudVisualizer(viz, **kwargs))
if pc2:
pc_viz2 = builder.AddSystem(
MeshcatPointCloudVisualizer(viz,
name='second_point_cloud',
X_WP=se3_from_xyz([0, 0.3, 0]),
default_rgb=[0., 255., 0.]))
# Make sure the system runs.
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
context = diagram.GetMutableSubsystemContext(
pc_viz, diagram_context)
# TODO(eric.cousineau): Replace `AbstractValue.Make(pc)` with just
# `pc` (#12086).
pc_viz.GetInputPort("point_cloud_P").FixValue(
context, AbstractValue.Make(pc))
if pc2:
context = diagram.GetMutableSubsystemContext(
pc_viz2, diagram_context)
pc_viz2.GetInputPort("point_cloud_P").FixValue(
context, AbstractValue.Make(pc2))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(sim_time)
def test_kuka(self):
"""Kuka IIWA with mesh geometry."""
file_name = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/"
"iiwa14_no_collision.sdf")
builder = DiagramBuilder()
kuka, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant=kuka).AddModelFromFile(file_name)
kuka.Finalize()
# Make sure that the frames to visualize exist.
iiwa = kuka.GetModelInstanceByName("iiwa14")
kuka.GetFrameByName("iiwa_link_7", iiwa)
kuka.GetFrameByName("iiwa_link_6", iiwa)
frames_to_draw = {"iiwa14": {"iiwa_link_7", "iiwa_link_6"}}
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
kuka_context = diagram.GetMutableSubsystemContext(
kuka, diagram_context)
vis_context = diagram.GetMutableSubsystemContext(
visualizer, diagram_context)
kuka_actuation_port = kuka.get_actuation_input_port()
kuka_actuation_port.FixValue(kuka_context,
np.zeros(kuka_actuation_port.size()))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(.1)
visualizer.draw(vis_context)
self.assertEqual(
visualizer.ax.get_title(),
"t = 0.1",
)
def test_kuka(self):
"""Kuka IIWA with mesh geometry."""
file_name = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/"
"iiwa14_no_collision.sdf")
builder = DiagramBuilder()
kuka, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant=kuka).AddModelFromFile(file_name)
kuka.Finalize()
frames_name_list = ["iiwa_link_7", "iiwa_link_6"]
frames_to_draw = []
for frame_name in frames_name_list:
frames_to_draw.append(
# Make sure the frames to visualize exists.
kuka.GetBodyFrameIdOrThrow(
kuka.GetBodyByName(frame_name).index()))
visualizer = builder.AddSystem(
MeshcatVisualizer(zmq_url=ZMQ_URL,
open_browser=False,
frames_to_draw=frames_to_draw))
builder.Connect(scene_graph.get_query_output_port(),
visualizer.get_geometry_query_input_port())
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
kuka_context = diagram.GetMutableSubsystemContext(
kuka, diagram_context)
kuka_actuation_port = kuka.get_actuation_input_port()
kuka_actuation_port.FixValue(kuka_context,
np.zeros(kuka_actuation_port.size()))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(.1)
def test_cart_pole(self):
"""Cart-Pole with simple geometry."""
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
cart_pole, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant=cart_pole).AddModelFromFile(file_name)
cart_pole.Finalize()
self.assertTrue(cart_pole.geometry_source_is_registered())
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
cart_pole_context = diagram.GetMutableSubsystemContext(
cart_pole, diagram_context)
vis_context = diagram.GetMutableSubsystemContext(
visualizer, diagram_context)
cart_pole.get_actuation_input_port().FixValue(cart_pole_context, 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)
with catch_drake_warnings(expected_count=1):
simulator.AdvanceTo(.1)
visualizer.draw(vis_context)
self.assertEqual(
visualizer.ax.get_title(),
"t = 0.1",
)
def test_adder_simulation(self):
builder = DiagramBuilder()
adder = builder.AddSystem(self._create_adder_system())
adder.set_name("custom_adder")
# Add ZOH so we can easily extract state.
zoh = builder.AddSystem(ZeroOrderHold(0.1, 3))
zoh.set_name("zoh")
builder.ExportInput(adder.get_input_port(0))
builder.ExportInput(adder.get_input_port(1))
builder.Connect(adder.get_output_port(0), zoh.get_input_port(0))
diagram = builder.Build()
context = diagram.CreateDefaultContext()
self._fix_adder_inputs(context)
simulator = Simulator(diagram, context)
simulator.Initialize()
simulator.AdvanceTo(1)
# Ensure that we have the outputs we want.
value = (diagram.GetMutableSubsystemContext(
zoh, context).get_discrete_state_vector().get_value())
self.assertTrue(np.allclose([5, 7, 9], value))
def run_pendulum_example(args):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant)
parser.AddModelFromFile(FindResourceOrThrow(
"drake/examples/pendulum/Pendulum.urdf"))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"))
plant.Finalize()
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
T_VW = np.array([[1., 0., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]])
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
scene_graph, T_VW=T_VW, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2]))
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
if args.playback:
visualizer.start_recording()
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the input port to zero.
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.FixInputPort(
plant.get_actuation_input_port().get_index(),
np.zeros(plant.num_actuators()))
plant_context.SetContinuousState([0.5, 0.1])
simulator.AdvanceTo(args.duration)
if args.playback:
visualizer.stop_recording()
ani = visualizer.get_recording_as_animation()
# Playback the recording and save the output.
ani.save("{}/pend_playback.mp4".format(temp_directory()), fps=30)
def test_kuka_deprecated_api(self):
"""Kuka IIWA with mesh geometry."""
file_name = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/"
"iiwa14_no_collision.sdf")
builder = DiagramBuilder()
kuka, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant=kuka).AddModelFromFile(file_name)
kuka.Finalize()
# Make sure that the frames to visualize exist.
kuka.GetModelInstanceByName("iiwa14")
kuka.GetFrameByName("iiwa_link_7")
kuka.GetFrameByName("iiwa_link_6")
frames_to_draw = {"iiwa14": {"iiwa_link_7", "iiwa_link_6"}}
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url=ZMQ_URL,
open_browser=False,
frames_to_draw=frames_to_draw))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
kuka_context = diagram.GetMutableSubsystemContext(
kuka, diagram_context)
kuka_actuation_port = kuka.get_actuation_input_port()
kuka_actuation_port.FixValue(kuka_context,
np.zeros(kuka_actuation_port.size()))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
with catch_drake_warnings(expected_count=1):
simulator.AdvanceTo(.1)
def __init__(self, is_visualizing=False):
self.station = ManipulationStation()
self.station.SetupManipulationClassStation(
IiwaCollisionModel.kBoxCollision)
self.station.Finalize()
self.plant = self.station.get_mutable_multibody_plant()
self.scene_graph = self.station.get_mutable_scene_graph()
self.is_visualizing = is_visualizing
# scene graph query output port.
self.query_output_port = self.scene_graph.GetOutputPort("query")
builder = DiagramBuilder()
builder.AddSystem(self.station)
# meshcat visualizer
if is_visualizing:
self.viz = MeshcatVisualizer(self.scene_graph,
zmq_url="tcp://127.0.0.1:6000")
# connect meshcat to manipulation station
builder.AddSystem(self.viz)
builder.Connect(self.station.GetOutputPort("pose_bundle"),
self.viz.GetInputPort("lcm_visualization"))
self.diagram = builder.Build()
# contexts
self.context_diagram = self.diagram.CreateDefaultContext()
self.context_station = self.diagram.GetSubsystemContext(
self.station, self.context_diagram)
self.context_scene_graph = self.station.GetSubsystemContext(
self.scene_graph, self.context_station)
self.context_plant = self.station.GetMutableSubsystemContext(
self.plant, self.context_station)
if is_visualizing:
self.viz.load()
self.context_meshcat = self.diagram.GetSubsystemContext(
self.viz, self.context_diagram)
def test_custom_geometry_name_parsing(self):
"""
Ensure that name parsing does not fail on programmatically added
anchored geometries.
"""
# Make a minimal example to ensure MeshcatVisualizer loads anchored
# geometry.
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
visualizer = builder.AddSystem(
MeshcatVisualizer(zmq_url=ZMQ_URL, open_browser=False))
builder.Connect(scene_graph.get_query_output_port(),
visualizer.get_geometry_query_input_port())
source_id = scene_graph.RegisterSource()
geom_inst = GeometryInstance(RigidTransform(), Box(1., 1., 1.), "box")
geom_id = scene_graph.RegisterAnchoredGeometry(source_id, geom_inst)
# Illustration properties required to ensure geometry is parsed
scene_graph.AssignRole(source_id, geom_id, IllustrationProperties())
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
def run_manipulation_example(args):
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation(time_step=0.005))
station.SetupManipulationClassStation()
station.Finalize()
plant = station.get_multibody_plant()
scene_graph = station.get_scene_graph()
pose_bundle_output_port = station.GetOutputPort("pose_bundle")
# Side-on view of the station.
Tview = np.array([[1., 0., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
scene_graph, T_VW=Tview, xlim=[-0.5, 1.0], ylim=[-1.2, 1.2],
draw_period=0.1))
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the control inputs to zero.
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_position").FixValue(
station_context, station.GetIiwaPosition(station_context))
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(7))
station.GetInputPort("wsg_position").FixValue(
station_context, np.zeros(1))
station.GetInputPort("wsg_force_limit").FixValue(
station_context, [40.0])
simulator.AdvanceTo(args.duration)
"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()
options.visualization_callback = draw
policy, cost_to_go = FittedValueIteration(simulator, cost_function, state_grid,
input_grid, timestep, options)
J = np.reshape(cost_to_go, Q.shape)
surf = ax.plot_surface(Q, Qdot, J, rstride=1, cstride=1, cmap=cm.jet)
Pi = np.reshape(policy.get_output_values(), Q.shape)
surf = ax2.plot_surface(Q, Qdot, Pi, rstride=1, cstride=1, cmap=cm.jet)
# animate the resulting policy.
builder = DiagramBuilder()
plant = builder.AddSystem(DoubleIntegrator())
logger = LogOutput(plant.get_output_port(0), builder)
vi_policy = builder.AddSystem(policy)
builder.Connect(vi_policy.get_output_port(0), plant.get_input_port(0))
builder.Connect(plant.get_output_port(0), vi_policy.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
state = simulator.get_mutable_context().SetContinuousState([-10.0, 0.0])
simulator.AdvanceTo(10.)
# Visualize the result as a video.
vis = DoubleIntegratorVisualizer()
ani = vis.animate(logger, repeat=True)
plt.show()
def test_diagram_simulation(self):
# Similar to: //systems/framework:diagram_test, ExampleDiagram
size = 3
builder = DiagramBuilder()
adder0 = builder.AddSystem(Adder(2, size))
adder0.set_name("adder0")
adder1 = builder.AddSystem(Adder(2, size))
adder1.set_name("adder1")
integrator = builder.AddSystem(Integrator(size))
integrator.set_name("integrator")
builder.Connect(adder0.get_output_port(0), adder1.get_input_port(0))
builder.Connect(adder1.get_output_port(0),
integrator.get_input_port(0))
builder.ExportInput(adder0.get_input_port(0))
builder.ExportInput(adder0.get_input_port(1))
builder.ExportInput(adder1.get_input_port(1))
builder.ExportOutput(integrator.get_output_port(0))
diagram = builder.Build()
# TODO(eric.cousineau): Figure out unicode handling if needed.
# See //systems/framework/test/diagram_test.cc:349 (sha: bc84e73)
# for an example name.
diagram.set_name("test_diagram")
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# Create and attach inputs.
# TODO(eric.cousineau): Not seeing any assertions being printed if no
# inputs are connected. Need to check this behavior.
input0 = np.array([0.1, 0.2, 0.3])
context.FixInputPort(0, input0)
input1 = np.array([0.02, 0.03, 0.04])
context.FixInputPort(1, input1)
input2 = BasicVector([0.003, 0.004, 0.005])
context.FixInputPort(2, input2) # Test the BasicVector overload.
# Initialize integrator states.
integrator_xc = (
diagram.GetMutableSubsystemState(integrator, context)
.get_mutable_continuous_state().get_vector())
integrator_xc.SetFromVector([0, 1, 2])
simulator.Initialize()
# Simulate briefly, and take full-context snapshots at intermediate
# points.
n = 6
times = np.linspace(0, 1, n)
context_log = []
for t in times:
simulator.StepTo(t)
# Record snapshot of *entire* context.
context_log.append(context.Clone())
xc_initial = np.array([0, 1, 2])
xc_final = np.array([0.123, 1.234, 2.345])
for i, context_i in enumerate(context_log):
t = times[i]
self.assertEqual(context_i.get_time(), t)
xc = context_i.get_continuous_state_vector().CopyToVector()
xc_expected = (float(i) / (n - 1) * (xc_final - xc_initial) +
xc_initial)
print("xc[t = {}] = {}".format(t, xc))
self.assertTrue(np.allclose(xc, xc_expected))
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)
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"))
diagram = builder.Build()
simulator = Simulator(diagram)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station_context.FixInputPort(
station.GetInputPort("iiwa_feedforward_torque").get_index(), np.zeros(7))
def main():
args_parser = argparse.ArgumentParser(
description=__doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
add_filename_and_parser_argparse_arguments(args_parser)
add_visualizers_argparse_arguments(args_parser)
args_parser.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.")
# TODO(eric.cousineau): Support sliders (or widgets) for floating body
# poses.
# TODO(russt): Once floating body sliders are supported, add an option to
# disable them too, either by welding via GetUniqueBaseBody #9747 or by
# hiding the widgets.
args_parser.add_argument(
"--test", action='store_true',
help="Disable opening the slider gui window for testing.")
args = args_parser.parse_args()
# NOTE: meshcat is required to create the JointSliders.
args.meshcat = True
filename, make_parser = parse_filename_and_parser(args_parser, args)
update_visualization, connect_visualizers = parse_visualizers(
args_parser, args)
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# Construct a MultibodyPlant.
# N.B. Do not use AddMultibodyPlantSceneGraph because we want to inject our
# custom pose-bundle adjustments for the sliders.
plant = MultibodyPlant(time_step=0.0)
plant.RegisterAsSourceForSceneGraph(scene_graph)
# Add the model from the file and finalize the plant.
make_parser(plant).AddModelFromFile(filename)
update_visualization(plant, scene_graph)
plant.Finalize()
meshcat = connect_visualizers(builder, plant, scene_graph)
assert meshcat is not None, "Meshcat visualizer not created but required."
# Add sliders to set positions of the joints.
sliders = builder.AddSystem(JointSliders(meshcat=meshcat, plant=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()))
if len(args.position):
sliders.SetPositions(args.position)
# Make the diagram and run it.
diagram = builder.Build()
simulator = Simulator(diagram)
if args.test:
simulator.AdvanceTo(0.1)
else:
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(np.inf)
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)
# y_traj.add_constraint(t=0, derivative_order=2, lb=[0, 0, 0]) # initial acceleration
# y_traj.add_constraint(t=1, derivative_order=0, lb=[0, 1.0, 0])
# y_traj.add_constraint(t=2, derivative_order=0, lb=[0, 2.0, 0])
# y_traj.add_constraint(t=3, derivative_order=0, lb=[0, 3.0, 0])
# y_traj.add_constraint(t=tf, derivative_order=0, lb=[0, 4.0, 0]) # end at zero
# y_traj.add_constraint(t=tf, derivative_order=1, lb=[0, 0, 0])
# y_traj.add_constraint(t=tf, derivative_order=2, lb=[0, 0, 0])
# y_traj.generate()
# Build
builder = DiagramBuilder()
plant = builder.AddSystem(QuadrotorPlant())
controller = builder.AddSystem(QuadrotorController(plant, y_traj, DURATION))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
# Set up visualization and env in MeshCat
from pydrake.geometry import Box
from pydrake.common.eigen_geometry import Isometry3
from pydrake.all import AddMultibodyPlantSceneGraph
from pydrake.multibody.tree import SpatialInertia, UnitInertia
env_plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
for idx, object_ in enumerate(OBJECTS):
sz, trans, rot = object_
body = env_plant.AddRigidBody(
str(idx + 1),
SpatialInertia(1.0, np.zeros(3), UnitInertia(1.0, 1.0, 1.0)))
pos = Isometry3()
pos.set_translation(trans) # np.array([0,1,1])
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--num_samples",
type=int,
default=50,
help="Number of Monte Carlo samples to use to estimate performance.")
parser.add_argument("--torque_limit",
type=float,
default=2.0,
help="Torque limit of the pendulum.")
args = parser.parse_args()
if args.torque_limit < 0:
raise InvalidArgumentError("Please supply a nonnegative torque limit.")
# Assemble the Pendulum plant.
builder = DiagramBuilder()
pendulum = builder.AddSystem(MultibodyPlant(0.0))
file_name = FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf")
Parser(pendulum).AddModelFromFile(file_name)
pendulum.WeldFrames(pendulum.world_frame(),
pendulum.GetFrameByName("base"))
pendulum.Finalize()
# Set the pendulum to start at uniformly random
# positions (but always zero velocity).
elbow = pendulum.GetMutableJointByName("theta")
upright_theta = np.pi
theta_expression = Variable(name="theta",
type=Variable.Type.RANDOM_UNIFORM) * 2. * np.pi
elbow.set_random_angle_distribution(theta_expression)
# Set up LQR, with high position gains to try to ensure the
# ROA is close to the theoretical torque-limited limit.
Q = np.diag([100., 1.])
R = np.identity(1) * 0.01
linearize_context = pendulum.CreateDefaultContext()
linearize_context.SetContinuousState(np.array([upright_theta, 0.]))
actuation_port = pendulum.get_actuation_input_port()
actuation_port.FixValue(linearize_context, 0)
controller = builder.AddSystem(
LinearQuadraticRegulator(pendulum, linearize_context, Q, R,
np.zeros(0), actuation_port.get_index()))
# Apply the torque limit.
torque_limit = args.torque_limit
torque_limiter = builder.AddSystem(
Saturation(min_value=np.array([-torque_limit]),
max_value=np.array([torque_limit])))
builder.Connect(controller.get_output_port(0),
torque_limiter.get_input_port(0))
builder.Connect(torque_limiter.get_output_port(0),
pendulum.get_actuation_input_port())
builder.Connect(pendulum.get_state_output_port(),
controller.get_input_port(0))
diagram = builder.Build()
# Perform the Monte Carlo simulation.
def make_simulator(generator):
''' Create a simulator for the system
using the given generator. '''
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(0)
simulator.Initialize()
return simulator
def calc_wrapped_error(system, context):
''' Given a context from the end of the simulation,
calculate an error -- which for this stabilizing
task is the distance from the
fixed point. '''
state = diagram.GetSubsystemContext(
pendulum, context).get_continuous_state_vector()
error = state.GetAtIndex(0) - upright_theta
# Wrap error to [-pi, pi].
return (error + np.pi) % (2 * np.pi) - np.pi
num_samples = args.num_samples
results = MonteCarloSimulation(make_simulator=make_simulator,
output=calc_wrapped_error,
final_time=1.0,
num_samples=num_samples,
generator=RandomGenerator())
# Compute results.
# The "success" region is fairly large since some "stabilized" trials
# may still be oscillating around the fixed point. Failed examples are
# consistently much farther from the fixed point than this.
binary_results = np.array([abs(res.output) < 0.1 for res in results])
passing_ratio = float(sum(binary_results)) / len(results)
# 95% confidence interval for the passing ratio.
passing_ratio_var = 1.96 * np.sqrt(passing_ratio *
(1. - passing_ratio) / len(results))
print("Monte-Carlo estimated performance across %d samples: "
"%.2f%% +/- %0.2f%%" %
(num_samples, passing_ratio * 100, passing_ratio_var * 100))
# Analytically compute the best possible ROA, for comparison, but
# calculating where the torque needed to lift the pendulum exceeds
# the torque limit.
arm_radius = 0.5
arm_mass = 1.0
# torque = r x f = r * (m * 9.81 * sin(theta))
# theta = asin(torque / (r * m))
if torque_limit <= (arm_radius * arm_mass * 9.81):
roa_half_width = np.arcsin(torque_limit /
(arm_radius * arm_mass * 9.81))
else:
roa_half_width = np.pi
roa_as_fraction_of_state_space = roa_half_width / np.pi
print("Max possible ROA = %0.2f%% of state space, which should"
" match with the above estimate." %
(100 * roa_as_fraction_of_state_space))
station = builder.AddSystem(
ManipulationStationHardwareInterface(camera_ids))
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
station.SetupDefaultStation()
station.Finalize()
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
if args.meshcat:
meshcat = builder.AddSystem(
MeshcatVisualizer(station.get_scene_graph(),
zmq_url=args.meshcat,
open_browser=args.open_browser))
builder.Connect(station.GetOutputPort("pose_bundle"),
meshcat.get_input_port(0))
teleop = builder.AddSystem(
JointSliders(station.get_controller_plant(), length=800))
if args.test:
teleop.window.withdraw() # Don't display the window when testing.
filter = builder.AddSystem(FirstOrderLowPassFilter(time_constant=2.0, size=7))
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"),
