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.Finalize()
T_VW = np.array([[1., 0., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]])
visualizer = ConnectPlanarSceneGraphVisualizer(
builder, scene_graph, T_VW=T_VW, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2])
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.get_actuation_input_port().FixValue(
plant_context, 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_simulator_ctor(self):
# Create simple system.
system = ConstantVectorSource([1])
def check_output(context):
# Check number of output ports and value for a given context.
output = system.AllocateOutput(context)
self.assertEquals(output.get_num_ports(), 1)
system.CalcOutput(context, output)
value = output.get_vector_data(0).get_value()
self.assertTrue(np.allclose([1], value))
# Create simulator with basic constructor.
simulator = Simulator(system)
simulator.Initialize()
simulator.set_target_realtime_rate(0)
simulator.set_publish_every_time_step(True)
self.assertTrue(
simulator.get_context() is simulator.get_mutable_context())
check_output(simulator.get_context())
simulator.StepTo(1)
# Create simulator specifying context.
context = system.CreateDefaultContext()
# @note `simulator` now owns `context`.
simulator = Simulator(system, context)
self.assertTrue(simulator.get_context() is context)
check_output(context)
simulator.StepTo(1)
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())
meshcat = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url=ZMQ_URL,
open_browser=False))
builder.Connect(
scene_graph.get_pose_bundle_output_port(),
meshcat.get_input_port(0))
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 simulate_diagram(diagram, ball_paddle_plant, state_logger,
ball_init_position, ball_init_velocity, simulation_time,
target_realtime_rate):
q_init_val = np.array([
1, 0, 0, 0, ball_init_position[0], ball_init_position[1],
ball_init_position[2]
])
v_init_val = np.hstack((np.zeros(3), ball_init_velocity))
qv_init_val = np.concatenate((q_init_val, v_init_val))
simulator_config = SimulatorConfig(
target_realtime_rate=target_realtime_rate,
publish_every_time_step=True)
simulator = Simulator(diagram)
ApplySimulatorConfig(simulator_config, simulator)
plant_context = diagram.GetSubsystemContext(ball_paddle_plant,
simulator.get_context())
ball_paddle_plant.SetPositionsAndVelocities(plant_context, qv_init_val)
simulator.get_mutable_context().SetTime(0)
state_log = state_logger.FindMutableLog(simulator.get_mutable_context())
state_log.Clear()
simulator.Initialize()
simulator.AdvanceTo(boundary_time=simulation_time)
PrintSimulatorStatistics(simulator)
return state_log.sample_times(), state_log.data()
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()
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=[-1.2, 1.2],
ylim=[-1.2, 1.2]))
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 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)
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 RunSimulation(self, real_time_rate=1.0):
'''
The Princess Diaries was a good movie.
'''
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# object_file_path = FindResourceOrThrow(
# "drake/examples/manipulation_station/models/061_foam_brick.sdf")
# sdf_file = FindResourceOrThrow("drake/multibody/benchmarks/acrobot/acrobot.sdf")
# urdf_file = FindResourceOrThrow("drake/multibody/benchmarks/acrobot/acrobot.urdf")
sdf_file = "assets/acrobot.sdf"
urdf_file = "assets/acrobot.urdf"
plant = builder.AddSystem(MultibodyPlant())
plant.RegisterAsSourceForSceneGraph(scene_graph)
Parser(plant, scene_graph).AddModelFromFile(sdf_file)
plant.Finalize(scene_graph)
assert plant.geometry_source_is_registered()
builder.Connect(
plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
builder.Connect(scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
# Add
nn_system = NNSystem(self.pytorch_nn_object)
builder.AddSystem(nn_system)
# NN -> plant
builder.Connect(nn_system.NN_out_output_port,
plant.get_actuation_input_port())
# plant -> NN
builder.Connect(plant.get_continuous_state_output_port(),
nn_system.NN_in_input_port)
# Add meshcat visualizer
meshcat = MeshcatVisualizer(scene_graph)
builder.AddSystem(meshcat)
# builder.Connect(scene_graph.GetOutputPort("lcm_visualization"),
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.GetInputPort("lcm_visualization"))
# build diagram
diagram = builder.Build()
meshcat.load()
# time.sleep(2.0)
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
# context = diagram.GetMutableSubsystemContext(
# self.station, simulator.get_mutable_context())
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
sim_duration = 5.
simulator.StepTo(sim_duration)
print("stepping complete")
def test_simulator_integrator_manipulation(self):
# TODO(eric.cousineau): Move this to `analysis_test.py`.
system = ConstantVectorSource([1])
# Create simulator with basic constructor.
simulator = Simulator(system)
simulator.Initialize()
simulator.set_target_realtime_rate(0)
integrator = simulator.get_mutable_integrator()
target_accuracy = 1E-6
integrator.set_target_accuracy(target_accuracy)
self.assertEqual(integrator.get_target_accuracy(), target_accuracy)
maximum_step_size = 0.2
integrator.set_maximum_step_size(maximum_step_size)
self.assertEqual(integrator.get_maximum_step_size(), maximum_step_size)
minimum_step_size = 2E-2
integrator.set_requested_minimum_step_size(minimum_step_size)
self.assertEqual(integrator.get_requested_minimum_step_size(),
minimum_step_size)
integrator.set_throw_on_minimum_step_size_violation(True)
self.assertTrue(integrator.get_throw_on_minimum_step_size_violation())
integrator.set_fixed_step_mode(True)
self.assertTrue(integrator.get_fixed_step_mode())
const_integrator = simulator.get_integrator()
self.assertTrue(const_integrator is integrator)
def test_subgraph_add_to_copying(self):
"""Ensures that index ordering is generally the same when copying a
plant using a MultibodyPlantSubgraph.add_to."""
# TODO(eric.cousineau): Increas number of bodies for viz, once
# `create_manual_map` can acommodate it.
plant_a, scene_graph_a, _ = self.make_arbitrary_multibody_stuff(
num_bodies=1)
# Check for general ordering with full subgraph "cloning".
builder_b = DiagramBuilder()
plant_b, scene_graph_b = AddMultibodyPlantSceneGraph(
builder_b, plant_a.time_step())
subgraph_a = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(plant_a, scene_graph_a))
a_to_b = 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)
a_to_b_expected = self.create_manual_map(plant_a, scene_graph_a,
plant_b, scene_graph_b)
self.assertEqual(a_to_b, a_to_b_expected)
if VISUALIZE:
for model in me.get_model_instances(plant_b):
util.build_with_no_control(builder_b, plant_b, model)
print("test_subgraph_add_to_copying")
DrakeVisualizer.AddToBuilder(builder_b, scene_graph_b)
diagram = builder_b.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.)
simulator.Initialize()
diagram.Publish(simulator.get_context())
simulator.AdvanceTo(1.)
def test_lcm_interface_system_diagram(self):
# First, check the class doc.
self.assertIn("only inherits from LeafSystem",
mut.LcmInterfaceSystem.__doc__)
# Next, construct a diagram and add both the interface system and
# a subscriber.
builder = DiagramBuilder()
lcm = DrakeLcm()
lcm_system = builder.AddSystem(mut.LcmInterfaceSystem(lcm=lcm))
# Create subscriber in the diagram.
subscriber = builder.AddSystem(
mut.LcmSubscriberSystem.Make(channel="TEST_CHANNEL",
lcm_type=lcmt_quaternion,
lcm=lcm))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
# Publish test message.
model_message = self._model_message()
lcm.Publish("TEST_CHANNEL", model_message.encode())
# Simulate to a non-zero time to ensure the subscriber picks up the
# message.
eps = np.finfo(float).eps
simulator.AdvanceTo(eps)
# Ensure that we have what we want.
context = subscriber.GetMyContextFromRoot(
simulator.get_mutable_context())
actual_message = subscriber.get_output_port(0).Eval(context)
self.assert_lcm_equal(actual_message, model_message)
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))
AddModelFromSdfFile(file_name=file_name,
plant=cart_pole,
scene_graph=scene_graph)
cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
cart_pole.Finalize(scene_graph)
assert cart_pole.geometry_source_is_registered()
builder.Connect(
cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
lcm = DrakeLcm()
ConnectVisualization(scene_graph=scene_graph, builder=builder, lcm=lcm)
diagram = builder.Build()
DispatchLoadMessage(scene_graph=scene_graph, lcm=lcm)
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 pendulum_example(duration=1., playback=True, show=True):
"""
Simulate the pendulum
Arguments:
duration: Simulation duration (sec)
playback: enable pyplot animations
"""
# To make a visualization, we have to attach a multibody plant, a scene graph, and a visualizer together. In Drake, we can connect all these systems together in a Diagram.
builder = DiagramBuilder()
# AddMultibodyPlantSceneGraph: Adds a multibody plant and scene graph to the Diagram, and connects their geometry ports. The second input is the timestep for MultibodyPlant
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.)
# Now we create the plant model from a file
parser = Parser(plant)
parser.AddModelFromFile(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"))
plant.Finalize()
# The SceneGraph port that communicates with the visualizer is the pose bundle output port. A PoseBundle is just a set of poses in SE(3) and a set of frame velocities, expressed in the world frame, used for rendering.
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
# T_VW is the projection matrix from view coordinates to world coordinates
T_VW = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])
# Now we add a planar visualizer to the the diagram, so we can see the results
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
T_VW=T_VW,
xlim=[-1.2, 1.2],
ylim=[-1.2, 1.2],
show=show))
# finally, we must connect the scene_graph to the visualizer so they can communicate
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
if playback:
visualizer.start_recording()
# To finalize the diagram, we build it
diagram = builder.Build()
# We create a simulator of our diagram to step through the diagram in time
simulator = Simulator(diagram)
# Initialize prepares the simulator for simulation
simulator.Initialize()
# Slow down the simulator to realtime. Otherwise it could run too fast
simulator.set_target_realtime_rate(1.)
# To set initial conditions, we modify the mutable simulator context (we could do this before Initialize)
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.SetContinuousState([0.5, 0.1])
# Now we fix the value of the actuation to get an unactuated simulation
plant.get_actuation_input_port().FixValue(
plant_context, np.zeros([plant.num_actuators()]))
# Run the simulation to the specified duration
simulator.AdvanceTo(duration)
# Return an animation, if one was made
if playback:
visualizer.stop_recording()
ani = visualizer.get_recording_as_animation()
return ani
else:
return None
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.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()))
DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph)
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)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.Initialize()
simulator.AdvanceTo(args.simulation_time)
def simulate_splines(diagram, diagram_context, sim_duration, real_time_rate=1.0):
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
diagram.Publish(diagram_context)
user_input('Simulate?')
simulator.StepTo(sim_duration)
user_input('Finish?')
def test_simulator_integrator_manipulation(self):
# TODO(eric.cousineau): Move this to `analysis_test.py`.
system = ConstantVectorSource([1])
# Create simulator with basic constructor.
simulator = Simulator(system)
simulator.Initialize()
simulator.set_target_realtime_rate(0)
integrator = simulator.get_mutable_integrator()
target_accuracy = 1E-6
integrator.set_target_accuracy(target_accuracy)
self.assertEqual(integrator.get_target_accuracy(), target_accuracy)
maximum_step_size = 0.2
integrator.set_maximum_step_size(maximum_step_size)
self.assertEqual(integrator.get_maximum_step_size(), maximum_step_size)
minimum_step_size = 2E-2
integrator.set_requested_minimum_step_size(minimum_step_size)
self.assertEqual(integrator.get_requested_minimum_step_size(),
minimum_step_size)
integrator.set_throw_on_minimum_step_size_violation(True)
self.assertTrue(integrator.get_throw_on_minimum_step_size_violation())
integrator.set_fixed_step_mode(True)
self.assertTrue(integrator.get_fixed_step_mode())
const_integrator = simulator.get_integrator()
self.assertTrue(const_integrator is integrator)
# Test context-less constructors for
# integrator types.
test_integrator = RungeKutta2Integrator(
system=system, max_step_size=0.01)
test_integrator = RungeKutta3Integrator(system=system)
# Test simulator's reset_integrator, and also the full constructors for
# all integrator types.
rk2 = RungeKutta2Integrator(
system=system,
max_step_size=0.01,
context=simulator.get_mutable_context())
with catch_drake_warnings(expected_count=1):
# TODO(12873) We need an API for this that isn't deprecated.
simulator.reset_integrator(rk2)
rk3 = RungeKutta3Integrator(
system=system,
context=simulator.get_mutable_context())
with catch_drake_warnings(expected_count=1):
# TODO(12873) We need an API for this that isn't deprecated.
simulator.reset_integrator(rk3)
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_simulator_integrator_manipulation(self):
system = ConstantVectorSource([1])
# Create simulator with basic constructor.
simulator = Simulator(system)
simulator.Initialize()
simulator.set_target_realtime_rate(0)
integrator = simulator.get_mutable_integrator()
target_accuracy = 1E-6
integrator.set_target_accuracy(target_accuracy)
self.assertEqual(integrator.get_target_accuracy(), target_accuracy)
maximum_step_size = 0.2
integrator.set_maximum_step_size(maximum_step_size)
self.assertEqual(integrator.get_maximum_step_size(), maximum_step_size)
minimum_step_size = 2E-2
integrator.set_requested_minimum_step_size(minimum_step_size)
self.assertEqual(integrator.get_requested_minimum_step_size(),
minimum_step_size)
integrator.set_throw_on_minimum_step_size_violation(True)
self.assertTrue(integrator.get_throw_on_minimum_step_size_violation())
integrator.set_fixed_step_mode(True)
self.assertTrue(integrator.get_fixed_step_mode())
const_integrator = simulator.get_integrator()
self.assertTrue(const_integrator is integrator)
# Test context-less constructors for
# integrator types.
test_integrator = RungeKutta2Integrator(
system=system, max_step_size=0.01)
test_integrator = RungeKutta3Integrator(system=system)
# Test simulator's reset_integrator,
# and also the full constructors for
# all integrator types.
simulator.reset_integrator(
RungeKutta2Integrator(
system=system,
max_step_size=0.01,
context=simulator.get_mutable_context()))
simulator.reset_integrator(
RungeKutta3Integrator(
system=system,
context=simulator.get_mutable_context()))
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 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 __init__(self, tree):
lcm = DrakeLcm()
self.tree = tree
self.visualizer = DrakeVisualizer(tree=self.tree,
lcm=lcm,
enable_playback=True)
self.x = np.concatenate([
robot.getZeroConfiguration(),
np.zeros(tree.get_num_velocities())
])
# Workaround for the fact that PublishLoadRobot is not public.
# Ultimately that should be fixed.
sim = Simulator(self.visualizer)
context = sim.get_mutable_context()
context.FixInputPort(0, BasicVector(self.x))
sim.Initialize()
def _make_visualization(self, stop_time):
simulator = Simulator(self.builder.Build())
simulator.Initialize()
self.meshcat.vis.render_static()
# Set simulator context
simulator.get_mutable_context().SetTime(0.0)
# Start recording and simulate the diagram
self.meshcat.reset_recording()
self.meshcat.start_recording()
simulator.AdvanceTo(stop_time)
# Publish the recording
self.meshcat.publish_recording()
# Render
self.meshcat.vis.render_static()
input("View visualization. Press <ENTER> to end")
print("Finished")
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 cartPoleTest(self):
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant())
AddModelFromSdfFile(file_name=file_name,
plant=cart_pole,
scene_graph=scene_graph)
cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
cart_pole.Finalize(scene_graph)
assert cart_pole.geometry_source_is_registered()
builder.Connect(
cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
visualizer.load()
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.duration)
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 kukaTest(args):
file_name = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/"
"iiwa14_no_collision.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
kuka = builder.AddSystem(MultibodyPlant())
AddModelFromSdfFile(file_name=file_name,
plant=kuka,
scene_graph=scene_graph)
kuka.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
kuka.Finalize(scene_graph)
assert kuka.geometry_source_is_registered()
builder.Connect(kuka.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(kuka.get_source_id()))
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
visualizer.load()
diagram_context = diagram.CreateDefaultContext()
kuka_context = diagram.GetMutableSubsystemContext(
kuka, diagram_context)
kuka_context.FixInputPort(
kuka.get_actuation_input_port().get_index(),
np.zeros(kuka.get_actuation_input_port().size()))
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.duration)
def simulate(builder, plant, position, regulator, x0, duration):
""""Actually run simulation and return data."""
# Set up plant and position
builder.Connect(plant.get_output_port(0), position.get_input_port(0))
controller = builder.AddSystem(regulator)
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 logging
plant_logger = LogOutput(plant.get_output_port(0), builder)
position_logger = LogOutput(position.get_output_port(0), builder)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
context.SetContinuousState(x0)
# Create the simulator and simulate
simulator = Simulator(diagram, context)
simulator.Initialize()
print("Simulating...")
simulator.AdvanceTo(duration)
print("Simulation complete!")
# Return data
assert len(plant_logger.sample_times()) == len(
position_logger.sample_times())
for t1, t2 in zip(plant_logger.sample_times(),
position_logger.sample_times()):
assert t1 == t2
t = plant_logger.sample_times()
r_data = plant_logger.data()[0, :]
r_dot_data = plant_logger.data()[1, :]
theta_dot_data = plant_logger.data()[2, :]
phi_data = plant_logger.data()[3, :]
phi_dot_data = plant_logger.data()[4, :]
theta_data = position_logger.data()[0, :]
return t, r_data, r_dot_data, theta_dot_data, phi_data, phi_dot_data, theta_data
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)
def test_simulation(self):
# Basic constant-torque pendulum simulation.
builder = framework.DiagramBuilder()
pendulum = builder.AddSystem(PendulumPlant())
source = builder.AddSystem(ConstantVectorSource([1.0]))
builder.Connect(source.get_output_port(0), pendulum.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
# TODO(russt): Clean up state vector access below.
state = simulator.get_mutable_context().get_mutable_state()\
.get_mutable_continuous_state().get_mutable_vector()
initial_state = np.array([1.0, 0.0])
state.SetFromVector(initial_state)
simulator.StepTo(1.0)
self.assertFalse((state.CopyToVector() == initial_state).all())
def check_diagram_print_capture(self,
add_systems,
expected_text,
do_advance=True): # noqa
# TODO(eric.cousineau): Checking printing is a bit awkward, especially
# this way. Find a simpler way to test, but one that's still evident in
# how it functions.
builder = DiagramBuilder()
# Define some constants.
# - Abstract (string).
s_port = builder.AddSystem(
ConstantSource("hello world")).get_output_port(0)
# - Vector.
x_port = builder.AddSystem(ConstantSource([1, 2,
3])).get_output_port(0)
# - Vector of size 1, which could be interpreted as a scalar.
z_port = builder.AddSystem(ConstantSource([10.])).get_output_port(0)
dt = 0.1
# Add test-specific systems.
add_systems(builder, dt, x_port, s_port, z_port)
# Build and simulate, capturing output.
diagram = builder.Build()
simulator = Simulator(diagram)
t_end = dt
with capture_custom_print_lines() as lines:
custom_print("--- Initialize ---")
simulator.Initialize()
if do_advance:
custom_print("--- AdvanceTo ---")
simulator.AdvanceTo(t_end)
# Check.
actual_text = "\n".join(lines)
if DEBUG:
print()
print(actual_text)
self.assertEqual(actual_text, expected_text)
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))
