simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Simulator time steps will be very small, so don't
# force the rest of the system to update every single time.
simulator.set_publish_every_time_step(False)
# From iiwa_wsg_simulation.cc:
# When using the default RK3 integrator, the simulation stops
# advancing once the gripper grasps the box. Grasping makes the
# problem computationally stiff, which brings the default RK3
# integrator to its knees.
timestep = 0.00005
integrator = RungeKutta2Integrator(diagram, timestep,
simulator.get_mutable_context())
simulator.reset_integrator(integrator)
# The simulator simulates forward from a given Context,
# so we adjust the simulator's initial Context to set up
# the initial state.
state = simulator.get_mutable_context().\
get_mutable_continuous_state_vector()
initial_state = np.zeros(x.shape)
initial_state[0:x.shape[0]] = x.copy()
state.SetFromVector(initial_state)
simulator.get_mutable_context().set_time(t)
# This kicks off simulation.
rbt_new = None
try:
simulator.StepTo(args.duration)
# so we adjust the simulator's initial Context to set up
# the initial state.
state = simulator.get_mutable_context().\
get_mutable_continuous_state_vector()
initial_state = np.zeros(state.size())
initial_state[0:q0.shape[0]] = q0
state.SetFromVector(initial_state)
# From iiwa_wsg_simulation.cc:
# When using the default RK3 integrator, the simulation stops
# advancing once the gripper grasps the box. Grasping makes the
# problem computationally stiff, which brings the default RK3
# integrator to its knees.
timestep = 0.0002
simulator.reset_integrator(
RungeKutta2Integrator(diagram, timestep,
simulator.get_mutable_context()))
# This kicks off simulation. Most of the run time will be spent
# in this call.
simulator.StepTo(args.duration)
print("Final state: ", state.CopyToVector())
if args.test is not True:
# Do some plotting to show off accessing signal logger data.
nq = rbt.get_num_positions()
plt.figure()
plt.subplot(3, 1, 1)
dims_to_draw = range(7)
color = iter(plt.cm.rainbow(np.linspace(0, 1, 7)))
for i in dims_to_draw: