def show_cloud(pc, 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)
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))
# Make sure the system runs.
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
context = diagram.GetMutableSubsystemContext(
pc_viz, diagram_context)
context.FixInputPort(
pc_viz.GetInputPort("point_cloud_P").get_index(),
AbstractValue.Make(pc))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(sim_time)
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)
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)
simulator.AdvanceTo(.1)
def test_texture_override(self):
"""Draws a textured box to test the texture override pathway."""
object_file_path = FindResourceOrThrow(
"drake/systems/sensors/test/models/box_with_mesh.sdf")
# Find the texture path just to ensure it exists and
# we're testing the code path we want to.
FindResourceOrThrow("drake/systems/sensors/test/models/meshes/box.png")
builder = DiagramBuilder()
plant = MultibodyPlant(0.002)
_, scene_graph = AddMultibodyPlantSceneGraph(builder, plant)
object_model = Parser(plant=plant).AddModelFromFile(object_file_path)
plant.Finalize()
# Add meshcat visualizer.
viz = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
viz.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(1.0)
def main():
# Simulate with doubles.
builder = DiagramBuilder()
source = builder.AddSystem(ConstantVectorSource([10.]))
adder = builder.AddSystem(SimpleAdder(100.))
builder.Connect(source.get_output_port(0), adder.get_input_port(0))
logger = builder.AddSystem(SignalLogger(1))
builder.Connect(adder.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.AdvanceTo(1)
x = logger.data()
print("Output values: {}".format(x))
assert np.allclose(x, 110.)
# Compute outputs with AutoDiff and Symbolic.
for T in (AutoDiffXd, Expression):
adder_T = SimpleAdder_[T](100.)
context = adder_T.CreateDefaultContext()
adder_T.get_input_port().FixValue(context, [10.])
output = adder_T.AllocateOutput()
adder_T.CalcOutput(context, output)
# N.B. At present, you cannot get a reference to existing AutoDiffXd
# or Expression numpy arrays, so we will explictly copy the vector:
# https://github.com/RobotLocomotion/drake/issues/8116
value, = output.get_vector_data(0).CopyToVector()
assert isinstance(value, T)
print("Output from {}: {}".format(type(adder_T), repr(value)))
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 _process_event(self, dut):
# Use a Simulator to invoke the update event on `dut`. (Wouldn't it be
# nice if the Systems API was simple enough that we could apply events
# without calling a Simulator!)
simulator = Simulator(dut)
simulator.AdvanceTo(0.00025) # Arbitrary positive value.
return simulator.get_context().Clone()
def visualize(q, dt=None):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, MultibodyPlant(0.001))
load_atlas(plant, add_ground=True)
plant_context = plant.CreateDefaultContext()
ConnectContactResultsToDrakeVisualizer(builder=builder, plant=plant)
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
if len(q.shape) == 1:
q = np.reshape(q, (1, -1))
for i in range(q.shape[0]):
print(f"knot point: {i}")
diagram_context = diagram.CreateDefaultContext()
plant_context = diagram.GetMutableSubsystemContext(plant, diagram_context)
set_null_input(plant, plant_context)
plant.SetPositions(plant_context, q[i])
simulator = Simulator(diagram, diagram_context)
simulator.set_target_realtime_rate(0.0)
simulator.AdvanceTo(0.0001)
if not dt is None:
time.sleep(5/(np.sum(dt))*dt[i])
else:
time.sleep(0.5)
def test_simulation(self):
# Basic constant-torque acrobot simulation.
acrobot = AcrobotPlant()
# Create the simulator.
simulator = Simulator(acrobot)
context = simulator.get_mutable_context()
# Set an input torque.
input = AcrobotInput()
input.set_tau(1.)
acrobot.GetInputPort("elbow_torque").FixValue(context, input)
# Set the initial state.
state = context.get_mutable_continuous_state_vector()
state.set_theta1(1.)
state.set_theta1dot(0.)
state.set_theta2(0.)
state.set_theta2dot(0.)
self.assertTrue(acrobot.DynamicsBiasTerm(context).shape == (2, ))
self.assertTrue(acrobot.MassMatrix(context).shape == (2, 2))
initial_total_energy = acrobot.EvalPotentialEnergy(context) + \
acrobot.EvalKineticEnergy(context)
# Simulate (and make sure the state actually changes).
initial_state = state.CopyToVector()
simulator.AdvanceTo(1.0)
self.assertLessEqual(
acrobot.EvalPotentialEnergy(context) +
acrobot.EvalKineticEnergy(context), initial_total_energy)
def experiment(start_state, force_schedule, sleeptime=None):
global experiment_count
experiment_count += 1
if experiment_count % 10 == 0:
print(f"{experiment_count} experiments run so far...")
builder = DiagramBuilder()
mbp, sg = make_mbp(builder)
mbp.Finalize()
DrakeVisualizer.AddToBuilder(builder, sg, lcm=global_lcm_ftw)
thrusters = add_thrusters(builder, mbp)
breaks = [0]
for _, t in force_schedule:
breaks.append(breaks[-1] + t)
forces = np.array([f for f, t in force_schedule] + [force_schedule[-1][0]])
force_traj = PiecewisePolynomial.ZeroOrderHold(breaks, forces.transpose())
controller = builder.AddSystem(TrajectorySource(force_traj))
builder.Connect(controller.get_output_port(),
thrusters.get_command_input_port())
diagram = builder.Build()
simulator = Simulator(diagram)
mbp_context = diagram.GetSubsystemContext(mbp, simulator.get_context())
mbp.SetPositionsAndVelocities(mbp_context, start_state)
for step in range(int(1000 * breaks[-1])):
simulator.AdvanceTo(0.001 * step)
mbp_context = diagram.GetSubsystemContext(mbp, simulator.get_context())
if sleeptime: time.sleep(sleeptime)
return mbp.GetPositionsAndVelocities(
diagram.GetSubsystemContext(mbp, simulator.get_context()))
def test_allegro_proximity_geometry(self):
"""Allegro hand with visual and collision geometry, drawn in
proximity-geom mode."""
file_name = FindResourceOrThrow(
"drake/manipulation/models/allegro_hand_description/sdf/"
"allegro_hand_description_left.sdf")
builder = DiagramBuilder()
hand, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant=hand).AddModelFromFile(file_name)
hand.Finalize()
visualizer = builder.AddSystem(
MeshcatVisualizer(zmq_url=ZMQ_URL,
open_browser=False,
role=Role.kProximity))
builder.Connect(scene_graph.get_query_output_port(),
visualizer.get_geometry_query_input_port())
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
hand_context = diagram.GetMutableSubsystemContext(
hand, diagram_context)
hand_actuation_port = hand.get_actuation_input_port()
hand_actuation_port.FixValue(hand_context,
np.zeros(hand_actuation_port.size()))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(.1)
def test_textured_meshes(self):
"""Draws a solid green box (the texture map is just a green pixel) to
test the texture override pathway. You should confirm that you see a
green box in the visualizer."""
object_file_path = FindResourceOrThrow(
"drake/systems/sensors/test/models/box_with_mesh.sdf")
# Find the texture path just to ensure it exists and
# we're testing the code path we want to.
FindResourceOrThrow("drake/systems/sensors/test/models/meshes/box.png")
builder = DiagramBuilder()
plant = MultibodyPlant(0.002)
_, scene_graph = AddMultibodyPlantSceneGraph(builder, plant)
object_model = Parser(plant=plant).AddModelFromFile(object_file_path)
plant.Finalize()
# Add meshcat visualizer.
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())
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(1.0)
def simulate(*, initial_state, controller_params, t_final, tape_period):
"""Simulates an Acrobot + Spong controller from the given initial state and
parameters until the given final time. Returns the state sampled at the
given tape_period.
"""
builder = DiagramBuilder()
plant = builder.AddSystem(AcrobotPlant())
controller = builder.AddSystem(AcrobotSpongController())
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
state_logger = LogOutput(plant.get_output_port(0), builder)
state_logger.set_publish_period(tape_period)
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
plant_context = diagram.GetMutableSubsystemContext(plant, context)
controller_context = diagram.GetMutableSubsystemContext(
controller, context)
plant_context.SetContinuousState(initial_state)
controller_context.get_mutable_numeric_parameter(0).SetFromVector(
controller_params)
simulator.AdvanceTo(t_final)
x_tape = state_logger.data()
return x_tape
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)
Parser(plant=kuka).AddModelFromFile(file_name)
kuka.Finalize()
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()
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.AdvanceTo(.1)
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_signal_logger(self):
# Log the output of a simple diagram containing a constant
# source and an integrator.
builder = DiagramBuilder()
kValue = 2.4
source = builder.AddSystem(ConstantVectorSource([kValue]))
kSize = 1
integrator = builder.AddSystem(Integrator(kSize))
logger_per_step = builder.AddSystem(SignalLogger(kSize))
builder.Connect(source.get_output_port(0),
integrator.get_input_port(0))
builder.Connect(integrator.get_output_port(0),
logger_per_step.get_input_port(0))
# Add a redundant logger via the helper method.
logger_per_step_2 = LogOutput(integrator.get_output_port(0), builder)
# Add a periodic logger
logger_periodic = builder.AddSystem(SignalLogger(kSize))
kPeriod = 0.1
logger_periodic.set_publish_period(kPeriod)
builder.Connect(integrator.get_output_port(0),
logger_periodic.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
kTime = 1.
simulator.AdvanceTo(kTime)
# Verify outputs of the every-step logger
t = logger_per_step.sample_times()
x = logger_per_step.data()
self.assertTrue(t.shape[0] > 2)
self.assertTrue(t.shape[0] == x.shape[1])
self.assertAlmostEqual(x[0, -1], t[-1] * kValue, places=2)
np.testing.assert_array_equal(x, logger_per_step_2.data())
# Verify outputs of the periodic logger
t = logger_periodic.sample_times()
x = logger_periodic.data()
# Should log exactly once every kPeriod, up to and including kTime.
self.assertTrue(t.shape[0] == np.floor(kTime / kPeriod) + 1.)
logger_per_step.reset()
# Verify that t and x retain their values after systems are deleted.
t_copy = t.copy()
x_copy = x.copy()
del builder
del integrator
del logger_periodic
del logger_per_step
del logger_per_step_2
del diagram
del simulator
del source
gc.collect()
self.assertTrue((t == t_copy).all())
self.assertTrue((x == x_copy).all())
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 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 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.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.AdvanceTo(args.simulation_time)
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()
# Note: pass window=None argument to confirm kwargs are passed
# through to meshcat.Visualizer.
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url=ZMQ_URL,
open_browser=False,
window=None,
delete_prefix_on_load=True))
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)
visualizer.delete_prefix()
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 main():
builder = DiagramBuilder()
source = builder.AddSystem(ConstantVectorSource([10.]))
logger = builder.AddSystem(VectorLogSink(1))
builder.Connect(source.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.AdvanceTo(1)
x = logger.FindLog(simulator.get_context()).data()
print("Output values: {}".format(x))
assert np.allclose(x, 10.)
def test_dense_integration(self):
x = Variable("x")
sys = SymbolicVectorSystem(state=[x], dynamics=[-x+x**3])
simulator = Simulator(sys)
integrator = simulator.get_mutable_integrator()
self.assertIsNone(integrator.get_dense_output())
integrator.StartDenseIntegration()
pp = integrator.get_dense_output()
self.assertIsInstance(pp, PiecewisePolynomial)
simulator.AdvanceTo(1.0)
self.assertIs(pp, integrator.StopDenseIntegration())
self.assertEqual(pp.start_time(), 0.0)
self.assertEqual(pp.end_time(), 1.0)
self.assertIsNone(integrator.get_dense_output())
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_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 test_simulation(self):
van_der_pol = VanDerPolOscillator()
# Create the simulator.
simulator = Simulator(van_der_pol)
context = simulator.get_mutable_context()
# Set the initial state.
state = context.get_mutable_continuous_state_vector()
state.SetFromVector([0., 2.])
# Simulate (and make sure the state actually changes).
initial_state = state.CopyToVector()
simulator.AdvanceTo(1.0)
self.assertFalse((state.CopyToVector() == initial_state).any())
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 _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 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()
cart_pole, scene_graph = AddMultibodyPlantSceneGraph(
builder=builder, time_step=args.time_step)
Parser(plant=cart_pole).AddModelFromFile(file_name)
cart_pole.Finalize()
DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
cart_pole_context = cart_pole.GetMyMutableContextFromRoot(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)