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_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.AddForceElement(UniformGravityFieldElement())
kuka.Finalize()
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None, 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.StepTo(.1)
def test_texture_override(self):
"""Draws a textured box to test the texture override pathway."""
object_file_path = FindResourceOrThrow(
"drake/systems/sensors/test/models/box_with_mesh.sdf")
# Find the texture path just to ensure it exists and
# we're testing the code path we want to.
FindResourceOrThrow("drake/systems/sensors/test/models/meshes/box.png")
builder = DiagramBuilder()
plant = MultibodyPlant(0.002)
_, scene_graph = AddMultibodyPlantSceneGraph(builder, plant)
object_model = Parser(plant=plant).AddModelFromFile(object_file_path)
plant.Finalize()
# Add meshcat visualizer.
viz = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
viz.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.StepTo(1.0)
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.StepTo(sim_time)
def main():
# Simulate with doubles.
builder = DiagramBuilder()
source = builder.AddSystem(ConstantVectorSource([10.]))
adder = builder.AddSystem(SimpleAdder(100.))
builder.Connect(source.get_output_port(0), adder.get_input_port(0))
logger = builder.AddSystem(SignalLogger(1))
builder.Connect(adder.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.StepTo(1)
x = logger.data()
print("Output values: {}".format(x))
assert np.allclose(x, 110.)
# Compute outputs with AutoDiff and Symbolic.
for T in (AutoDiffXd, Expression):
adder_T = SimpleAdder_[T](100.)
context = adder_T.CreateDefaultContext()
context.FixInputPort(0, BasicVector_[T]([10.]))
output = adder_T.AllocateOutput()
adder_T.CalcOutput(context, output)
# N.B. At present, you cannot get a reference to existing AutoDiffXd
# or Expression numpy arrays, so we will explictly copy the vector:
# https://github.com/RobotLocomotion/drake/issues/8116
value, = output.get_vector_data(0).CopyToVector()
assert isinstance(value, T)
print("Output from {}: {}".format(type(adder_T), repr(value)))
def 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 RunSimulation(self, real_time_rate=1.0):
'''
The Princess Diaries was a good movie.
'''
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# object_file_path = FindResourceOrThrow(
# "drake/examples/manipulation_station/models/061_foam_brick.sdf")
# sdf_file = FindResourceOrThrow("drake/multibody/benchmarks/acrobot/acrobot.sdf")
# urdf_file = FindResourceOrThrow("drake/multibody/benchmarks/acrobot/acrobot.urdf")
sdf_file = "assets/acrobot.sdf"
urdf_file = "assets/acrobot.urdf"
plant = builder.AddSystem(MultibodyPlant())
plant.RegisterAsSourceForSceneGraph(scene_graph)
Parser(plant, scene_graph).AddModelFromFile(sdf_file)
plant.Finalize(scene_graph)
assert plant.geometry_source_is_registered()
builder.Connect(
plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
builder.Connect(scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
# Add
nn_system = NNSystem(self.pytorch_nn_object)
builder.AddSystem(nn_system)
# NN -> plant
builder.Connect(nn_system.NN_out_output_port,
plant.get_actuation_input_port())
# plant -> NN
builder.Connect(plant.get_continuous_state_output_port(),
nn_system.NN_in_input_port)
# Add meshcat visualizer
meshcat = MeshcatVisualizer(scene_graph)
builder.AddSystem(meshcat)
# builder.Connect(scene_graph.GetOutputPort("lcm_visualization"),
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.GetInputPort("lcm_visualization"))
# build diagram
diagram = builder.Build()
meshcat.load()
# time.sleep(2.0)
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
# context = diagram.GetMutableSubsystemContext(
# self.station, simulator.get_mutable_context())
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
sim_duration = 5.
simulator.StepTo(sim_duration)
print("stepping complete")
def test_signal_logger(self):
# Log the output of a simple diagram containing a constant
# source and an integrator.
builder = DiagramBuilder()
kValue = 2.4
source = builder.AddSystem(ConstantVectorSource([kValue]))
kSize = 1
integrator = builder.AddSystem(Integrator(kSize))
logger = builder.AddSystem(SignalLogger(kSize))
builder.Connect(source.get_output_port(0),
integrator.get_input_port(0))
builder.Connect(integrator.get_output_port(0),
logger.get_input_port(0))
# Add a redundant logger via the helper method.
logger2 = LogOutput(integrator.get_output_port(0), builder)
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.StepTo(1)
t = logger.sample_times()
x = logger.data()
self.assertTrue(t.shape[0] > 2)
self.assertTrue(t.shape[0] == x.shape[1])
self.assertAlmostEqual(x[0, -1], t[-1] * kValue, places=2)
np.testing.assert_array_equal(x, logger2.data())
logger.reset()
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.AddForceElement(UniformGravityFieldElement())
cart_pole.Finalize()
assert cart_pole.geometry_source_is_registered()
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None, 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.StepTo(.1)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate",
type=float,
default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument("--simulation_time",
type=float,
default=10.0,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--time_step",
type=float,
default=0.,
help="If greater than zero, the plant is modeled as a system with "
"discrete updates and period equal to this time_step. "
"If 0, the plant is modeled as a continuous system.")
args = parser.parse_args()
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant(time_step=args.time_step))
AddModelFromSdfFile(file_name=file_name,
plant=cart_pole,
scene_graph=scene_graph)
cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
cart_pole.Finalize(scene_graph)
assert cart_pole.geometry_source_is_registered()
builder.Connect(
cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
lcm = DrakeLcm()
ConnectVisualization(scene_graph=scene_graph, builder=builder, lcm=lcm)
diagram = builder.Build()
DispatchLoadMessage(scene_graph=scene_graph, lcm=lcm)
diagram_context = diagram.CreateDefaultContext()
cart_pole_context = diagram.GetMutableSubsystemContext(
cart_pole, diagram_context)
cart_pole_context.FixInputPort(
cart_pole.get_actuation_input_port().get_index(), [0])
cart_slider = cart_pole.GetJointByName("CartSlider")
pole_pin = cart_pole.GetJointByName("PolePin")
cart_slider.set_translation(context=cart_pole_context, translation=0.)
pole_pin.set_angle(context=cart_pole_context, angle=2.)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.Initialize()
simulator.StepTo(args.simulation_time)
def simulate_splines(diagram, diagram_context, sim_duration, real_time_rate=1.0):
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
diagram.Publish(diagram_context)
user_input('Simulate?')
simulator.StepTo(sim_duration)
user_input('Finish?')
def main():
builder = DiagramBuilder()
source = builder.AddSystem(ConstantVectorSource([10.]))
logger = builder.AddSystem(SignalLogger(1))
builder.Connect(source.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.StepTo(1)
x = logger.data()
print("Output values: {}".format(x))
assert np.allclose(x, 10.)
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_signal_logger(self):
# Log the output of a simple diagram containing a constant
# source and an integrator.
builder = DiagramBuilder()
kValue = 2.4
source = builder.AddSystem(ConstantVectorSource([kValue]))
kSize = 1
integrator = builder.AddSystem(Integrator(kSize))
logger_per_step = builder.AddSystem(SignalLogger(kSize))
builder.Connect(source.get_output_port(0),
integrator.get_input_port(0))
builder.Connect(integrator.get_output_port(0),
logger_per_step.get_input_port(0))
# Add a redundant logger via the helper method.
logger_per_step_2 = LogOutput(integrator.get_output_port(0), builder)
# Add a periodic logger
logger_periodic = builder.AddSystem(SignalLogger(kSize))
kPeriod = 0.1
logger_periodic.set_publish_period(kPeriod)
builder.Connect(integrator.get_output_port(0),
logger_periodic.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
kTime = 1.
simulator.StepTo(kTime)
# Verify outputs of the every-step logger
t = logger_per_step.sample_times()
x = logger_per_step.data()
self.assertTrue(t.shape[0] > 2)
self.assertTrue(t.shape[0] == x.shape[1])
self.assertAlmostEqual(x[0, -1], t[-1] * kValue, places=2)
np.testing.assert_array_equal(x, logger_per_step_2.data())
# Verify outputs of the periodic logger
t = logger_periodic.sample_times()
x = logger_periodic.data()
# Should log exactly once every kPeriod, up to and including kTime.
self.assertTrue(t.shape[0] == np.floor(kTime / kPeriod) + 1.)
logger_per_step.reset()
def test_simulation(self):
# Basic constant-torque 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 cartPoleTest(self):
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant())
AddModelFromSdfFile(file_name=file_name,
plant=cart_pole,
scene_graph=scene_graph)
cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
cart_pole.Finalize(scene_graph)
assert cart_pole.geometry_source_is_registered()
builder.Connect(
cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
visualizer.load()
diagram_context = diagram.CreateDefaultContext()
cart_pole_context = diagram.GetMutableSubsystemContext(
cart_pole, diagram_context)
cart_pole_context.FixInputPort(
cart_pole.get_actuation_input_port().get_index(), [0])
cart_slider = cart_pole.GetJointByName("CartSlider")
pole_pin = cart_pole.GetJointByName("PolePin")
cart_slider.set_translation(context=cart_pole_context, translation=0.)
pole_pin.set_angle(context=cart_pole_context, angle=2.)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.Initialize()
simulator.StepTo(args.duration)
def test_simulation(self):
# Basic compass_gait simulation.
compass_gait = CompassGait()
# Create the simulator.
simulator = Simulator(compass_gait)
context = simulator.get_mutable_context()
context.set_accuracy(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.StepTo(1.0)
self.assertFalse((state.CopyToVector() == initial_state).any())
def test_adder_simulation(self):
builder = DiagramBuilder()
adder = builder.AddSystem(self._create_adder_system())
adder.set_name("custom_adder")
# Add ZOH so we can easily extract state.
zoh = builder.AddSystem(ZeroOrderHold(0.1, 3))
zoh.set_name("zoh")
builder.ExportInput(adder.get_input_port(0))
builder.ExportInput(adder.get_input_port(1))
builder.Connect(adder.get_output_port(0), zoh.get_input_port(0))
diagram = builder.Build()
context = diagram.CreateDefaultContext()
self._fix_adder_inputs(context)
simulator = Simulator(diagram, context)
simulator.Initialize()
simulator.StepTo(1)
# Ensure that we have the outputs we want.
value = (diagram.GetMutableSubsystemContext(
zoh, context).get_discrete_state_vector().get_value())
self.assertTrue(np.allclose([5, 7, 9], value))
def 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 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_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_simulation(self):
# Basic constant-torque pendulum simulation.
builder = framework.DiagramBuilder()
pendulum = builder.AddSystem(PendulumPlant())
source = builder.AddSystem(ConstantVectorSource([1.0]))
builder.Connect(source.get_output_port(0), pendulum.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
# TODO(russt): Clean up state vector access below.
state = simulator.get_mutable_context().get_mutable_state()\
.get_mutable_continuous_state().get_mutable_vector()
initial_state = np.array([1.0, 0.0])
state.SetFromVector(initial_state)
simulator.StepTo(1.0)
self.assertFalse((state.CopyToVector() == initial_state).all())
def 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))
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.
station.SetWsgPosition(0.1, station_context)
station.SetWsgVelocity(0, station_context)
# Place the object in the middle of the workspace.
X_WObject = Isometry3.Identity()
X_WObject.set_translation([.6, 0, 0])
station.get_multibody_plant().tree().SetFreeBodyPoseOrThrow(
station.get_multibody_plant().GetBodyByName("base_link", object),
X_WObject,
station.GetMutableSubsystemContext(station.get_multibody_plant(),
station_context))
# 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)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.StepTo(args.duration)
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 = Isometry3.Identity()
X_TObject.set_translation([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.AddForceElement(UniformGravityFieldElement())
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))
# 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.StepTo(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)
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 args.log:
station_context.FixInputPort(
station.GetInputPort("wsg_force_limit").get_index(),
np.zeros(1) + 40)
station.SetIiwaPosition(station_context, input_dict["q0"])
else:
# Eval the output port once to read the initial positions of the IIWA.
q0 = station.GetOutputPort("iiwa_position_measured").Eval(
station_context)
q0[:] = [0., 0.5, 0., -1.75, 0., 1., 0.]
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:
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(1.0)
print("Starting simulation for %f seconds." % duration)
simulator.StepTo(duration)
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)
import pdb
pdb.set_trace()
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)
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)
AddModelFromSdfFile(args.filename, plant)
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)
