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_integrator_constructors(self):
"""Test all constructors for all integrator types."""
sys = ConstantVectorSource([1])
con = sys.CreateDefaultContext()
RungeKutta2Integrator(system=sys, max_step_size=0.01)
RungeKutta2Integrator(system=sys, max_step_size=0.01, context=con)
RungeKutta3Integrator(system=sys)
RungeKutta3Integrator(system=sys, context=con)
def test_copy(self):
# Copy a context using `copy` or `clone`.
system = ConstantVectorSource([1])
context = system.CreateDefaultContext()
context_2 = copy.copy(context)
self.assertNotEquals(context, context_2)
context_3 = context.Clone()
self.assertNotEquals(context, context_3)
def test_copy(self):
# Copy a context using `deepcopy` or `clone`.
system = ConstantVectorSource([1])
context = system.CreateDefaultContext()
context_copies = [
copy.copy(context),
copy.deepcopy(context),
context.Clone(),
]
# TODO(eric.cousineau): Compare copies.
for context_copy in context_copies:
self.assertTrue(context_copy is not context)
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 main():
# Simulate with doubles.
builder = DiagramBuilder()
source = builder.AddSystem(ConstantVectorSource([10.]))
adder = builder.AddSystem(SimpleAdder(100.))
builder.Connect(source.get_output_port(0), adder.get_input_port(0))
logger = builder.AddSystem(SignalLogger(1))
builder.Connect(adder.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.AdvanceTo(1)
x = logger.data()
print("Output values: {}".format(x))
assert np.allclose(x, 110.)
# Compute outputs with AutoDiff and Symbolic.
for T in (AutoDiffXd, Expression):
adder_T = SimpleAdder_[T](100.)
context = adder_T.CreateDefaultContext()
adder_T.get_input_port().FixValue(context, [10.])
output = adder_T.AllocateOutput()
adder_T.CalcOutput(context, output)
# N.B. At present, you cannot get a reference to existing AutoDiffXd
# or Expression numpy arrays, so we will explictly copy the vector:
# https://github.com/RobotLocomotion/drake/issues/8116
value, = output.get_vector_data(0).CopyToVector()
assert isinstance(value, T)
print("Output from {}: {}".format(type(adder_T), repr(value)))
def test_connect_lcm_scope(self):
builder = DiagramBuilder()
source = builder.AddSystem(ConstantVectorSource(np.zeros(4)))
mut.ConnectLcmScope(src=source.get_output_port(0),
channel="TEST_CHANNEL",
builder=builder,
lcm=DrakeMockLcm())
def test_signal_logger(self):
# Log the output of a simple diagram containing a constant
# source and an integrator.
builder = DiagramBuilder()
kValue = 2.4
source = builder.AddSystem(ConstantVectorSource([kValue]))
kSize = 1
integrator = builder.AddSystem(Integrator(kSize))
logger_per_step = builder.AddSystem(SignalLogger(kSize))
builder.Connect(source.get_output_port(0),
integrator.get_input_port(0))
builder.Connect(integrator.get_output_port(0),
logger_per_step.get_input_port(0))
# Add a redundant logger via the helper method.
logger_per_step_2 = LogOutput(integrator.get_output_port(0), builder)
# Add a periodic logger
logger_periodic = builder.AddSystem(SignalLogger(kSize))
kPeriod = 0.1
logger_periodic.set_publish_period(kPeriod)
builder.Connect(integrator.get_output_port(0),
logger_periodic.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
kTime = 1.
simulator.AdvanceTo(kTime)
# Verify outputs of the every-step logger
t = logger_per_step.sample_times()
x = logger_per_step.data()
self.assertTrue(t.shape[0] > 2)
self.assertTrue(t.shape[0] == x.shape[1])
self.assertAlmostEqual(x[0, -1], t[-1] * kValue, places=2)
np.testing.assert_array_equal(x, logger_per_step_2.data())
# Verify outputs of the periodic logger
t = logger_periodic.sample_times()
x = logger_periodic.data()
# Should log exactly once every kPeriod, up to and including kTime.
self.assertTrue(t.shape[0] == np.floor(kTime / kPeriod) + 1.)
logger_per_step.reset()
# Verify that t and x retain their values after systems are deleted.
t_copy = t.copy()
x_copy = x.copy()
del builder
del integrator
del logger_periodic
del logger_per_step
del logger_per_step_2
del diagram
del simulator
del source
gc.collect()
self.assertTrue((t == t_copy).all())
self.assertTrue((x == x_copy).all())
def test_minimal_simulation(self):
# Create a simple system.
system = ConstantVectorSource([1.])
def make_simulator(generator):
simulator = Simulator(system)
simulator.Initialize()
simulator.set_target_realtime_rate(0)
return simulator
def calc_output(system, context):
return 42.
result = RandomSimulation(make_simulator=make_simulator,
output=calc_output,
final_time=1.0,
generator=RandomGenerator())
self.assertEqual(result, 42.)
result = MonteCarloSimulation(make_simulator=make_simulator,
output=calc_output,
final_time=1.0,
num_samples=10,
generator=RandomGenerator())
self.assertIsInstance(result, list)
self.assertEqual(len(result), 10)
self.assertIsInstance(result[0], RandomSimulationResult)
self.assertIsInstance(result[0].generator_snapshot, RandomGenerator)
self.assertEqual(result[0].output, 42.)
for i in range(1, len(result)):
self.assertIsNot(result[0].generator_snapshot,
result[i].generator_snapshot)
def test_simulator_integrator_manipulation(self):
# TODO(eric.cousineau): Move this to `analysis_test.py`.
system = ConstantVectorSource([1])
# Create simulator with basic constructor.
simulator = Simulator(system)
simulator.Initialize()
simulator.set_target_realtime_rate(0)
integrator = simulator.get_mutable_integrator()
target_accuracy = 1E-6
integrator.set_target_accuracy(target_accuracy)
self.assertEqual(integrator.get_target_accuracy(), target_accuracy)
maximum_step_size = 0.2
integrator.set_maximum_step_size(maximum_step_size)
self.assertEqual(integrator.get_maximum_step_size(), maximum_step_size)
minimum_step_size = 2E-2
integrator.set_requested_minimum_step_size(minimum_step_size)
self.assertEqual(integrator.get_requested_minimum_step_size(),
minimum_step_size)
integrator.set_throw_on_minimum_step_size_violation(True)
self.assertTrue(integrator.get_throw_on_minimum_step_size_violation())
integrator.set_fixed_step_mode(True)
self.assertTrue(integrator.get_fixed_step_mode())
const_integrator = simulator.get_integrator()
self.assertTrue(const_integrator is integrator)
def test_eval(self):
"""Tests evaluation (e.g. caching, API sugars, etc.)."""
# `Eval` and `EvalAbstract`: Test with constant systems.
model_values = [
AbstractValue.Make("Hello World"),
AbstractValue.Make(BasicVector([1., 2., 3.])),
]
for model_value in model_values:
is_abstract = not isinstance(model_value.get_value(), BasicVector)
if is_abstract:
zoh = ConstantValueSource(copy.copy(model_value))
else:
zoh = ConstantVectorSource(model_value.get_value().get_value())
context = zoh.CreateDefaultContext()
output_port = zoh.get_output_port(0)
value_abstract = output_port.EvalAbstract(context)
value = output_port.Eval(context)
self.assertEqual(type(value_abstract), type(model_value))
self.assertEqual(type(value), type(model_value.get_value()))
if is_abstract:
check = self.assertEqual
else:
def check(a, b):
self.assertEqual(type(a.get_value()), type(b.get_value()))
np.testing.assert_equal(a.get_value(), b.get_value())
check(value_abstract.get_value(), model_value.get_value())
check(value, model_value.get_value())
def test_simulator_context_manipulation(self):
system = ConstantVectorSource([1])
# Use default-constructed context.
simulator = Simulator(system)
self.assertTrue(simulator.has_context())
context_default = simulator.get_mutable_context()
# WARNING: Once we call `simulator.reset_context()`, it will delete the
# context it currently owns, which is `context_default` in this case.
# BE CAREFUL IN SITUATIONS LIKE THIS!
# TODO(eric.cousineau): Bind `release_context()`, or migrate context
# usage to use `shared_ptr`.
context = system.CreateDefaultContext()
simulator.reset_context(context)
self.assertIs(context, simulator.get_mutable_context())
# WARNING: This will also invalidate `context`. Be careful!
simulator.reset_context(None)
self.assertFalse(simulator.has_context())
def build_with_no_control(builder, plant, model):
"""Connects a zero-torque input to a given model instance in a plant."""
# TODO(eric.cousineau): Use `multibody_plant_prototypes.control` if the dependency can
# be simplified.
nu = plant.num_actuated_dofs(model)
constant = builder.AddSystem(ConstantVectorSource(np.zeros(nu)))
builder.Connect(constant.get_output_port(0),
plant.get_actuation_input_port(model))
def test_deprecated_connect_lcm_scope(self):
builder = DiagramBuilder()
source = builder.AddSystem(ConstantVectorSource(np.zeros(4)))
with catch_drake_warnings(expected_count=1):
mut.ConnectLcmScope(src=source.get_output_port(0),
channel="TEST_CHANNEL",
builder=builder,
lcm=DrakeLcm(),
publish_period=0.001)
def test_ctor_api(self):
"""Tests construction of systems for systems whose executions semantics
are not tested above.
"""
ConstantValueSource(AbstractValue.Make("Hello world"))
ConstantVectorSource(source_value=[1., 2.])
ZeroOrderHold(period_sec=0.1, vector_size=2)
ZeroOrderHold(period_sec=0.1,
abstract_model_value=AbstractValue.Make("Hello world"))
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)
simulator.set_publish_every_time_step(publish=True)
simulator.set_publish_at_initialization(publish=True)
simulator.set_target_realtime_rate(realtime_rate=1.0)
def test_simulator_flags(self):
system = ConstantVectorSource([1])
simulator = Simulator(system)
ResetIntegratorFromFlags(simulator, "runge_kutta2", 0.00123)
integrator = simulator.get_integrator()
self.assertEqual(type(integrator), RungeKutta2Integrator)
self.assertEqual(integrator.get_maximum_step_size(), 0.00123)
self.assertGreater(len(GetIntegrationSchemes()), 5)
def test_simulator_integrator_manipulation(self):
# TODO(eric.cousineau): Move this to `analysis_test.py`.
system = ConstantVectorSource([1])
# Create simulator with basic constructor.
simulator = Simulator(system)
simulator.Initialize()
simulator.set_target_realtime_rate(0)
integrator = simulator.get_mutable_integrator()
target_accuracy = 1E-6
integrator.set_target_accuracy(target_accuracy)
self.assertEqual(integrator.get_target_accuracy(), target_accuracy)
maximum_step_size = 0.2
integrator.set_maximum_step_size(maximum_step_size)
self.assertEqual(integrator.get_maximum_step_size(), maximum_step_size)
minimum_step_size = 2E-2
integrator.set_requested_minimum_step_size(minimum_step_size)
self.assertEqual(integrator.get_requested_minimum_step_size(),
minimum_step_size)
integrator.set_throw_on_minimum_step_size_violation(True)
self.assertTrue(integrator.get_throw_on_minimum_step_size_violation())
integrator.set_fixed_step_mode(True)
self.assertTrue(integrator.get_fixed_step_mode())
const_integrator = simulator.get_integrator()
self.assertTrue(const_integrator is integrator)
# Test context-less constructors for
# integrator types.
test_integrator = RungeKutta2Integrator(
system=system, max_step_size=0.01)
test_integrator = RungeKutta3Integrator(system=system)
# Test simulator's reset_integrator, and also the full constructors for
# all integrator types.
rk2 = RungeKutta2Integrator(
system=system,
max_step_size=0.01,
context=simulator.get_mutable_context())
with catch_drake_warnings(expected_count=1):
# TODO(12873) We need an API for this that isn't deprecated.
simulator.reset_integrator(rk2)
rk3 = RungeKutta3Integrator(
system=system,
context=simulator.get_mutable_context())
with catch_drake_warnings(expected_count=1):
# TODO(12873) We need an API for this that isn't deprecated.
simulator.reset_integrator(rk3)
def test_vector_output_port_eval(self):
np_value = np.array([1., 2., 3.])
model_value = AbstractValue.Make(BasicVector(np_value))
source = ConstantVectorSource(np_value)
context = source.CreateDefaultContext()
output_port = source.get_output_port(0)
value = output_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
np.testing.assert_equal(value, np_value)
value_abs = output_port.EvalAbstract(context)
self.assertEqual(type(value_abs), type(model_value))
self.assertEqual(type(value_abs.get_value().get_value()), np.ndarray)
np.testing.assert_equal(value_abs.get_value().get_value(), np_value)
basic = output_port.EvalBasicVector(context)
self.assertEqual(type(basic), BasicVector)
self.assertEqual(type(basic.get_value()), np.ndarray)
np.testing.assert_equal(basic.get_value(), np_value)
def test_vector_output_port_eval(self):
np_value = np.array([1., 2., 3.])
model_value = AbstractValue.Make(BasicVector(np_value))
source = ConstantVectorSource(np_value)
context = source.CreateDefaultContext()
output_port = source.get_output_port(0)
value = output_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
np.testing.assert_equal(value, np_value)
value_abs = output_port.EvalAbstract(context)
self.assertEqual(type(value_abs), type(model_value))
self.assertEqual(type(value_abs.get_value().get_value()), np.ndarray)
np.testing.assert_equal(value_abs.get_value().get_value(), np_value)
basic = output_port.EvalBasicVector(context)
self.assertEqual(type(basic), BasicVector)
self.assertEqual(type(basic.get_value()), np.ndarray)
np.testing.assert_equal(basic.get_value(), np_value)
def test_simulator_flags(self):
# TODO(eric.cousineau): Move this to `analysis_test.py`.
system = ConstantVectorSource([1])
simulator = Simulator(system)
ResetIntegratorFromFlags(simulator, "runge_kutta2", 0.00123)
integrator = simulator.get_integrator()
self.assertEqual(type(integrator), RungeKutta2Integrator)
self.assertEqual(integrator.get_maximum_step_size(), 0.00123)
self.assertGreater(len(GetIntegrationSchemes()), 5)
def test_lcm_scope(self):
builder = DiagramBuilder()
source = builder.AddSystem(ConstantVectorSource(np.zeros(4)))
scope, publisher = mut.LcmScopeSystem.AddToBuilder(
builder=builder,
lcm=DrakeLcm(),
signal=source.get_output_port(0),
channel="TEST_CHANNEL",
publish_period=0.001)
self.assertIsInstance(scope, mut.LcmScopeSystem)
self.assertIsInstance(publisher, mut.LcmPublisherSystem)
def main():
builder = DiagramBuilder()
source = builder.AddSystem(ConstantVectorSource([10.]))
logger = builder.AddSystem(VectorLogSink(1))
builder.Connect(source.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.AdvanceTo(1)
x = logger.FindLog(simulator.get_context()).data()
print("Output values: {}".format(x))
assert np.allclose(x, 10.)
def 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_simulator_integrator_manipulation(self):
system = ConstantVectorSource([1])
# Create simulator with basic constructor.
simulator = Simulator(system)
simulator.Initialize()
simulator.set_target_realtime_rate(0)
integrator = simulator.get_mutable_integrator()
target_accuracy = 1E-6
integrator.set_target_accuracy(target_accuracy)
self.assertEqual(integrator.get_target_accuracy(), target_accuracy)
maximum_step_size = 0.2
integrator.set_maximum_step_size(maximum_step_size)
self.assertEqual(integrator.get_maximum_step_size(), maximum_step_size)
minimum_step_size = 2E-2
integrator.set_requested_minimum_step_size(minimum_step_size)
self.assertEqual(integrator.get_requested_minimum_step_size(),
minimum_step_size)
integrator.set_throw_on_minimum_step_size_violation(True)
self.assertTrue(integrator.get_throw_on_minimum_step_size_violation())
integrator.set_fixed_step_mode(True)
self.assertTrue(integrator.get_fixed_step_mode())
const_integrator = simulator.get_integrator()
self.assertTrue(const_integrator is integrator)
# Test context-less constructors for
# integrator types.
test_integrator = RungeKutta2Integrator(
system=system, max_step_size=0.01)
test_integrator = RungeKutta3Integrator(system=system)
# Test simulator's reset_integrator,
# and also the full constructors for
# all integrator types.
simulator.reset_integrator(
RungeKutta2Integrator(
system=system,
max_step_size=0.01,
context=simulator.get_mutable_context()))
simulator.reset_integrator(
RungeKutta3Integrator(
system=system,
context=simulator.get_mutable_context()))
def test_simulator_status(self):
SimulatorStatus.ReturnReason.kReachedBoundaryTime
SimulatorStatus.ReturnReason.kReachedTerminationCondition
SimulatorStatus.ReturnReason.kEventHandlerFailed
system = ConstantVectorSource([1.])
simulator = Simulator(system)
status = simulator.AdvanceTo(1.)
self.assertRegex(status.FormatMessage(),
"^Simulator successfully reached the boundary time")
self.assertTrue(status.succeeded())
self.assertEqual(status.boundary_time(), 1.)
self.assertEqual(status.return_time(), 1.)
self.assertEqual(status.reason(),
SimulatorStatus.ReturnReason.kReachedBoundaryTime)
self.assertIsNone(status.system())
self.assertEqual(status.message(), "")
self.assertTrue(status.IsIdenticalStatus(other=status))
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("sdf_path", help="path to sdf")
parser.add_argument("--interactive", action='store_true')
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
plant = MultibodyPlant(time_step=0.01)
plant.RegisterAsSourceForSceneGraph(scene_graph)
parser = Parser(plant)
model = parser.AddModelFromFile(args.sdf_path)
plant.Finalize()
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder, output_port=scene_graph.get_query_output_port(),
zmq_url=args.meshcat, open_browser=args.open_browser)
if args.interactive:
# Add sliders to set positions of the joints.
sliders = builder.AddSystem(JointSliders(robot=plant))
else:
q0 = plant.GetPositions(plant.CreateDefaultContext())
sliders = builder.AddSystem(ConstantVectorSource(q0))
to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
builder.Connect(sliders.get_output_port(0), to_pose.get_input_port())
builder.Connect(
to_pose.get_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(1E6)
def test_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_constant_vector_source(self):
source = ConstantVectorSource(source_value=[1., 2.])
context = source.CreateDefaultContext()
source.get_source_value(context)
source.get_mutable_source_value(context)
def main():
goal_position = np.array([0.5, 0., 0.025])
blue_box_clean_position = [0.4, 0., 0.05]
red_box_clean_position = [0.6, 0., 0.05]
goal_delta = 0.05
parser = argparse.ArgumentParser(description=__doc__)
MeshcatVisualizer.add_argparse_argument(parser)
parser.add_argument('--use_meshcat',
action='store_true',
help="Must be set for meshcat to be used.")
parser.add_argument('--disable_planar_viz',
action='store_true',
help="Don't create a planar visualizer. Probably"
" breaks something that assumes the planar"
" vis exists.")
parser.add_argument('--teleop',
action='store_true',
help="Enable teleop, so *don't* use the state machine"
" and motion primitives.")
args = parser.parse_args()
builder = DiagramBuilder()
# Set up the ManipulationStation
station = builder.AddSystem(ManipulationStation(0.001))
mbp = station.get_multibody_plant()
station.SetupManipulationClassStation()
add_goal_region_visual_geometry(mbp, goal_position, goal_delta)
add_box_at_location(mbp,
name="blue_box",
color=[0.25, 0.25, 1., 1.],
pose=RigidTransform(p=[0.4, 0.0, 0.025]))
add_box_at_location(mbp,
name="red_box",
color=[1., 0.25, 0.25, 1.],
pose=RigidTransform(p=[0.6, 0.0, 0.025]))
station.Finalize()
iiwa_q0 = np.array([0.0, 0.6, 0.0, -1.75, 0., 1., np.pi / 2.])
# Attach a visualizer.
if args.use_meshcat:
meshcat = builder.AddSystem(
MeshcatVisualizer(station.get_scene_graph(), zmq_url=args.meshcat))
builder.Connect(station.GetOutputPort("pose_bundle"),
meshcat.get_input_port(0))
if not args.disable_planar_viz:
plt.gca().clear()
viz = builder.AddSystem(
PlanarSceneGraphVisualizer(station.get_scene_graph(),
xlim=[0.25, 0.8],
ylim=[-0.1, 0.5],
ax=plt.gca()))
builder.Connect(station.GetOutputPort("pose_bundle"),
viz.get_input_port(0))
plt.draw()
# Hook up DifferentialIK, since both control modes use it.
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 = 1.0
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))
differential_ik.set_name("Differential IK")
differential_ik.parameters.set_nominal_joint_position(iiwa_q0)
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
if not args.teleop:
symbol_list = [
SymbolL2Close("blue_box_in_goal", "blue_box", goal_position,
goal_delta),
SymbolL2Close("red_box_in_goal", "red_box", goal_position,
goal_delta),
SymbolRelativePositionL2("blue_box_on_red_box",
"blue_box",
"red_box",
l2_thresh=0.01,
offset=np.array([0., 0., 0.05])),
SymbolRelativePositionL2("red_box_on_blue_box",
"red_box",
"blue_box",
l2_thresh=0.01,
offset=np.array([0., 0., 0.05])),
]
primitive_list = [
MoveBoxPrimitive("put_blue_box_in_goal", mbp, "blue_box",
goal_position),
MoveBoxPrimitive("put_red_box_in_goal", mbp, "red_box",
goal_position),
MoveBoxPrimitive("put_blue_box_away", mbp, "blue_box",
blue_box_clean_position),
MoveBoxPrimitive("put_red_box_away", mbp, "red_box",
red_box_clean_position),
MoveBoxPrimitive("put_red_box_on_blue_box", mbp, "red_box",
np.array([0., 0., 0.05]), "blue_box"),
MoveBoxPrimitive("put_blue_box_on_red_box", mbp, "blue_box",
np.array([0., 0., 0.05]), "red_box"),
]
task_execution_system = builder.AddSystem(
TaskExectionSystem(
mbp,
symbol_list=symbol_list,
primitive_list=primitive_list,
dfa_json_file="specifications/red_and_blue_boxes_stacking.json"
))
builder.Connect(station.GetOutputPort("plant_continuous_state"),
task_execution_system.GetInputPort("mbp_state_vector"))
builder.Connect(task_execution_system.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
builder.Connect(task_execution_system.get_output_port(1),
station.GetInputPort("wsg_position"))
#movebox = MoveBoxPrimitive("test_move_box", mbp, "red_box", goal_position)
#rpy_xyz_trajectory, gripper_traj = movebox.generate_rpyxyz_and_gripper_trajectory(mbp.CreateDefaultContext())
#rpy_xyz_trajectory_source = builder.AddSystem(TrajectorySource(rpy_xyz_trajectory))
#builder.Connect(rpy_xyz_trajectory_source.get_output_port(0),
# differential_ik.GetInputPort("rpy_xyz_desired"))
#wsg_position_source = builder.AddSystem(TrajectorySource(gripper_traj))
#builder.Connect(wsg_position_source.get_output_port(0),
# station.GetInputPort("wsg_position"))
# Target zero feedforward residual torque at all times.
fft = builder.AddSystem(ConstantVectorSource(np.zeros(7)))
builder.Connect(fft.get_output_port(0),
station.GetInputPort("iiwa_feedforward_torque"))
input_force_fix = builder.AddSystem(ConstantVectorSource([40.0]))
builder.Connect(input_force_fix.get_output_port(0),
station.GetInputPort("wsg_force_limit"))
end_time = 10000
else: # Set up teleoperation.
# Hook up a pygame-based keyboard+mouse interface for
# teleoperation, and low pass its output to drive the EE target
# for the differential IK.
print_instructions()
teleop = builder.AddSystem(MouseKeyboardTeleop(grab_focus=True))
filter_ = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=0.005, 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"))
# Target zero feedforward residual torque at all times.
fft = builder.AddSystem(ConstantVectorSource(np.zeros(7)))
builder.Connect(fft.get_output_port(0),
station.GetInputPort("iiwa_feedforward_torque"))
# Simulate functionally forever.
end_time = 10000
# Create symbol log
#symbol_log = SymbolFromTransformLog(
# [SymbolL2Close('at_goal', 'red_box', goal_position, .025),
# SymbolL2Close('at_goal', 'blue_box', goal_position, .025)])
#
#symbol_logger_system = builder.AddSystem(
# SymbolLoggerSystem(
# station.get_multibody_plant(), symbol_logger=symbol_log))
#builder.Connect(
# station.GetOutputPort("plant_continuous_state"),
# symbol_logger_system.GetInputPort("mbp_state_vector"))
# Remaining input ports need to be tied up.
diagram = builder.Build()
g = pydot.graph_from_dot_data(diagram.GetGraphvizString())[0]
g.write_png("system_diagram.png")
diagram_context = diagram.CreateDefaultContext()
station_context = diagram.GetMutableSubsystemContext(
station, diagram_context)
station.SetIiwaPosition(station_context, iiwa_q0)
differential_ik.SetPositions(
diagram.GetMutableSubsystemContext(differential_ik, diagram_context),
iiwa_q0)
if args.teleop:
teleop.SetPose(differential_ik.ForwardKinematics(iiwa_q0))
filter_.set_initial_output_value(
diagram.GetMutableSubsystemContext(filter_, diagram_context),
teleop.get_output_port(0).Eval(
diagram.GetMutableSubsystemContext(teleop, diagram_context)))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(end_time)
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)