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 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 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_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.AdvanceTo(.1)
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_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.)
context.FixInputPort(0, 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.CalcPotentialEnergy(context) + \
acrobot.CalcKineticEnergy(context)
# Simulate (and make sure the state actually changes).
initial_state = state.CopyToVector()
simulator.StepTo(1.0)
self.assertLessEqual(acrobot.CalcPotentialEnergy(context) +
acrobot.CalcKineticEnergy(context),
initial_total_energy)
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 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_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.StepTo(1.0)
self.assertFalse((state.CopyToVector() == initial_state).any())
def test_simulation(self):
# Basic rimless_wheel simulation.
rimless_wheel = RimlessWheel()
# Create the simulator.
simulator = Simulator(rimless_wheel)
context = simulator.get_mutable_context()
context.set_accuracy(1e-8)
# Set the initial state.
state = context.get_mutable_continuous_state_vector()
state.set_theta(0.)
state.set_thetadot(4.)
# Simulate (and make sure the state actually changes).
initial_state = state.CopyToVector()
simulator.StepTo(1.0)
self.assertFalse((state.CopyToVector() == initial_state).any())
def test_simulation(self):
# Basic compass_gait simulation.
compass_gait = CompassGait()
# Create the simulator.
simulator = Simulator(compass_gait)
context = simulator.get_mutable_context()
context.SetAccuracy(1e-8)
# Set the initial state.
state = context.get_mutable_continuous_state_vector()
state.set_stance(0.)
state.set_swing(0.)
state.set_stancedot(0.4)
state.set_swingdot(-2.0)
# 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_simulation(self):
# Basic constant-torque pendulum simulation.
pendulum = PendulumPlant()
# Create the simulator.
simulator = Simulator(pendulum)
context = simulator.get_mutable_context()
# Set an input torque.
input = PendulumInput()
input.set_tau(1.)
context.FixInputPort(0, input)
# Set the initial state.
state = context.get_mutable_continuous_state_vector()
state.set_theta(1.)
state.set_thetadot(0.)
# Simulate (and make sure the state actually changes).
initial_state = state.CopyToVector()
simulator.StepTo(1.0)
self.assertFalse((state.CopyToVector() == initial_state).any())
def test_simple_car(self):
simple_car = SimpleCar()
simulator = Simulator(simple_car)
context = simulator.get_mutable_context()
output = simple_car.AllocateOutput()
# Fix the input.
command = DrivingCommand()
command.set_steering_angle(0.5)
command.set_acceleration(1.)
context.FixInputPort(0, command)
# Verify the inputs.
command_eval = simple_car.EvalVectorInput(context, 0)
self.assertTrue(np.allclose(
command.get_value(), command_eval.get_value()))
# Initialize all the states to zero and take a simulation step.
state = context.get_mutable_continuous_state_vector()
state.SetFromVector([0.] * state.size())
simulator.StepTo(1.0)
# Verify the outputs.
simple_car.CalcOutput(context, output)
state_index = simple_car.state_output().get_index()
state_value = output.get_vector_data(state_index)
self.assertIsInstance(state_value, SimpleCarState)
self.assertTrue(
np.allclose(state.CopyToVector(), state_value.get_value()))
pose_index = simple_car.pose_output().get_index()
pose_value = output.get_vector_data(pose_index)
self.assertIsInstance(pose_value, PoseVector)
self.assertTrue(pose_value.get_translation()[0] > 0.)
velocity_index = simple_car.velocity_output().get_index()
velocity_value = output.get_vector_data(velocity_index)
self.assertIsInstance(velocity_value, FrameVelocity)
self.assertTrue(velocity_value.get_velocity().translational()[0] > 0.)
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 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.
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(
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 main():
parser = argparse.ArgumentParser(
description=__doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument(
"filename", type=str,
help="Path to an SDF or URDF file.")
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
filename = args.filename
if not os.path.isfile(filename):
parser.error("File does not exist: {}".format(filename))
# Construct Plant + SceneGraph.
builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant())
scene_graph = builder.AddSystem(SceneGraph())
plant.RegisterAsSourceForSceneGraph(scene_graph)
builder.Connect(
scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
builder.Connect(
plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
# Load the model file.
Parser(plant).AddModelFromFile(filename)
plant.Finalize()
# Publish to Drake Visualizer.
drake_viz_pub = ConnectDrakeVisualizer(builder, scene_graph)
# Publish to Meshcat.
if args.meshcat is not None:
meshcat_viz = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=args.meshcat))
builder.Connect(
scene_graph.get_pose_bundle_output_port(),
meshcat_viz.get_input_port(0))
diagram = builder.Build()
context = diagram.CreateDefaultContext()
# Use Simulator to dispatch initialization events.
# TODO(eric.cousineau): Simplify as part of #10015.
Simulator(diagram).Initialize()
# Publish draw messages with current state.
diagram.Publish(context)
def 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_fitted_value_iteration_pendulum(self):
plant = PendulumPlant()
simulator = Simulator(plant)
def quadratic_regulator_cost(context):
x = context.get_continuous_state_vector().CopyToVector()
x[0] = x[0] - math.pi
u = plant.EvalVectorInput(context, 0).CopyToVector()
return x.dot(x) + u.dot(u)
qbins = np.linspace(0., 2. * math.pi, 51)
qdotbins = np.linspace(-10., 10., 41)
state_grid = [set(qbins), set(qdotbins)]
input_limit = 2.
input_mesh = [set(np.linspace(-input_limit, input_limit, 9))]
timestep = 0.01
[Q, Qdot] = np.meshgrid(qbins, qdotbins)
fig = plt.figure()
ax = fig.gca(projection='3d')
def draw(iteration, mesh, cost_to_go, policy):
# Drawing is slow, don't draw every frame.
if iteration % 20 != 0:
return
plt.title("iteration " + str(iteration))
J = np.reshape(cost_to_go, Q.shape)
surf = ax.plot_surface(Q,
Qdot,
J,
rstride=1,
cstride=1,
cmap=color_map.jet)
plt.pause(0.00001)
surf.remove()
options = DynamicProgrammingOptions()
options.convergence_tol = 1.
options.state_indices_with_periodic_boundary_conditions = {0}
options.visualization_callback = draw
policy, cost_to_go = FittedValueIteration(simulator,
quadratic_regulator_cost,
state_grid, input_mesh,
timestep, options)
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 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 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 test_fitted_value_iteration_pendulum(self):
plant = PendulumPlant()
simulator = Simulator(plant)
def quadratic_regulator_cost(context):
x = context.get_continuous_state_vector().CopyToVector()
x[0] = x[0] - math.pi
u = plant.EvalVectorInput(context, 0).CopyToVector()
return x.dot(x) + u.dot(u)
# Note: intentionally under-sampled to keep the problem small
qbins = np.linspace(0., 2.*math.pi, 11)
qdotbins = np.linspace(-10., 10., 11)
state_grid = [set(qbins), set(qdotbins)]
input_limit = 2.
input_mesh = [set(np.linspace(-input_limit, input_limit, 5))]
timestep = 0.01
num_callbacks = [0]
def callback(iteration, mesh, cost_to_go, policy):
# Drawing is slow, don't draw every frame.
num_callbacks[0] += 1
options = DynamicProgrammingOptions()
options.convergence_tol = 1.
options.periodic_boundary_conditions = [
PeriodicBoundaryCondition(0, 0., 2.*math.pi)
]
options.visualization_callback = callback
options.input_port_index = InputPortSelection.kUseFirstInputIfItExists
options.assume_non_continuous_states_are_fixed = False
policy, cost_to_go = FittedValueIteration(simulator,
quadratic_regulator_cost,
state_grid, input_mesh,
timestep, options)
self.assertGreater(num_callbacks[0], 0)
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 test_composition_array_with_scene_graph(self):
"""Tests subgraphs (post-finalize) with a scene graph. This time, using
sugar from parse_as_multibody_plant_subgraph."""
# Create IIWA.
iiwa_file = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/urdf/"
"iiwa14_spheres_dense_elbow_collision.urdf")
iiwa_subgraph, iiwa_model = mut.parse_as_multibody_plant_subgraph(
iiwa_file)
self.assertIsInstance(iiwa_subgraph, mut.MultibodyPlantSubgraph)
self.assertIsInstance(iiwa_model, ModelInstanceIndex)
# Make 10 copies of the IIWA in a line.
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=0.01)
models = []
for i in range(10):
sub_to_full = iiwa_subgraph.add_to(
plant, scene_graph, model_instance_remap=f"iiwa_{i}")
self.assertIsInstance(sub_to_full, mut.MultibodyPlantElementsMap)
X_WB = RigidTransform(p=[i * 0.5, 0, 0])
model = sub_to_full.model_instances[iiwa_model]
base_frame = plant.GetFrameByName("base", model)
plant.WeldFrames(plant.world_frame(), base_frame, X_WB)
models.append(model)
plant.Finalize()
if VISUALIZE:
print("test_composition_array_with_scene_graph")
DrakeVisualizer.AddToBuilder(builder, scene_graph)
diagram = builder.Build()
d_context = diagram.CreateDefaultContext()
for i, model in enumerate(models):
self.assertEqual(plant.GetModelInstanceName(model), f"iiwa_{i}")
if VISUALIZE:
print(" Visualize composite")
Simulator(diagram, d_context.Clone()).Initialize()
input(" Press enter...")
def 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 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 = args_parser.parse_args()
filename, make_parser = parse_filename_and_parser(args_parser, args)
update_visualization, connect_visualizers = parse_visualizers(
args_parser, args)
# Construct Plant + SceneGraph.
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
# Load the model(s) from specified file.
make_parser(plant).AddAllModelsFromFile(filename)
update_visualization(plant, scene_graph)
plant.Finalize()
connect_visualizers(builder, plant, scene_graph)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
# Use Simulator to dispatch initialization events.
# TODO(eric.cousineau): Simplify as part of #10015.
Simulator(diagram).Initialize()
# Publish draw messages with current state.
diagram.Publish(context)
# Wait for the user to cancel us.
if args.meshcat:
print("Use Ctrl-C to quit")
try:
time.sleep(1e3)
except KeyboardInterrupt:
pass
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)
class PusherSliderEnv(gym.Env):
metadata = {"render.modes": []}
def __init__(self, config=None):
if config is None:
config_path = os.path.join(os.path.dirname(__file__),
"config.yaml")
config = yaml.safe_load(open(config_path, 'r'))
self._config = config
self._step_dt = config["step_dt"]
self._model_name = config["model_name"]
self._sim_diagram = DrakeSimDiagram(config)
mbp = self._sim_diagram.mbp
builder = self._sim_diagram.builder
# === Add table =====
dims = config["table"]["size"]
color = np.array(config["table"]["color"])
friction_params = config["table"]["coulomb_friction"]
box_shape = Box(*dims)
X_W_T = RigidTransform(p=np.array([0., 0., -dims[2] / 2.]))
# This rigid body will be added to the world model instance since
# the model instance is not specified.
box_body = mbp.AddRigidBody(
"table",
SpatialInertia(mass=1.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(mbp.world_frame(), box_body.body_frame(), X_W_T)
mbp.RegisterVisualGeometry(box_body, RigidTransform(), box_shape,
"table_vis", color)
mbp.RegisterCollisionGeometry(box_body, RigidTransform(), box_shape,
"table_collision",
CoulombFriction(*friction_params))
# === Add pusher ====
parser = Parser(mbp, self._sim_diagram.sg)
self._pusher_model_id = parser.AddModelFromFile(
path_util.pusher_peg, "pusher")
base_link = mbp.GetBodyByName("base", self._pusher_model_id)
mbp.WeldFrames(mbp.world_frame(), base_link.body_frame())
def pusher_port_func():
actuation_input_port = mbp.get_actuation_input_port(
self._pusher_model_id)
state_output_port = mbp.get_state_output_port(
self._pusher_model_id)
builder.ExportInput(actuation_input_port, "pusher_actuation_input")
builder.ExportOutput(state_output_port, "pusher_state_output")
self._sim_diagram.finalize_functions.append(pusher_port_func)
# === Add slider ====
parser = Parser(mbp, self._sim_diagram.sg)
self._slider_model_id = parser.AddModelFromFile(
path_util.model_paths[self._model_name], self._model_name)
def slider_port_func():
state_output_port = mbp.get_state_output_port(
self._slider_model_id)
builder.ExportOutput(state_output_port, "slider_state_output")
self._sim_diagram.finalize_functions.append(slider_port_func)
if "rgbd_sensors" in config and config["rgbd_sensors"]["enabled"]:
self._sim_diagram.add_rgbd_sensors_from_config(config)
if "visualization" in config:
if not config["visualization"]:
pass
elif config["visualization"] == "meshcat":
self._sim_diagram.connect_to_meshcat()
elif config["visualization"] == "drake_viz":
self._sim_diagram.connect_to_drake_visualizer()
else:
raise ValueError("Unknown visualization:",
config["visualization"])
self._sim_diagram.finalize()
builder = DiagramBuilder()
builder.AddSystem(self._sim_diagram)
pid = builder.AddSystem(
PidController(kp=[0, 0], kd=[1000, 1000], ki=[0, 0]))
builder.Connect(self._sim_diagram.GetOutputPort("pusher_state_output"),
pid.get_input_port_estimated_state())
builder.Connect(
pid.get_output_port_control(),
self._sim_diagram.GetInputPort("pusher_actuation_input"))
builder.ExportInput(pid.get_input_port_desired_state(),
"pid_input_port_desired_state")
self._diagram = builder.Build()
self._pid_desired_state_port = self._diagram.get_input_port(0)
self._simulator = Simulator(self._diagram)
self.reset()
self.action_space = spaces.Box(low=-1.,
high=1.,
shape=(2, ),
dtype=np.float32)
# TODO: Observation space for images is currently too loose
self.observation_space = convert_observation_to_space(
self.get_observation())
def get_observation(self, context=None):
assert self._sim_diagram.is_finalized()
if context is None:
context = self._simulator.get_context()
context = self._diagram.GetMutableSubsystemContext(
self._sim_diagram, context)
mbp_context = self._sim_diagram.GetMutableSubsystemContext(
self._sim_diagram.mbp, context)
obs = dict()
obs["time"] = context.get_time()
obs["pusher"] = dict()
obs["pusher"]["position"] = np.copy(
self._sim_diagram.mbp.GetPositions(mbp_context,
self._pusher_model_id))
obs["pusher"]["velocity"] = np.copy(
self._sim_diagram.mbp.GetVelocities(mbp_context,
self._pusher_model_id))
q = np.copy(
self._sim_diagram.mbp.GetPositions(mbp_context,
self._slider_model_id))
v = np.copy(
self._sim_diagram.mbp.GetVelocities(mbp_context,
self._slider_model_id))
obs["slider"] = dict()
obs["slider"]["position"] = {
'translation': q[4:],
'quaternion': q[0:4],
'raw': q
}
obs["slider"]["velocity"] = {
'linear': v[3:],
'angular': v[0:3],
'raw': v
}
if self._config["rgbd_sensors"]["enabled"]:
obs["images"] = self._sim_diagram.get_image_observations(context)
return obs
def pusher_slider_on_table(self, context=None):
if context is None:
context = self._simulator.get_context()
context = self._diagram.GetMutableSubsystemContext(
self._sim_diagram, context)
mbp_context = self._sim_diagram.GetMutableSubsystemContext(
self._sim_diagram.mbp, context)
table_dim = np.array(self._config["table"]["size"])
q_pusher = self._sim_diagram.mbp.GetPositions(mbp_context,
self._pusher_model_id)
q_slider = self._sim_diagram.mbp.GetPositions(
mbp_context, self._slider_model_id)[4:6]
tol = 0.03
high_edge = table_dim[:2] / 2.0 - tol
low_edge = -high_edge
return ((low_edge < q_pusher) & (q_pusher < high_edge) &
(low_edge < q_slider) & (q_slider < high_edge)).all()
def step(self, action, dt=None):
assert self._sim_diagram.is_finalized()
assert len(action) == 2
if dt is None:
dt = self._step_dt
# the time to advance to
t_advance = self._simulator.get_context().get_time() + dt
context = self._simulator.get_mutable_context()
# set the value of the PID controller input port
pid_setpoint = np.concatenate((np.zeros(2), action))
self._pid_desired_state_port.FixValue(context, pid_setpoint)
# simulate and take observation
self._simulator.AdvanceTo(t_advance)
obs = self.get_observation()
reward = 0.
done = not self.pusher_slider_on_table()
info = {}
return obs, reward, done, info
# TODO: Make reset position non-deterministic?
def reset(self, q_pusher=None, q_slider=None):
assert self._sim_diagram.is_finalized()
if q_pusher is None:
q_pusher = np.array([-0.1, 0.])
if q_slider is None:
q_slider = np.array([1., 0., 0., 0., 0., 0., 0.05])
pid_setpoint = np.zeros(4)
context = self._simulator.get_mutable_context()
mbp_context = self._diagram.GetMutableSubsystemContext(
self._sim_diagram.mbp, context)
context.SetTime(0.)
self._sim_diagram.mbp.SetPositions(mbp_context, self._pusher_model_id,
q_pusher)
self._sim_diagram.mbp.SetVelocities(mbp_context, self._pusher_model_id,
np.zeros(2))
self._sim_diagram.mbp.SetPositions(mbp_context, self._slider_model_id,
q_slider)
self._sim_diagram.mbp.SetVelocities(mbp_context, self._slider_model_id,
np.zeros(6))
self._pid_desired_state_port.FixValue(context, pid_setpoint)
self._simulator.Initialize()
return self.get_observation()
#TODO: Implement as necessary
def render(self, mode="human"):
pass
def close(self):
pass
#TODO: Implement?
def seed(self, seed=None):
pass
def get_labels_to_mask(self):
"""
Returns a set of labels that represent slider object in the scene
:return: TinyDB database
"""
# write sql query
label_db = self._sim_diagram.get_label_db()
# return all objects that have matching model_instance_id in the list
Label = Query()
labels_to_mask = label_db.search(
Label.model_instance_id.one_of(int(self._slider_model_id)))
# create new TinyDB with just those
mask_db = TinyDB(storage=MemoryStorage)
mask_db.insert_multiple(labels_to_mask)
return mask_db
teleop.window.withdraw() # Don't display the window when testing.
filter = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6))
builder.Connect(teleop.get_output_port(0), filter.get_input_port(0))
builder.Connect(filter.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
wsg_buttons = builder.AddSystem(SchunkWsgButtons(teleop.window))
builder.Connect(wsg_buttons.GetOutputPort("position"), station.GetInputPort(
"wsg_position"))
builder.Connect(wsg_buttons.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
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))
q0 = station.GetOutputPort("iiwa_position_measured").Eval(
station_context).get_value()
differential_ik.parameters.set_nominal_joint_position(q0)
teleop.SetPose(differential_ik.ForwardKinematics(q0))
filter.set_initial_output_value(
diagram.GetMutableSubsystemContext(
filter, simulator.get_mutable_context()),
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_contacts(), 0)
self.assertEqual(contact_viz._contact_key_counter, 4)
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)
def test_simulator_api(self):
"""Tests basic Simulator API."""
# TODO(eric.cousineau): Migrate tests from `general_test.py` to here.
system = ConstantVectorSource([1.])
simulator = Simulator(system)
self.assertIs(simulator.get_system(), system)
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)
def test_leaf_system_overrides(self):
test = self
class TrivialSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.called_publish = False
self.called_feedthrough = False
self.called_continuous = False
self.called_discrete = False
self.called_initialize = False
self.called_per_step = False
self.called_periodic = False
# Ensure we have desired overloads.
self._DeclarePeriodicPublish(0.1)
self._DeclarePeriodicPublish(0.1, 0)
self._DeclarePeriodicPublish(period_sec=0.1, offset_sec=0.)
self._DeclarePeriodicDiscreteUpdate(period_sec=0.1,
offset_sec=0.)
self._DeclareInitializationEvent(event=PublishEvent(
trigger_type=TriggerType.kInitialization,
callback=self._on_initialize))
self._DeclarePerStepEvent(
event=PublishEvent(trigger_type=TriggerType.kPerStep,
callback=self._on_per_step))
self._DeclarePeriodicEvent(
period_sec=0.1,
offset_sec=0.0,
event=PublishEvent(trigger_type=TriggerType.kPeriodic,
callback=self._on_periodic))
self._DeclareContinuousState(2)
self._DeclareDiscreteState(1)
# Ensure that we have inputs / outputs to call direct
# feedthrough.
self._DeclareInputPort(PortDataType.kVectorValued, 1)
self._DeclareVectorInputPort(name="test_input",
model_vector=BasicVector(1),
random_type=None)
self._DeclareVectorOutputPort(BasicVector(1), noop)
def _DoPublish(self, context, events):
# Call base method to ensure we do not get recursion.
LeafSystem._DoPublish(self, context, events)
# N.B. We do not test for a singular call to `DoPublish`
# (checking `assertFalse(self.called_publish)` first) because
# the above `_DeclareInitializationEvent` will call both its
# callback and this event when invoked via
# `Simulator::Initialize` from `call_leaf_system_overrides`,
# even when we explicitly say not to publish at initialize.
self.called_publish = True
def _DoHasDirectFeedthrough(self, input_port, output_port):
# Test inputs.
test.assertIn(input_port, [0, 1])
test.assertEqual(output_port, 0)
# Call base method to ensure we do not get recursion.
base_return = LeafSystem._DoHasDirectFeedthrough(
self, input_port, output_port)
test.assertTrue(base_return is None)
# Return custom methods.
self.called_feedthrough = True
return False
def _DoCalcTimeDerivatives(self, context, derivatives):
# Note: Don't call base method here; it would abort because
# derivatives.size() != 0.
test.assertEqual(derivatives.get_vector().size(), 2)
self.called_continuous = True
def _DoCalcDiscreteVariableUpdates(self, context, events,
discrete_state):
# Call base method to ensure we do not get recursion.
LeafSystem._DoCalcDiscreteVariableUpdates(
self, context, events, discrete_state)
self.called_discrete = True
def _on_initialize(self, context, event):
test.assertIsInstance(context, Context)
test.assertIsInstance(event, PublishEvent)
test.assertFalse(self.called_initialize)
self.called_initialize = True
def _on_per_step(self, context, event):
test.assertIsInstance(context, Context)
test.assertIsInstance(event, PublishEvent)
self.called_per_step = True
def _on_periodic(self, context, event):
test.assertIsInstance(context, Context)
test.assertIsInstance(event, PublishEvent)
test.assertFalse(self.called_periodic)
self.called_periodic = True
system = TrivialSystem()
self.assertFalse(system.called_publish)
self.assertFalse(system.called_feedthrough)
self.assertFalse(system.called_continuous)
self.assertFalse(system.called_discrete)
self.assertFalse(system.called_initialize)
results = call_leaf_system_overrides(system)
self.assertTrue(system.called_publish)
self.assertTrue(system.called_feedthrough)
self.assertFalse(results["has_direct_feedthrough"])
self.assertTrue(system.called_continuous)
self.assertTrue(system.called_discrete)
self.assertTrue(system.called_initialize)
self.assertEqual(results["discrete_next_t"], 0.1)
self.assertFalse(system.HasAnyDirectFeedthrough())
self.assertFalse(system.HasDirectFeedthrough(output_port=0))
self.assertFalse(
system.HasDirectFeedthrough(input_port=0, output_port=0))
# Test explicit calls.
system = TrivialSystem()
context = system.CreateDefaultContext()
system.Publish(context)
self.assertTrue(system.called_publish)
context_update = context.Clone()
system.CalcTimeDerivatives(
context, context_update.get_mutable_continuous_state())
self.assertTrue(system.called_continuous)
# Test per-step and periodic call backs
system = TrivialSystem()
simulator = Simulator(system)
simulator.StepTo(0.1)
self.assertTrue(system.called_per_step)
self.assertTrue(system.called_periodic)
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"),
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))
# Eval the output port once to read the initial positions of the IIWA.
q0 = station.GetOutputPort("iiwa_position_measured").Eval(
station_context).get_value()
teleop.set_position(q0)
filter.set_initial_output_value(diagram.GetMutableSubsystemContext(
filter, simulator.get_mutable_context()), q0)
# This is important to avoid duplicate publishes to the hardware interface:
def test_leaf_system_overrides(self):
test = self
class TrivialSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.called_publish = False
self.called_feedthrough = False
self.called_continuous = False
self.called_discrete = False
self.called_initialize = False
self.called_per_step = False
self.called_periodic = False
self.called_getwitness = False
self.called_witness = False
self.called_guard = False
self.called_reset = False
# Ensure we have desired overloads.
self.DeclarePeriodicPublish(1.0)
self.DeclarePeriodicPublish(1.0, 0)
self.DeclarePeriodicPublish(period_sec=1.0, offset_sec=0.)
self.DeclarePeriodicDiscreteUpdate(
period_sec=1.0, offset_sec=0.)
self.DeclareInitializationEvent(
event=PublishEvent(
trigger_type=TriggerType.kInitialization,
callback=self._on_initialize))
self.DeclarePerStepEvent(
event=PublishEvent(
trigger_type=TriggerType.kPerStep,
callback=self._on_per_step))
self.DeclarePeriodicEvent(
period_sec=1.0,
offset_sec=0.0,
event=PublishEvent(
trigger_type=TriggerType.kPeriodic,
callback=self._on_periodic))
self.DeclareContinuousState(2)
self.DeclareDiscreteState(1)
# Ensure that we have inputs / outputs to call direct
# feedthrough.
self.DeclareInputPort(PortDataType.kVectorValued, 1)
self.DeclareVectorInputPort(
name="test_input", model_vector=BasicVector(1),
random_type=None)
self.DeclareVectorOutputPort(
"noop", BasicVector(1), noop,
prerequisites_of_calc=set([self.nothing_ticket()]))
self.witness = self.MakeWitnessFunction(
"witness", WitnessFunctionDirection.kCrossesZero,
self._witness)
self.reset_witness = self.MakeWitnessFunction(
"reset", WitnessFunctionDirection.kCrossesZero,
self._guard, UnrestrictedUpdateEvent(self._reset))
def DoPublish(self, context, events):
# Call base method to ensure we do not get recursion.
LeafSystem.DoPublish(self, context, events)
# N.B. We do not test for a singular call to `DoPublish`
# (checking `assertFalse(self.called_publish)` first) because
# the above `_DeclareInitializationEvent` will call both its
# callback and this event when invoked via
# `Simulator::Initialize` from `call_leaf_system_overrides`,
# even when we explicitly say not to publish at initialize.
self.called_publish = True
def DoHasDirectFeedthrough(self, input_port, output_port):
# Test inputs.
test.assertIn(input_port, [0, 1])
test.assertEqual(output_port, 0)
# Call base method to ensure we do not get recursion.
with catch_drake_warnings(expected_count=1):
base_return = LeafSystem.DoHasDirectFeedthrough(
self, input_port, output_port)
test.assertTrue(base_return is None)
# Return custom methods.
self.called_feedthrough = True
return False
def DoCalcTimeDerivatives(self, context, derivatives):
# Note: Don't call base method here; it would abort because
# derivatives.size() != 0.
test.assertEqual(derivatives.get_vector().size(), 2)
self.called_continuous = True
def DoCalcDiscreteVariableUpdates(
self, context, events, discrete_state):
# Call base method to ensure we do not get recursion.
LeafSystem.DoCalcDiscreteVariableUpdates(
self, context, events, discrete_state)
self.called_discrete = True
def DoGetWitnessFunctions(self, context):
self.called_getwitness = True
return [self.witness, self.reset_witness]
def _on_initialize(self, context, event):
test.assertIsInstance(context, Context)
test.assertIsInstance(event, PublishEvent)
test.assertFalse(self.called_initialize)
self.called_initialize = True
def _on_per_step(self, context, event):
test.assertIsInstance(context, Context)
test.assertIsInstance(event, PublishEvent)
self.called_per_step = True
def _on_periodic(self, context, event):
test.assertIsInstance(context, Context)
test.assertIsInstance(event, PublishEvent)
test.assertFalse(self.called_periodic)
self.called_periodic = True
def _witness(self, context):
test.assertIsInstance(context, Context)
self.called_witness = True
return 1.0
def _guard(self, context):
test.assertIsInstance(context, Context)
self.called_guard = True
return context.get_time() - 0.5
def _reset(self, context, event, state):
test.assertIsInstance(context, Context)
test.assertIsInstance(event, UnrestrictedUpdateEvent)
test.assertIsInstance(state, State)
self.called_reset = True
system = TrivialSystem()
self.assertFalse(system.called_publish)
self.assertFalse(system.called_feedthrough)
self.assertFalse(system.called_continuous)
self.assertFalse(system.called_discrete)
self.assertFalse(system.called_initialize)
results = call_leaf_system_overrides(system)
self.assertTrue(system.called_publish)
self.assertTrue(system.called_feedthrough)
self.assertFalse(results["has_direct_feedthrough"])
self.assertTrue(system.called_continuous)
self.assertTrue(system.called_discrete)
self.assertTrue(system.called_initialize)
self.assertEqual(results["discrete_next_t"], 1.0)
self.assertFalse(system.HasAnyDirectFeedthrough())
self.assertFalse(system.HasDirectFeedthrough(output_port=0))
self.assertFalse(
system.HasDirectFeedthrough(input_port=0, output_port=0))
# Test explicit calls.
system = TrivialSystem()
context = system.CreateDefaultContext()
system.Publish(context)
self.assertTrue(system.called_publish)
context_update = context.Clone()
system.CalcTimeDerivatives(
context=context,
derivatives=context_update.get_mutable_continuous_state())
self.assertTrue(system.called_continuous)
witnesses = system.GetWitnessFunctions(context)
self.assertEqual(len(witnesses), 2)
system.CalcDiscreteVariableUpdates(
context=context,
discrete_state=context_update.get_mutable_discrete_state())
self.assertTrue(system.called_discrete)
# Test per-step, periodic, and witness call backs
system = TrivialSystem()
simulator = Simulator(system)
simulator.get_mutable_context().SetAccuracy(0.1)
# Stepping to 0.99 so that we get exactly one periodic event.
simulator.AdvanceTo(0.99)
self.assertTrue(system.called_per_step)
self.assertTrue(system.called_periodic)
self.assertTrue(system.called_getwitness)
self.assertTrue(system.called_witness)
self.assertTrue(system.called_guard)
self.assertTrue(system.called_reset)
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 = BasicVector([0.1, 0.2, 0.3])
context.FixInputPort(0, input0)
input1 = BasicVector([0.02, 0.03, 0.04])
context.FixInputPort(1, input1)
input2 = BasicVector([0.003, 0.004, 0.005])
context.FixInputPort(2, input2)
# 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 main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate", type=float, default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument(
"--duration", type=float, default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware", action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument(
"--test", action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
"--filter_time_const", type=float, default=0.005,
help="Time constant for the first order low pass filter applied to"
"the teleop commands")
parser.add_argument(
"--velocity_limit_factor", type=float, default=1.0,
help="This value, typically between 0 and 1, further limits the "
"iiwa14 joint velocities. It multiplies each of the seven "
"pre-defined joint velocity limits. "
"Note: The pre-defined velocity limits are specified by "
"iiwa14_velocity_limits, found in this python file.")
parser.add_argument(
'--setup', type=str, default='default',
help="The manipulation station setup to simulate. ",
choices=['default', 'clutter_clearing'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
if args.test:
# Don't grab mouse focus during testing.
grab_focus = False
# See: https://stackoverflow.com/a/52528832/7829525
os.environ["SDL_VIDEODRIVER"] = "dummy"
else:
grab_focus = True
builder = DiagramBuilder()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
# Initializes the chosen station type.
if args.setup == 'default':
station.SetupDefaultStation()
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation()
ycb_objects = CreateDefaultYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
station.Finalize()
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
if args.meshcat:
meshcat = builder.AddSystem(MeshcatVisualizer(
station.get_scene_graph(), zmq_url=args.meshcat))
builder.Connect(station.GetOutputPort("pose_bundle"),
meshcat.get_input_port(0))
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
time_step = 0.005
params.set_timestep(time_step)
# True velocity limits for the IIWA14 (in rad, rounded down to the first
# decimal)
iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])
# Stay within a small fraction of those limits for this teleop demo.
factor = args.velocity_limit_factor
params.set_joint_velocity_limits((-factor*iiwa14_velocity_limits,
factor*iiwa14_velocity_limits))
differential_ik = builder.AddSystem(DifferentialIK(
robot, robot.GetFrameByName("iiwa_link_7"), params, time_step))
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
teleop = builder.AddSystem(MouseKeyboardTeleop(grab_focus=grab_focus))
filter_ = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6))
builder.Connect(teleop.get_output_port(0), filter_.get_input_port(0))
builder.Connect(filter_.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
builder.Connect(teleop.GetOutputPort("position"), station.GetInputPort(
"wsg_position"))
builder.Connect(teleop.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(7))
simulator.AdvanceTo(1e-6)
q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context)
differential_ik.parameters.set_nominal_joint_position(q0)
teleop.SetPose(differential_ik.ForwardKinematics(q0))
filter_.set_initial_output_value(
diagram.GetMutableSubsystemContext(
filter_, simulator.get_mutable_context()),
teleop.get_output_port(0).Eval(diagram.GetMutableSubsystemContext(
teleop, simulator.get_mutable_context())))
differential_ik.SetPositions(diagram.GetMutableSubsystemContext(
differential_ik, simulator.get_mutable_context()), q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
print_instructions()
simulator.AdvanceTo(args.duration)
def __init__(self, config=None):
if config is None:
config_path = os.path.join(os.path.dirname(__file__),
"config.yaml")
config = yaml.safe_load(open(config_path, 'r'))
self._config = config
self._step_dt = config["step_dt"]
self._model_name = config["model_name"]
self._sim_diagram = DrakeSimDiagram(config)
mbp = self._sim_diagram.mbp
builder = self._sim_diagram.builder
# === Add table =====
dims = config["table"]["size"]
color = np.array(config["table"]["color"])
friction_params = config["table"]["coulomb_friction"]
box_shape = Box(*dims)
X_W_T = RigidTransform(p=np.array([0., 0., -dims[2] / 2.]))
# This rigid body will be added to the world model instance since
# the model instance is not specified.
box_body = mbp.AddRigidBody(
"table",
SpatialInertia(mass=1.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(mbp.world_frame(), box_body.body_frame(), X_W_T)
mbp.RegisterVisualGeometry(box_body, RigidTransform(), box_shape,
"table_vis", color)
mbp.RegisterCollisionGeometry(box_body, RigidTransform(), box_shape,
"table_collision",
CoulombFriction(*friction_params))
# === Add pusher ====
parser = Parser(mbp, self._sim_diagram.sg)
self._pusher_model_id = parser.AddModelFromFile(
path_util.pusher_peg, "pusher")
base_link = mbp.GetBodyByName("base", self._pusher_model_id)
mbp.WeldFrames(mbp.world_frame(), base_link.body_frame())
def pusher_port_func():
actuation_input_port = mbp.get_actuation_input_port(
self._pusher_model_id)
state_output_port = mbp.get_state_output_port(
self._pusher_model_id)
builder.ExportInput(actuation_input_port, "pusher_actuation_input")
builder.ExportOutput(state_output_port, "pusher_state_output")
self._sim_diagram.finalize_functions.append(pusher_port_func)
# === Add slider ====
parser = Parser(mbp, self._sim_diagram.sg)
self._slider_model_id = parser.AddModelFromFile(
path_util.model_paths[self._model_name], self._model_name)
def slider_port_func():
state_output_port = mbp.get_state_output_port(
self._slider_model_id)
builder.ExportOutput(state_output_port, "slider_state_output")
self._sim_diagram.finalize_functions.append(slider_port_func)
if "rgbd_sensors" in config and config["rgbd_sensors"]["enabled"]:
self._sim_diagram.add_rgbd_sensors_from_config(config)
if "visualization" in config:
if not config["visualization"]:
pass
elif config["visualization"] == "meshcat":
self._sim_diagram.connect_to_meshcat()
elif config["visualization"] == "drake_viz":
self._sim_diagram.connect_to_drake_visualizer()
else:
raise ValueError("Unknown visualization:",
config["visualization"])
self._sim_diagram.finalize()
builder = DiagramBuilder()
builder.AddSystem(self._sim_diagram)
pid = builder.AddSystem(
PidController(kp=[0, 0], kd=[1000, 1000], ki=[0, 0]))
builder.Connect(self._sim_diagram.GetOutputPort("pusher_state_output"),
pid.get_input_port_estimated_state())
builder.Connect(
pid.get_output_port_control(),
self._sim_diagram.GetInputPort("pusher_actuation_input"))
builder.ExportInput(pid.get_input_port_desired_state(),
"pid_input_port_desired_state")
self._diagram = builder.Build()
self._pid_desired_state_port = self._diagram.get_input_port(0)
self._simulator = Simulator(self._diagram)
self.reset()
self.action_space = spaces.Box(low=-1.,
high=1.,
shape=(2, ),
dtype=np.float32)
# TODO: Observation space for images is currently too loose
self.observation_space = convert_observation_to_space(
self.get_observation())
# One input, one output, two state variables.
VectorSystem.__init__(self, 1, 2, direct_feedthrough=False)
self.DeclareContinuousState(2)
# qqdot(t) = u(t)
def DoCalcVectorTimeDerivatives(self, context, u, x, xdot):
xdot[0] = x[1]
xdot[1] = u
# y(t) = x(t)
def DoCalcVectorOutput(self, context, u, x, y):
y[:] = x
plant = DoubleIntegrator()
simulator = Simulator(plant)
options = DynamicProgrammingOptions()
def min_time_cost(context):
x = context.get_continuous_state_vector().CopyToVector()
if x.dot(x) < .05:
return 0.
return 1.
def quadratic_regulator_cost(context):
x = context.get_continuous_state_vector().CopyToVector()
u = plant.EvalVectorInput(context, 0).CopyToVector()
return x.dot(x) + 20*u.dot(u)
from pydrake.systems.analysis import Simulator
from plant import SLIPState, SpringLoadedInvertedPendulum
plant = SpringLoadedInvertedPendulum()
# Parameters from Geyer05, Figure 2.4
# Note: Geyer uses angle of attack = 90 - touchdown_angle
touchdown_angle = np.deg2rad(30)
Etilde = 1.61
s = SLIPState(np.zeros(8))
s.theta = touchdown_angle
s.r = 1
simulator = Simulator(plant)
context = simulator.get_mutable_context()
context.FixInputPort(0, [touchdown_angle])
context.SetAccuracy(1e-5)
zs = np.linspace(np.cos(touchdown_angle)+0.001, 0.95, 25)
zns = 0*zs
for i in range(len(zs)):
s.z = zs[i]
s.xdot = plant.apex_velocity_from_dimensionless_system_energy(Etilde, s.z)
context.SetTime(0.)
context.get_mutable_continuous_state_vector().SetFromVector(s[:])
simulator.Initialize()
# Note: With this duration, I sometimes get an extra "touchdown" after the
# apex, which results in apex-touchdown; touchdown-takeoff-apex on the
# console. It's not a double reset, the consecutive touchdowns are two
def test_decomposition_controller_like_workflow(self):
"""Tests subgraphs (post-finalize) for decomposition, with a
scene-graph. Also shows a workflow of replacing joints / welding
joints."""
builder = DiagramBuilder()
# N.B. I (Eric) am using ManipulationStation because it's currently
# the simplest way to get a complex scene in pydrake.
station = ManipulationStation(time_step=0.001)
station.SetupManipulationClassStation()
station.Finalize()
builder.AddSystem(station)
plant = station.get_multibody_plant()
scene_graph = station.get_scene_graph()
iiwa = plant.GetModelInstanceByName("iiwa")
wsg = plant.GetModelInstanceByName("gripper")
if VISUALIZE:
print("test_decomposition_controller_like_workflow")
DrakeVisualizer.AddToBuilder(builder,
station.GetOutputPort("query_object"))
diagram = builder.Build()
# Set the context with which things should be computed.
d_context = diagram.CreateDefaultContext()
context = plant.GetMyContextFromRoot(d_context)
q_iiwa = [0.3, 0.7, 0.3, 0.6, 0.5, 0.6, 0.7]
ndof = 7
q_wsg = [-0.03, 0.03]
plant.SetPositions(context, iiwa, q_iiwa)
plant.SetPositions(context, wsg, q_wsg)
# Add copy of only the IIWA to a control diagram.
control_builder = DiagramBuilder()
control_plant = control_builder.AddSystem(MultibodyPlant(time_step=0))
# N.B. This has the same scene, but with all joints outside of the
# IIWA frozen
# TODO(eric.cousineau): Re-investigate weird "build_with_no_control"
# behavior (with scene graph) with default conditions and time_step=0
# - see Anzu commit 2cf08cfc3.
to_control = mut.add_plant_with_articulated_subset_to(
plant_src=plant,
scene_graph_src=scene_graph,
articulated_models_src=[iiwa],
context_src=context,
plant_dest=control_plant)
self.assertIsInstance(to_control, mut.MultibodyPlantElementsMap)
control_iiwa = to_control.model_instances[iiwa]
control_plant.Finalize()
self.assertEqual(control_plant.num_positions(),
plant.num_positions(iiwa))
kp = np.array([2000., 1500, 1500, 1500, 1500, 500, 500])
kd = np.sqrt(2 * kp)
ki = np.zeros(7)
controller = control_builder.AddSystem(
InverseDynamicsController(robot=control_plant,
kp=kp,
ki=ki,
kd=kd,
has_reference_acceleration=False))
# N.B. Rather than use model instances for direct correspence, we could
# use the mappings themselves within a custom system.
control_builder.Connect(
control_plant.get_state_output_port(control_iiwa),
controller.get_input_port_estimated_state())
control_builder.Connect(
controller.get_output_port_control(),
control_plant.get_actuation_input_port(control_iiwa))
# Control to having the elbow slightly bent.
q_iiwa_final = np.zeros(7)
q_iiwa_final[3] = -np.pi / 2
t_start = 0.
t_end = 1.
nx = 2 * ndof
def q_desired_interpolator(t: ContextTimeArg) -> VectorArg(nx):
s = (t - t_start) / (t_end - t_start)
ds = 1 / (t_end - t_start)
q = q_iiwa + s * (q_iiwa_final - q_iiwa)
v = ds * (q_iiwa_final - q_iiwa)
x = np.hstack((q, v))
return x
interpolator = control_builder.AddSystem(
FunctionSystem(q_desired_interpolator))
control_builder.Connect(interpolator.get_output_port(0),
controller.get_input_port_desired_state())
control_diagram = control_builder.Build()
control_d_context = control_diagram.CreateDefaultContext()
control_context = control_plant.GetMyContextFromRoot(control_d_context)
to_control.copy_state(context, control_context)
util.compare_frame_poses(plant, context, control_plant,
control_context, "iiwa_link_0", "iiwa_link_7")
util.compare_frame_poses(plant, context, control_plant,
control_context, "body", "left_finger")
from_control = to_control.inverse()
def viz_monitor(control_d_context):
# Simulate control, visualizing in original diagram.
assert (control_context is
control_plant.GetMyContextFromRoot(control_d_context))
from_control.copy_state(control_context, context)
d_context.SetTime(control_d_context.get_time())
diagram.Publish(d_context)
return EventStatus.DidNothing()
simulator = Simulator(control_diagram, control_d_context)
simulator.Initialize()
if VISUALIZE:
simulator.set_monitor(viz_monitor)
simulator.set_target_realtime_rate(1.)
simulator.AdvanceTo(t_end)
env_plant.RegisterVisualGeometry(body, pos, Box(sz[0], sz[1], sz[2]),
str(idx + 1) + 'v',
np.array([1, 1, 1, 1]), scene_graph)
env_plant.Finalize()
plant.RegisterGeometry(scene_graph)
builder.Connect(plant.get_geometry_pose_output_port(),
scene_graph.get_source_pose_port(plant.source_id()))
meshcat = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=None))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.get_input_port(0))
# end setup for visualization
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()
# Run simulation
context.set_time(0.)
mutable_state_vector = context.get_continuous_state_vector().CopyToVector()
mutable_state_vector[:12] = START_STATE
context.SetContinuousState(mutable_state_vector)
simulator.Initialize()
import pdb
pdb.set_trace()
from time import sleep
sleep(3)
simulator.StepTo(DURATION)
def main():
args_parser = argparse.ArgumentParser(
description=__doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
args_parser.add_argument(
"filename", type=str,
help="Path to an SDF or URDF file.")
args_parser.add_argument(
"--package_path",
type=str,
default=None,
help="Full path to the root package for reading in SDF resources.")
position_group = args_parser.add_mutually_exclusive_group()
position_group.add_argument(
"--position", type=float, nargs="+", default=[],
help="A list of positions which must be the same length as the number "
"of positions in the sdf model. Note that most models have a "
"floating-base joint by default (unless the sdf explicitly welds "
"the base to the world, and so have 7 positions corresponding to "
"the quaternion representation of that floating-base position.")
position_group.add_argument(
"--joint_position", type=float, nargs="+", default=[],
help="A list of positions which must be the same length as the number "
"of positions ASSOCIATED WITH JOINTS in the sdf model. This "
"does not include, e.g., floating-base coordinates, which will "
"be assigned a default value.")
args_parser.add_argument(
"--test", action='store_true',
help="Disable opening the gui window for testing.")
# TODO(russt): Add option to weld the base to the world pending the
# availability of GetUniqueBaseBody requested in #9747.
args = args_parser.parse_args()
filename = args.filename
if not os.path.isfile(filename):
args_parser.error("File does not exist: {}".format(filename))
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# Construct a MultibodyPlant.
plant = MultibodyPlant()
plant.RegisterAsSourceForSceneGraph(scene_graph)
# Create the parser.
parser = Parser(plant)
# Get the package pathname.
if args.package_path:
# Verify that package.xml is found in the designated path.
package_path = os.path.abspath(args.package_path)
if not os.path.isfile(os.path.join(package_path, "package.xml")):
parser.error("package.xml not found at: {}".format(package_path))
# Get the package map and populate it using the package path.
package_map = parser.package_map()
package_map.PopulateFromFolder(package_path)
# Add the model from the file and finalize the plant.
parser.AddModelFromFile(filename)
plant.Finalize(scene_graph)
# Add sliders to set positions of the joints.
sliders = builder.AddSystem(JointSliders(robot=plant))
to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
builder.Connect(sliders.get_output_port(0), to_pose.get_input_port())
builder.Connect(
to_pose.get_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
# Connect this to drake_visualizer.
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
if len(args.position):
sliders.set_position(args.position)
elif len(args.joint_position):
sliders.set_joint_position(args.joint_position)
# Make the diagram and run it.
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_publish_every_time_step(False)
if args.test:
sliders.window.withdraw()
simulator.StepTo(0.1)
else:
simulator.set_target_realtime_rate(1.0)
simulator.StepTo(np.inf)
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 make_simulator(generator):
simulator = Simulator(system)
simulator.Initialize()
simulator.set_target_realtime_rate(0)
return simulator
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))
if not args.hardware:
# Set the initial positions of the IIWA to a comfortable configuration
# inside the workspace of the station.
q0 = [0, 0.6, 0, -1.75, 0, 1.0, 0]
station.SetIiwaPosition(q0, station_context)
station.SetIiwaVelocity(np.zeros(7), station_context)
# Set the initial configuration of the gripper to open.
def main():
args_parser = argparse.ArgumentParser(
description=__doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
args_parser.add_argument(
"filename", type=str,
help="Path to an SDF or URDF file.")
args_parser.add_argument(
"--package_path",
type=str,
default=None,
help="Full path to the root package for reading in SDF resources.")
position_group = args_parser.add_mutually_exclusive_group()
position_group.add_argument(
"--position", type=float, nargs="+", default=[],
help="A list of positions which must be the same length as the number "
"of positions in the sdf model. Note that most models have a "
"floating-base joint by default (unless the sdf explicitly welds "
"the base to the world, and so have 7 positions corresponding to "
"the quaternion representation of that floating-base position.")
position_group.add_argument(
"--joint_position", type=float, nargs="+", default=[],
help="A list of positions which must be the same length as the number "
"of positions ASSOCIATED WITH JOINTS in the sdf model. This "
"does not include, e.g., floating-base coordinates, which will "
"be assigned a default value.")
args_parser.add_argument(
"--test", action='store_true',
help="Disable opening the gui window for testing.")
# TODO(russt): Add option to weld the base to the world pending the
# availability of GetUniqueBaseBody requested in #9747.
MeshcatVisualizer.add_argparse_argument(args_parser)
args = args_parser.parse_args()
filename = args.filename
if not os.path.isfile(filename):
args_parser.error("File does not exist: {}".format(filename))
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# Construct a MultibodyPlant.
plant = MultibodyPlant(0.0)
plant.RegisterAsSourceForSceneGraph(scene_graph)
# Create the parser.
parser = Parser(plant)
# Get the package pathname.
if args.package_path:
# Verify that package.xml is found in the designated path.
package_path = os.path.abspath(args.package_path)
if not os.path.isfile(os.path.join(package_path, "package.xml")):
parser.error("package.xml not found at: {}".format(package_path))
# Get the package map and populate it using the package path.
package_map = parser.package_map()
package_map.PopulateFromFolder(package_path)
# Add the model from the file and finalize the plant.
parser.AddModelFromFile(filename)
plant.Finalize()
# Add sliders to set positions of the joints.
sliders = builder.AddSystem(JointSliders(robot=plant))
to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
builder.Connect(sliders.get_output_port(0), to_pose.get_input_port())
builder.Connect(
to_pose.get_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
# Connect this to drake_visualizer.
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
# Connect to Meshcat.
if args.meshcat is not None:
meshcat_viz = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=args.meshcat))
builder.Connect(
scene_graph.get_pose_bundle_output_port(),
meshcat_viz.get_input_port(0))
if len(args.position):
sliders.set_position(args.position)
elif len(args.joint_position):
sliders.set_joint_position(args.joint_position)
# Make the diagram and run it.
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_publish_every_time_step(False)
if args.test:
sliders.window.withdraw()
simulator.AdvanceTo(0.1)
else:
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(np.inf)
def test_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_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()))
# One input, one output, two state variables.
VectorSystem.__init__(self, 1, 2, direct_feedthrough=False)
self.DeclareContinuousState(2)
# qqdot(t) = u(t)
def DoCalcVectorTimeDerivatives(self, context, u, x, xdot):
xdot[0] = x[1]
xdot[1] = u
# y(t) = x(t)
def DoCalcVectorOutput(self, context, u, x, y):
y[:] = x
plant = DoubleIntegrator()
simulator = Simulator(plant)
options = DynamicProgrammingOptions()
def min_time_cost(context):
x = context.get_continuous_state_vector().CopyToVector()
if x.dot(x) < .05:
return 0.
return 1.
def quadratic_regulator_cost(context):
x = context.get_continuous_state_vector().CopyToVector()
u = plant.EvalVectorInput(context, 0).CopyToVector()
return x.dot(x) + 20 * u.dot(u)
builder = DiagramBuilder()
plant = builder.AddSystem(QuadrotorPlant())
controller = builder.AddSystem(StabilizingLQRController(plant, [0, 0, 1]))
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 in MeshCat
scene_graph = builder.AddSystem(SceneGraph())
QuadrotorGeometry.AddToBuilder(builder, plant.get_output_port(0), scene_graph)
meshcat = builder.AddSystem(MeshcatVisualizer(
scene_graph, zmq_url=args.meshcat,
open_browser=args.open_browser))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.get_input_port(0))
# end setup for visualization
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()
for i in range(args.trials):
context.SetTime(0.)
context.SetContinuousState(np.random.randn(12,))
simulator.Initialize()
simulator.StepTo(args.duration)
def test_all_leaf_system_overrides(self):
test = self
class TrivialSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.called_publish = False
self.called_feedthrough = False
self.called_continuous = False
self.called_discrete = False
self.called_initialize = False
self.called_per_step = False
self.called_periodic = False
self.called_getwitness = False
self.called_witness = False
self.called_guard = False
self.called_reset = False
# Ensure we have desired overloads.
self.DeclarePeriodicPublish(1.0)
self.DeclarePeriodicPublish(1.0, 0)
self.DeclarePeriodicPublish(period_sec=1.0, offset_sec=0.)
self.DeclarePeriodicDiscreteUpdate(period_sec=1.0,
offset_sec=0.)
self.DeclareInitializationEvent(event=PublishEvent(
trigger_type=TriggerType.kInitialization,
callback=self._on_initialize))
self.DeclarePerStepEvent(
event=PublishEvent(trigger_type=TriggerType.kPerStep,
callback=self._on_per_step))
self.DeclarePeriodicEvent(
period_sec=1.0,
offset_sec=0.0,
event=PublishEvent(trigger_type=TriggerType.kPeriodic,
callback=self._on_periodic))
self.DeclareContinuousState(2)
self.DeclareDiscreteState(1)
# Ensure that we have inputs / outputs to call direct
# feedthrough.
self.DeclareInputPort(PortDataType.kVectorValued, 1)
self.DeclareVectorInputPort(name="test_input",
model_vector=BasicVector(1),
random_type=None)
self.DeclareVectorOutputPort("noop",
BasicVector(1),
noop,
prerequisites_of_calc=set(
[self.nothing_ticket()]))
self.witness = self.MakeWitnessFunction(
"witness", WitnessFunctionDirection.kCrossesZero,
self._witness)
self.reset_witness = self.MakeWitnessFunction(
"reset", WitnessFunctionDirection.kCrossesZero,
self._guard, UnrestrictedUpdateEvent(self._reset))
def DoPublish(self, context, events):
# Call base method to ensure we do not get recursion.
LeafSystem.DoPublish(self, context, events)
# N.B. We do not test for a singular call to `DoPublish`
# (checking `assertFalse(self.called_publish)` first) because
# the above `_DeclareInitializationEvent` will call both its
# callback and this event when invoked via
# `Simulator::Initialize` from `call_leaf_system_overrides`,
# even when we explicitly say not to publish at initialize.
self.called_publish = True
def DoHasDirectFeedthrough(self, input_port, output_port):
# Test inputs.
test.assertIn(input_port, [0, 1])
test.assertEqual(output_port, 0)
# Call base method to ensure we do not get recursion.
with catch_drake_warnings(expected_count=1):
base_return = LeafSystem.DoHasDirectFeedthrough(
self, input_port, output_port)
test.assertTrue(base_return is None)
# Return custom methods.
self.called_feedthrough = True
return False
def DoCalcTimeDerivatives(self, context, derivatives):
# Note: Don't call base method here; it would abort because
# derivatives.size() != 0.
test.assertEqual(derivatives.get_vector().size(), 2)
self.called_continuous = True
def DoCalcDiscreteVariableUpdates(self, context, events,
discrete_state):
# Call base method to ensure we do not get recursion.
LeafSystem.DoCalcDiscreteVariableUpdates(
self, context, events, discrete_state)
self.called_discrete = True
def DoGetWitnessFunctions(self, context):
self.called_getwitness = True
return [self.witness, self.reset_witness]
def _on_initialize(self, context, event):
test.assertIsInstance(context, Context)
test.assertIsInstance(event, PublishEvent)
test.assertFalse(self.called_initialize)
self.called_initialize = True
def _on_per_step(self, context, event):
test.assertIsInstance(context, Context)
test.assertIsInstance(event, PublishEvent)
self.called_per_step = True
def _on_periodic(self, context, event):
test.assertIsInstance(context, Context)
test.assertIsInstance(event, PublishEvent)
test.assertFalse(self.called_periodic)
self.called_periodic = True
def _witness(self, context):
test.assertIsInstance(context, Context)
self.called_witness = True
return 1.0
def _guard(self, context):
test.assertIsInstance(context, Context)
self.called_guard = True
return context.get_time() - 0.5
def _reset(self, context, event, state):
test.assertIsInstance(context, Context)
test.assertIsInstance(event, UnrestrictedUpdateEvent)
test.assertIsInstance(state, State)
self.called_reset = True
system = TrivialSystem()
self.assertFalse(system.called_publish)
self.assertFalse(system.called_feedthrough)
self.assertFalse(system.called_continuous)
self.assertFalse(system.called_discrete)
self.assertFalse(system.called_initialize)
results = call_leaf_system_overrides(system)
self.assertTrue(system.called_publish)
self.assertTrue(system.called_feedthrough)
self.assertFalse(results["has_direct_feedthrough"])
self.assertTrue(system.called_continuous)
self.assertTrue(system.called_discrete)
self.assertTrue(system.called_initialize)
self.assertEqual(results["discrete_next_t"], 1.0)
self.assertFalse(system.HasAnyDirectFeedthrough())
self.assertFalse(system.HasDirectFeedthrough(output_port=0))
self.assertFalse(
system.HasDirectFeedthrough(input_port=0, output_port=0))
# Test explicit calls.
system = TrivialSystem()
context = system.CreateDefaultContext()
system.Publish(context)
self.assertTrue(system.called_publish)
context_update = context.Clone()
system.CalcTimeDerivatives(
context=context,
derivatives=context_update.get_mutable_continuous_state())
self.assertTrue(system.called_continuous)
witnesses = system.GetWitnessFunctions(context)
self.assertEqual(len(witnesses), 2)
system.CalcDiscreteVariableUpdates(
context=context,
discrete_state=context_update.get_mutable_discrete_state())
self.assertTrue(system.called_discrete)
# Test per-step, periodic, and witness call backs
system = TrivialSystem()
simulator = Simulator(system)
simulator.get_mutable_context().SetAccuracy(0.1)
# Stepping to 0.99 so that we get exactly one periodic event.
simulator.AdvanceTo(0.99)
self.assertTrue(system.called_per_step)
self.assertTrue(system.called_periodic)
self.assertTrue(system.called_getwitness)
self.assertTrue(system.called_witness)
self.assertTrue(system.called_guard)
self.assertTrue(system.called_reset)
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()))
