def Simulate2dRamone(x0, duration,
desired_goal = 0.0,
print_period = 0.0):
builder = DiagramBuilder()
tree = RigidBodyTree()
AddModelInstanceFromUrdfFile("ramone_act.urdf", FloatingBaseType.kRollPitchYaw, None, tree)
plant = builder.AddSystem(RigidBodyPlant(tree))
controller = builder.AddSystem(
Ramone2dController(tree,
desired_goal=desired_goal,
print_period=print_period))
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
state_log = builder.AddSystem(SignalLogger(plant.get_num_states()))
builder.Connect(plant.get_output_port(0), state_log.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(True)
simulator.get_mutable_context().set_accuracy(1e-4)
state = simulator.get_mutable_context().get_mutable_continuous_state_vector()
state.SetFromVector(x0)
simulator.StepTo(duration)
return tree, controller, state_log, plant
def runPendulumExample(args):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
parser = Parser(plant)
parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf"))
plant.Finalize()
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
Tview = np.array([[1., 0., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
scene_graph, Tview=Tview, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2]))
builder.Connect(pose_bundle_output_port,
visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the input port to zero.
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.FixInputPort(
plant.get_actuation_input_port().get_index(),
np.zeros(plant.num_actuators()))
plant_context.SetContinuousState([0.5, 0.1])
simulator.StepTo(args.duration)
def runPendulumExample(args):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
parser = Parser(plant)
parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf"))
plant.Finalize()
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
Tview=Tview,
xlim=[-1.2, 1.2],
ylim=[-1.2, 1.2]))
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the input port to zero.
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.FixInputPort(plant.get_actuation_input_port().get_index(),
np.zeros(plant.num_actuators()))
plant_context.SetContinuousState([0.5, 0.1])
simulator.StepTo(args.duration)
def Simulate2dHopper(x0,
duration,
actuators_off=False,
desired_lateral_velocity=0.0,
desired_height=3.0,
print_period=0.0):
# The diagram, plant and contorller
diagram = build_block_diagram(actuators_off, desired_lateral_velocity,
desired_height, print_period)
# Start visualizer recording
visualizer = diagram.GetSubsystemByName('visualizer')
visualizer.start_recording()
simulator = Simulator(diagram)
simulator.Initialize()
plant = diagram.GetSubsystemByName('plant')
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.get_mutable_discrete_state_vector().SetFromVector(x0)
potential = plant.CalcPotentialEnergy(plant_context)
simulator.AdvanceTo(duration)
visualizer.stop_recording()
animation = visualizer.get_recording_as_animation()
controller = diagram.GetSubsystemByName('controller')
state_log = diagram.GetSubsystemByName('state_log')
return plant, controller, state_log, animation
def simulate():
# Animate the resulting policy.
builder = DiagramBuilder()
plant = builder.AddSystem(DoubleIntegrator())
vi_policy = builder.AddSystem(policy)
builder.Connect(plant.get_output_port(0), vi_policy.get_input_port(0))
builder.Connect(vi_policy.get_output_port(0), plant.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
logger = LogOutput(plant.get_output_port(0), builder)
logger.set_name("logger")
simulator.get_mutable_context().SetContinuousState(env.reset())
simulator.AdvanceTo(10. if get_ipython() is not None else 5.)
return logger
def simulateRobot(time, B, v_command):
tree = RigidBodyTree(
FindResource(
os.path.dirname(os.path.realpath(__file__)) +
"/block_pusher2.urdf"), FloatingBaseType.kFixed)
# Set up a block diagram with the robot (dynamics), the controller, and a
# visualization block.
builder = DiagramBuilder()
robot = builder.AddSystem(RigidBodyPlant(tree))
controller = builder.AddSystem(DController(tree, B, v_command))
builder.Connect(robot.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), robot.get_input_port(0))
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
Tview,
xlim=[-2.8, 4.8],
ylim=[-2.8, 10]))
builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0))
logger = builder.AddSystem(
SignalLogger(tree.get_num_positions() + tree.get_num_velocities()))
builder.Connect(robot.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(True)
# Set the initial conditions
context = simulator.get_mutable_context()
state = context.get_mutable_continuous_state_vector()
start1 = 3 * np.pi / 16
start2 = 15 * np.pi / 16
#np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps, np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps
state.SetFromVector(
(start1, start2, -start1, -start2, np.pi + start1, start2,
np.pi - start1, -start2, 1, 1, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
0., 0.)) # (theta1, theta2, theta1dot, theta2dot)
# Simulate for 10 seconds
simulator.StepTo(time)
#import pdb; pdb.set_trace()
return (logger.data()[8:11, :], logger.data()[:8, :],
logger.data()[19:22, :], logger.data()[11:19, :],
logger.sample_times())
def Simulate2dBallAndBeam(x0, duration):
builder = DiagramBuilder()
# Load in the ball and beam from a description file.
tree = RigidBodyTree()
AddModelInstancesFromSdfString(
open("ball_and_beam.sdf", 'r').read(), FloatingBaseType.kFixed, None,
tree)
# A RigidBodyPlant wraps a RigidBodyTree to allow
# forward dynamical simulation.
plant = builder.AddSystem(RigidBodyPlant(tree))
# Spawn a controller and hook it up
controller = builder.AddSystem(BallAndBeam2dController(tree))
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
# Create a logger to log at 30hz
state_log = builder.AddSystem(SignalLogger(plant.get_num_states()))
state_log._DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging
builder.Connect(plant.get_output_port(0), state_log.get_input_port(0))
# Create a simulator
diagram = builder.Build()
simulator = Simulator(diagram)
# Don't limit realtime rate for this sim, since we
# produce a video to render it after simulating the whole thing.
#simulator.set_target_realtime_rate(100.0)
simulator.set_publish_every_time_step(False)
# Force the simulator to use a fixed-step integrator,
# which is much faster for this stiff system. (Due to the
# spring-model of collision, the default variable-timestep
# integrator will take very short steps. I've chosen the step
# size here to be fast while still being stable in most situations.)
integrator = simulator.get_mutable_integrator()
integrator.set_fixed_step_mode(True)
integrator.set_maximum_step_size(0.001)
# Set the initial state
state = simulator.get_mutable_context(
).get_mutable_continuous_state_vector()
state.SetFromVector(x0)
# Simulate!
simulator.StepTo(duration)
return tree, controller, state_log
def jacobian_controller_example():
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation())
station.SetupClutterClearingStation()
station.Finalize()
controller = builder.AddSystem(
PseudoInverseController(station.get_multibody_plant()))
integrator = builder.AddSystem(Integrator(7))
builder.Connect(controller.get_output_port(), integrator.get_input_port())
builder.Connect(integrator.get_output_port(),
station.GetInputPort("iiwa_position"))
builder.Connect(station.GetOutputPort("iiwa_position_measured"),
controller.get_input_port())
meshcat = ConnectMeshcatVisualizer(
builder,
station.get_scene_graph(),
output_port=station.GetOutputPort("pose_bundle"))
diagram = builder.Build()
simulator = Simulator(diagram)
station_context = station.GetMyContextFromRoot(
simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros((7, 1)))
station.GetInputPort("wsg_position").FixValue(station_context, [0.1])
integrator.GetMyContextFromRoot(simulator.get_mutable_context(
)).get_mutable_continuous_state_vector().SetFromVector(
station.GetIiwaPosition(station_context))
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(0.01)
return simulator
def simulate_acrobot():
builder = DiagramBuilder()
acrobot = builder.AddSystem(AcrobotPlant())
saturation = builder.AddSystem(Saturation(min_value=[-10], max_value=[10]))
builder.Connect(saturation.get_output_port(0), acrobot.get_input_port(0))
wrapangles = WrapToSystem(4)
wrapangles.set_interval(0, 0, 2. * math.pi)
wrapangles.set_interval(1, -math.pi, math.pi)
wrapto = builder.AddSystem(wrapangles)
builder.Connect(acrobot.get_output_port(0), wrapto.get_input_port(0))
controller = builder.AddSystem(BalancingLQR())
builder.Connect(wrapto.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
saturation.get_input_port(0))
tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"),
FloatingBaseType.kFixed)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.]))
builder.Connect(acrobot.get_output_port(0), visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
context = simulator.get_mutable_context()
state = context.get_mutable_continuous_state_vector()
parser = argparse.ArgumentParser()
parser.add_argument("-N",
"--trials",
type=int,
help="Number of trials to run.",
default=5)
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run each sim.",
default=4.0)
args = parser.parse_args()
print(AcrobotPlant)
for i in range(args.trials):
context.set_time(0.)
state.SetFromVector(UprightState().CopyToVector() +
0.05 * np.random.randn(4, ))
simulator.StepTo(args.duration)
def Simulate2dHopper(x0,
duration,
desired_lateral_velocity=0.0,
print_period=0.0):
builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant(0.0005))
scene_graph = builder.AddSystem(SceneGraph())
plant.RegisterAsSourceForSceneGraph(scene_graph)
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())
# Build the plant
parser = Parser(plant)
parser.AddModelFromFile("raibert_hopper_2d.sdf")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground"))
plant.AddForceElement(UniformGravityFieldElement())
plant.Finalize()
# Create a logger to log at 30hz
state_dim = plant.num_positions() + plant.num_velocities()
state_log = builder.AddSystem(SignalLogger(state_dim))
state_log.DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging
builder.Connect(plant.get_continuous_state_output_port(),
state_log.get_input_port(0))
# The controller
controller = builder.AddSystem(
Hopper2dController(plant,
desired_lateral_velocity=desired_lateral_velocity,
print_period=print_period))
builder.Connect(plant.get_continuous_state_output_port(),
controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
# The diagram
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.get_mutable_discrete_state_vector().SetFromVector(x0)
simulator.StepTo(duration)
return plant, controller, state_log
def animate_cartpole(policy, duration=10.):
# Animate the resulting policy.
builder = DiagramBuilder()
tree = RigidBodyTree("/opt/underactuated/src/cartpole/cartpole.urdf",
FloatingBaseType.kFixed)
plant = RigidBodyPlant(tree)
plant_system = builder.AddSystem(plant)
# TODO(russt): add wrap-around logic to barycentric mesh
# (so the policy has it, too)
class WrapTheta(VectorSystem):
def __init__(self):
VectorSystem.__init__(self, 4, 4)
def _DoCalcVectorOutput(self, context, input, state, output):
output[:] = input
twoPI = 2.*math.pi
output[1] = output[1] - twoPI * math.floor(output[1] / twoPI)
wrap = builder.AddSystem(WrapTheta())
builder.Connect(plant_system.get_output_port(0), wrap.get_input_port(0))
vi_policy = builder.AddSystem(policy)
builder.Connect(wrap.get_output_port(0), vi_policy.get_input_port(0))
builder.Connect(vi_policy.get_output_port(0), plant_system.get_input_port(0))
logger = builder.AddSystem(SignalLogger(4))
logger._DeclarePeriodicPublish(0.033333, 0.0)
builder.Connect(plant_system.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_publish_every_time_step(False)
state = simulator.get_mutable_context().get_mutable_continuous_state_vector()
state.SetFromVector([-1., math.pi-1, 1., -1.])
# Do the sim.
simulator.StepTo(duration)
# Visualize the result as a video.
vis = PlanarRigidBodyVisualizer(tree, xlim=[-12.5, 12.5], ylim=[-1, 2.5])
ani = vis.animate(logger, repeat=True)
# plt.show()
# Things added to get visualizations in an ipynb
plt.close(vis.fig)
HTML(ani.to_html5_video())
def RunPendulumSimulation(pendulum_plant,
pendulum_controller,
x0=[0.9, 0.0],
duration=10):
'''
Accepts a pendulum_plant (which should be a
DampedOscillatingPendulumPlant) and simulates it for
'duration' seconds from initial state `x0`. Returns a
logger object which can return simulated timesteps `
logger.sample_times()` (N-by-1) and simulated states
`logger.data()` (2-by-N).
'''
# Create a simple block diagram containing the plant in feedback
# with the controller.
builder = DiagramBuilder()
plant = builder.AddSystem(pendulum_plant)
controller = builder.AddSystem(pendulum_controller)
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
# Create a logger to capture the simulation of our plant
# (We tell the logger to expect a 3-variable input,
# and hook it up to the pendulum plant's 3-variable output.)
logger = builder.AddSystem(SignalLogger(3))
logger.DeclarePeriodicPublish(0.033333, 0.0)
builder.Connect(plant.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
# Create the simulator.
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_publish_every_time_step(False)
# Set the initial conditions for the simulation.
state = simulator.get_mutable_context().get_mutable_state()\
.get_mutable_continuous_state().get_mutable_vector()
state.SetFromVector(x0)
# Simulate for the requested duration.
simulator.AdvanceTo(duration)
# Return the logger, which stores the output of the
# plant across the time steps of the simulation.
return logger
def simulate(initial_state, const_input, boat, controller, duration):
diagram = build_block_diagram(boat, controller)
# set up a simulator
simulator = Simulator(diagram)
simulator_context = simulator.get_mutable_context()
# set the initial conditions
boat = diagram.GetSubsystemByName('boat')
boat_context = diagram.GetMutableSubsystemContext(boat, simulator_context)
boat_context.get_mutable_continuous_state_vector().SetFromVector(initial_state)
# set inputs to boat
simulator_context.FixInputPort(0, const_input)
# simulate from time zero to duration
simulator.AdvanceTo(duration)
return diagram.GetSubsystemByName('logger')
def simulateUntil(t, state_init, u_opt_poly, x_opt_poly, K_poly):
##################################################################################
# Setup diagram for simulation
diagram = makeDiagram(n_quadrotors,
n_balls,
use_visualizer=True,
trajectory_u=u_opt_poly,
trajectory_x=x_opt_poly,
trajectory_K=K_poly)
simulator = Simulator(diagram)
integrator = simulator.get_mutable_integrator()
integrator.set_maximum_step_size(
0.01
) # Reduce the max step size so that we can always detect collisions
context = simulator.get_mutable_context()
context.SetAccuracy(1e-4)
context.SetTime(0.)
context.SetContinuousState(state_init)
simulator.Initialize()
simulator.AdvanceTo(t)
return context.get_continuous_state_vector().CopyToVector()
def runManipulationExample(args):
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation(time_step=0.005))
station.SetupDefaultStation()
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, Tview=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.StepTo(args.duration)
def simulate(q0, qdot0, sim_time, controller):
# initialize block diagram
builder = DiagramBuilder()
# add system and controller
double_integrator = builder.AddSystem(get_double_integrator())
controller = builder.AddSystem(controller)
# wirw system and controller
builder.Connect(double_integrator.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), double_integrator.get_input_port(0))
# measure double-integrator state and input
state_logger = LogOutput(double_integrator.get_output_port(0), builder)
input_logger = LogOutput(controller.get_output_port(0), builder)
# finalize block diagram
diagram = builder.Build()
# instantiate simulator
simulator = Simulator(diagram)
simulator.set_publish_every_time_step(False) # makes sim faster
# set initial conditions
context = simulator.get_mutable_context()
context.SetContinuousState([q0, qdot0])
# run simulation
simulator.AdvanceTo(sim_time)
# unpack sim results
q_sim, qdot_sim = state_logger.data()
u_sim = input_logger.data().flatten()
t_sim = state_logger.sample_times()
return q_sim, qdot_sim, u_sim, t_sim
def main():
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
path = subprocess.check_output([
'venv/share/drake/common/resource_tool',
'-print_resource_path',
'drake/examples/multibody/cart_pole/cart_pole.sdf',
]).strip()
Parser(plant).AddModelFromFile(path)
plant.Finalize()
# Add to visualizer.
DrakeVisualizer.AddToBuilder(builder, scene_graph)
# Add controller.
controller = builder.AddSystem(BalancingLQR(plant))
builder.Connect(plant.get_state_output_port(),
controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
diagram = builder.Build()
# Set up a simulator to run this diagram.
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()
# Simulate.
duration = 8.0
for i in range(5):
initial_state = UPRIGHT_STATE + 0.1 * np.random.randn(4)
print(f"Iteration: {i}. Initial: {initial_state}")
context.SetContinuousState(initial_state)
context.SetTime(0.0)
simulator.Initialize()
simulator.AdvanceTo(duration)
controller = builder.AddSystem(QuadIlqrMpcController())
logger_x = builder.AddSystem(SignalLogger(n))
logger_u = builder.AddSystem(SignalLogger(m))
builder.Connect(controller.get_output_port(0), quad.get_input_port(0))
builder.Connect(quad.get_output_port(0), logger_x.get_input_port(0))
builder.Connect(quad.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), logger_u.get_input_port(0))
diagram = builder.Build()
# Create the simulator.
simulator = Simulator(diagram)
# if __name__ == '__main__':
# Set the initial conditions, x(0).
state = simulator.get_mutable_context().get_mutable_continuous_state_vector()
state.SetFromVector(traj_specs.x0)
input_vector = simulator.get_mutable_context(
).get_mutable_discrete_state_vector()
input_vector.SetFromVector(traj_specs.u0)
#%% Simulate
simulator.StepTo(h * 300)
#%% plot
PlotTraj(logger_x.data().T,
dt=None,
xw_list=traj_specs.xw_list,
t=logger_x.sample_times())
#%% open meshcat
vis = meshcat.Visualizer()
builder.Connect(null_controller.get_output_port(0), cassie.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(True)
# kinsol = rtree.doKinematics(q)
# com = rtree.centerOfMass(kinsol)
# print com
# nominal standing state
# state = simulator.get_mutable_context().get_mutable_continuous_state_vector()
# state.SetFromVector(q+qd)
cassie.set_state_vector(simulator.get_mutable_context(), x)
simulator.StepTo(1.0)
# simulator.Initialize();
# from bot_lcmgl import lcmgl
# import lcm as true_lcm
# viz_lcmgl = lcmgl("Visualize-Points", true_lcm.LCM())
# cache = rtree.doKinematics(x[:23], x[23:])
# thigh_l = rtree.transformPoints(cache, GetFourBarHipMountPoint(),
# rtree.FindBodyIndex("thigh_left"), 0)
# heel_l = rtree.transformPoints(cache, GetFourBarHeelMountPoint(),
# rtree.FindBodyIndex("heel_spring_left"), 0)
# thigh_r = rtree.transformPoints(cache, -GetFourBarHipMountPoint(),
# rtree.FindBodyIndex("thigh_right"), 0)
# Create a simulator for it.
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:
import matplotlib.pyplot as plt
from pydrake.all import (DiagramBuilder, SignalLogger, Simulator)
from discrete_time_system import *
# Create a simple block diagram containing our system.
builder = DiagramBuilder()
system = builder.AddSystem(SimpleDiscreteTimeSystem())
logger = builder.AddSystem(SignalLogger(1))
builder.Connect(system.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
# Create the simulator.
simulator = Simulator(diagram)
# Set the initial conditions, x(0).
state = simulator.get_mutable_context().get_mutable_discrete_state_vector()
state.SetFromVector([0.9])
# Simulate for 10 seconds.
simulator.StepTo(10)
# Plot the results.
plt.stem(logger.sample_times(), logger.data().transpose())
plt.xlabel('t')
plt.ylabel('x(t)')
plt.show()
tree.compile()
builder = DiagramBuilder()
compass_gait = builder.AddSystem(CompassGait())
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10.0)
args = parser.parse_args()
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
xlim=[-1., 5.],
ylim=[-1., 2.],
figsize_multiplier=2))
builder.Connect(compass_gait.get_output_port(1), visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()
diagram.Publish(context) # draw once to get the window open
context.set_accuracy(1e-4)
context.SetContinuousState([0., 0., 0.4, -2.])
simulator.StepTo(args.duration)
def lyapunov_simulation(
x0,
xf,
uref=0,
create_dynamics=create_symbolic_dynamics,
to_point=False,
a=0.1,
u_max=0.5,
eps=2.):
'''
Simulates and plots our Lyapunov Controller in different scenarios.
Parameters:
x0: initial state (x, y, yaw)
x1: final state (x, y, yaw)
uref: reference input
create_dynamics: symbolic dynamics creation function
a, eps: lyapunov controller parameters
u_max: control limit (u_max = 1/r)
'''
params = np.array([a, u_max, eps])
uavPlant = create_dynamics()
# construction site for our closed-loop system
builder = DiagramBuilder()
# add the robot to the diagram
# the method .AddSystem() simply returns a pointer to the system
# we passed as input, so it's ok to give it the same name
uav = builder.AddSystem(uavPlant)
# add the controller
if not to_point:
controller = builder.AddSystem(LyapunovController(lyapunov_controller, params, xf))
else:
controller = builder.AddSystem(LyapunovController(lyapunov_controller_to_point, uref, xf))
# wire the controller with the system
builder.Connect(uavPlant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), uavPlant.get_input_port(0))
# add a logger to the diagram
# this will store the state trajectory
logger = LogOutput(uavPlant.get_output_port(0), builder)
control_logger = LogOutput(controller.get_output_port(0), builder)
# complete the construction of the diagram
diagram = builder.Build()
# reset logger to delete old trajectories
# this is needed if you want to simulate the system multiple times
logger.reset()
# set up a simulation environment
simulator = Simulator(diagram)
# simulator.set_target_realtime_rate(1)
# set the initial cartesian state to a random initial position
# try initial_state = np.random.randn(3) for a random initial state
context = simulator.get_mutable_context()
context.SetContinuousState(x0)
# simulate from zero to sim_time
# the trajectory will be stored in the logger
if not to_point:
sim_time = 400
else:
sim_time = 0.1
simulator.AdvanceTo(sim_time)
# print('done!')
# draw_simulation(logger.data())
return (simulator, logger.data(), control_logger.data())
controller = builder.AddSystem(Diff_Drive_Controller(plant))
builder.Connect(plant.get_continuous_state_output_port(),
controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
#Data Logger
logger = LogOutput(plant.get_continuous_state_output_port(), builder)
#Run Simulation
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.)
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
print plant_context.get_mutable_discrete_state_vector()
plant_context.get_mutable_discrete_state_vector().SetFromVector(x0)
simulator.Initialize()
simulator.StepTo(duration)
#print('sample_times',logger.sample_times()) #nx1 array
print('Final State: ', logger.data()[:, -1]) #nxm array
x_log = logger.data()[4, :]
data = logger.data()
theta_log = np.asarray([
math.asin(2 * (data[0, i] * data[2, i] - data[1, i] * data[3, i]))
for i in range(data.shape[1])
])
#theta = math.asin(2*(x[0]*x[2] - x[1]*x[3]))
def getPredictedMotion(B, v_command, time):
#object_positions = object_positions + 0.1
#manipulator_positions = manipulator_positions + 0.1
#object_velocities = object_velocities + 0.1
#manipulator_velocities = manipulator_velocities + 0.1
#object_positions = object_positions[:, range(0,object_positions.shape[1],2)]
#manipulator_positions = manipulator_positions[:, range(0,manipulator_positions.shape[1],2)]
#object_velocities = object_velocities[:, range(0,object_velocities.shape[1],2)]
#manipulator_velocities = manipulator_velocities[:, range(0,manipulator_velocities.shape[1],2)]
#times = times[range(0,times.size,2)]
#import pdb; pdb.set_trace()
step = 0.01
A = 10 * np.eye(3)
tree = RigidBodyTree(
FindResource(
os.path.dirname(os.path.realpath(__file__)) +
"/block_pusher2.urdf"), FloatingBaseType.kFixed)
# Set up a block diagram with the robot (dynamics), the controller, and a
# visualization block.
builder = DiagramBuilder()
#robot = builder.AddSystem(RigidBodyPlant(tree))
robot = builder.AddSystem(
QuasiStaticRigidBodyPlant(tree, step, A, np.linalg.inv(B)))
controller = builder.AddSystem(QController(tree, v_command, step))
#builder.Connect(robot.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), robot.get_input_port(0))
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
Tview,
xlim=[-2.8, 4.8],
ylim=[-2.8, 10]))
builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0))
logger = builder.AddSystem(
SignalLogger(tree.get_num_positions() + tree.get_num_velocities()))
builder.Connect(robot.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(True)
# Set the initial conditions
context = simulator.get_mutable_context()
state = context.get_mutable_discrete_state_vector()
start1 = 3 * np.pi / 16
start2 = 15 * np.pi / 16
#np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps, np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps
state.SetFromVector(
(start1, start2, -start1, -start2, np.pi + start1, start2,
np.pi - start1, -start2, 1, 1, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
0., 0.)) # (theta1, theta2, theta1dot, theta2dot)
# Simulate for 10 seconds
simulator.StepTo(time)
#import pdb; pdb.set_trace()
return (logger.data()[8:11, :], logger.data()[:8, :],
logger.data()[19:22, :], logger.data()[11:19, :],
logger.sample_times())
def SimulateHand(duration=10.,
num_fingers=3,
mu=0.5,
manipuland_sdf="models/manipuland_box.sdf",
initial_manipuland_pose=np.array([1.5, 0., 0.]),
n_grasp_search_iters=100,
manipuland_trajectory_callback=None,
control_period=0.0333,
print_period=1.0):
''' Given a great many passthrough arguments
(see docs for HandController and
usage example in set_5_mpc.ipynb), constructs
a simulation of a num_fingers-fingered hand
and simulates it for duration seconds from
a specified initial manipuland pose.
Returns:
(hand, plant, controller, state_log, contact_log)
hand: The RigidBodyTree of the complete hand.
plant: The RigidBodyPlant that owns the hand RBT
controller: The HandController object
state_log: A SignalLogger that has logged the output
of the state output port of plant.
contact_log: A PlanarHandContactLogger object that
has logged the output of the contact results output
port of the plant. '''
builder = DiagramBuilder()
tree = BuildHand(num_fingers, manipuland_sdf)
num_finger_links = 3 # from sdf
num_hand_q = num_finger_links * num_fingers
# Generate the nominal posture for the hand
# First link wide open, next links at right angles
x_nom = np.zeros(2 * num_finger_links * num_fingers + 6)
for i in range(num_fingers):
if i < num_fingers / 2:
x_nom[(num_finger_links*i):(num_finger_links*(i+1))] = \
np.array([2, 1.57, 1.57])
else:
x_nom[(num_finger_links*i):(num_finger_links*(i+1))] = \
-np.array([2, 1.57, 1.57])
# Drop in the initial manipuland location
x_nom[num_hand_q:(num_hand_q + 3)] = initial_manipuland_pose
# A RigidBodyPlant wraps a RigidBodyTree to allow
# forward dynamical simulation. It handles e.g. collision
# modeling.
plant = builder.AddSystem(RigidBodyPlant(tree))
# Alter the ground material used in simulation to make
# it dissipate more energy (to make the object less bouncy)
# and stickier (to make it easier to hold) and softer
# (again to make it less bouncy)
allmaterials = CompliantMaterial()
allmaterials.set_youngs_modulus(1E6) # default 1E9
allmaterials.set_dissipation(1.0) # default 0.32
allmaterials.set_friction(0.9) # default 0.9.
plant.set_default_compliant_material(allmaterials)
# Spawn a controller and hook it up
controller = builder.AddSystem(
HandController(
tree,
x_nom=x_nom,
num_fingers=num_fingers,
mu=mu,
n_grasp_search_iters=n_grasp_search_iters,
manipuland_trajectory_callback=manipuland_trajectory_callback,
control_period=control_period,
print_period=print_period))
nq = controller.nq
qinit, info = controller.ComputeTargetPosture(x_nom,
x_nom[(nq - 3):nq],
backoff_distance=0.0)
if info != 1:
print "Warning: initial condition IK solve returned info ", info
xinit = np.zeros(x_nom.shape)
xinit[0:(nq - 3)] = qinit[0:-3]
xinit[(nq - 3):nq] = x_nom[(nq - 3):nq]
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
for i in range(num_fingers):
builder.Connect(controller.get_output_port(i), plant.get_input_port(i))
# Create a state logger to log at 30hz
state_log = builder.AddSystem(SignalLogger(plant.get_num_states()))
state_log.DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging
builder.Connect(plant.get_output_port(0), state_log.get_input_port(0))
# And a contact result logger, same rate
contact_log = builder.AddSystem(PlanarHandContactLogger(controller, plant))
contact_log.DeclarePeriodicPublish(0.0333, 0.0)
builder.Connect(plant.contact_results_output_port(),
contact_log.get_input_port(0))
# Create a simulator
diagram = builder.Build()
simulator = Simulator(diagram)
# Don't limit realtime rate for this sim, since we
# produce a video to render it after simulating the whole thing.
# simulator.set_target_realtime_rate(100.0)
simulator.set_publish_every_time_step(False)
# Force the simulator to use a fixed-step integrator,
# which is much faster for this stiff system. (Due to the
# spring-model of collision, the default variable-timestep
# integrator will take very short steps. I've chosen the step
# size here to be fast while still being stable in most situations.)
integrator = simulator.get_mutable_integrator()
integrator.set_fixed_step_mode(True)
integrator.set_maximum_step_size(0.005)
# Set the initial state
state = simulator.get_mutable_context().\
get_mutable_continuous_state_vector()
state.SetFromVector(xinit)
# Simulate!
simulator.StepTo(duration)
return tree, plant, controller, state_log, contact_log
builder = DiagramBuilder()
acrobot = builder.AddSystem(RigidBodyPlant(tree))
parser = argparse.ArgumentParser()
parser.add_argument("-T", "--duration",
type=float,
help="Duration to run sim.",
default=10000.0)
args = parser.parse_args()
visualizer = builder.AddSystem(PlanarRigidBodyVisualizer(tree,
xlim=[-4., 4.],
ylim=[-4., 4.]))
builder.Connect(acrobot.get_output_port(0), visualizer.get_input_port(0))
ax = visualizer.fig.add_axes([.2, .95, .6, .025])
torque_system = builder.AddSystem(SliderSystem(ax, 'Torque', -5., 5.))
builder.Connect(torque_system.get_output_port(0),
acrobot.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
context = simulator.get_mutable_context()
context.SetContinuousState([1., 0., 0., 0.])
simulator.StepTo(args.duration)
def Simulate2dHopper(x0,
duration,
desired_lateral_velocity=0.0,
print_period=0.0):
builder = DiagramBuilder()
# Load in the hopper from a description file.
# It's spawned with a fixed floating base because
# the robot description file includes the world as its
# root link -- it does this so that I can create a robot
# system with planar dynamics manually. (Drake doesn't have
# a planar floating base type accessible right now that I
# know about -- it only has 6DOF floating base types.)
tree = RigidBodyTree()
AddModelInstancesFromSdfString(
open("raibert_hopper_2d.sdf", 'r').read(), FloatingBaseType.kFixed,
None, tree)
# A RigidBodyPlant wraps a RigidBodyTree to allow
# forward dynamical simulation. It handles e.g. collision
# modeling.
plant = builder.AddSystem(RigidBodyPlant(tree))
# Alter the ground material used in simulation to make
# it dissipate more energy (to make the hopping more critical)
# and stickier (to make the hopper less likely to slip).
allmaterials = CompliantMaterial()
allmaterials.set_youngs_modulus(1E8) # default 1E9
allmaterials.set_dissipation(1.0) # default 0.32
allmaterials.set_friction(1.0) # default 0.9.
plant.set_default_compliant_material(allmaterials)
# Spawn a controller and hook it up
controller = builder.AddSystem(
Hopper2dController(tree,
desired_lateral_velocity=desired_lateral_velocity,
print_period=print_period))
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
# Create a logger to log at 30hz
state_log = builder.AddSystem(SignalLogger(plant.get_num_states()))
state_log._DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging
builder.Connect(plant.get_output_port(0), state_log.get_input_port(0))
# Create a simulator
diagram = builder.Build()
simulator = Simulator(diagram)
# Don't limit realtime rate for this sim, since we
# produce a video to render it after simulating the whole thing.
#simulator.set_target_realtime_rate(100.0)
simulator.set_publish_every_time_step(False)
# Force the simulator to use a fixed-step integrator,
# which is much faster for this stiff system. (Due to the
# spring-model of collision, the default variable-timestep
# integrator will take very short steps. I've chosen the step
# size here to be fast while still being stable in most situations.)
integrator = simulator.get_mutable_integrator()
integrator.set_fixed_step_mode(True)
integrator.set_maximum_step_size(0.0005)
# Set the initial state
state = simulator.get_mutable_context(
).get_mutable_continuous_state_vector()
state.SetFromVector(x0)
# Simulate!
simulator.StepTo(duration)
return tree, controller, state_log
builder = DiagramBuilder()
compass_gait = builder.AddSystem(CompassGait())
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10.0)
args = parser.parse_args()
logger = builder.AddSystem(SignalLogger(14))
builder.Connect(compass_gait.get_output_port(1), logger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_publish_every_time_step(True)
simulator.get_mutable_context().SetAccuracy(1e-4)
state = simulator.get_mutable_context().get_mutable_continuous_state_vector()
state.SetFromVector([0., 0., 0.4, -2.])
simulator.StepTo(args.duration)
plt.figure()
plt.plot(logger.data()[4, :], logger.data()[11, :])
plt.xlabel('left leg angle')
plt.ylabel('left leg angular velocity')
plt.show()
import matplotlib.pyplot as plt
from pydrake.all import DiagramBuilder, SignalLogger, Simulator
from discrete_time_system import *
# Create a simple block diagram containing our system.
builder = DiagramBuilder()
system = builder.AddSystem(SimpleDiscreteTimeSystem())
logger = builder.AddSystem(SignalLogger(1))
builder.Connect(system.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
# Create the simulator.
simulator = Simulator(diagram)
# Set the initial conditions, x(0).
state = simulator.get_mutable_context().get_mutable_discrete_state_vector()
state.SetFromVector([0.9])
# Simulate for 10 seconds.
simulator.AdvanceTo(10)
# Plot the results.
plt.stem(logger.sample_times(), logger.data().transpose())
plt.xlabel("t")
plt.ylabel("x(t)")
plt.show()
# And also log
signalLogRate = 60
signalLogger = builder.AddSystem(SignalLogger(nx))
signalLogger.DeclarePeriodicPublish(1. / signalLogRate, 0.0)
builder.Connect(rbplant_sys.get_output_port(0),
signalLogger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
# TODO(russt): Clean up state vector access below.
state = simulator.get_mutable_context().get_mutable_state()\
.get_mutable_continuous_state().get_mutable_vector()
if set_initial_state:
initial_state = np.zeros((nx, 1))
initial_state[0] = 1.0
state.SetFromVector(initial_state)
simulator.StepTo(args.duration)
print(state.CopyToVector())
# Generate an animation of whatever happened
ani = visualizer.animate(signalLogger, repeat=True)
if (args.animate):
print("Animating the simulation on repeat -- "
# Done! Compile it all together and visualize it.
diagram = builder.Build()
kuka_utils.render_system_with_graphviz(diagram, "view.gv")
# Create a simulator for it.
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)
# 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(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
def simulate_and_log_policy_system(policy_system, expmt, ic=None):
global tree
global logger
expmt_settings = {
"pendulum": {
'constructor_or_path': PendulumPlant,
'state_dim': 2,
'twoPI_boundary_condition_state_idxs': (0,),
'initial_state': [0.1, 0.0],
},
"acrobot": {
'constructor_or_path': "/opt/underactuated/src/acrobot/acrobot.urdf",
'state_dim': 4,
'twoPI_boundary_condition_state_idxs': (0, 1),
'initial_state': [0.5, 0.5, 0.0, 0.0],
},
"cartpole": {
'constructor_or_path': "/opt/underactuated/src/cartpole/cartpole.urdf",
'state_dim': 4,
'twoPI_boundary_condition_state_idxs': (1,),
'initial_state': [0.5, 0.5, 0.0, 0.0],
},
}
assert expmt in expmt_settings.keys()
# Animate the resulting policy.
settings = expmt_settings[expmt]
builder = DiagramBuilder()
constructor_or_path = settings['constructor_or_path']
if not isinstance(constructor_or_path, str):
plant = constructor_or_path()
else:
tree = RigidBodyTree(constructor_or_path, FloatingBaseType.kFixed)
plant = RigidBodyPlant(tree)
plant_system = builder.AddSystem(plant)
# TODO(russt): add wrap-around logic to barycentric mesh
# (so the policy has it, too)
class WrapTheta(VectorSystem):
def __init__(self):
VectorSystem.__init__(self, settings['state_dim'], settings['state_dim'])
def _DoCalcVectorOutput(self, context, input, state, output):
output[:] = input
twoPI = 2.*math.pi
for idx in settings['twoPI_boundary_condition_state_idxs']:
# output[idx] += math.pi
output[idx] = output[idx]# - twoPI * math.floor(output[idx] / twoPI)
# output[idx] -= math.pi
wrap = builder.AddSystem(WrapTheta())
builder.Connect(plant_system.get_output_port(0), wrap.get_input_port(0))
vi_policy = builder.AddSystem(policy_system)
builder.Connect(wrap.get_output_port(0), vi_policy.get_input_port(0))
builder.Connect(vi_policy.get_output_port(0), plant_system.get_input_port(0))
x_logger = builder.AddSystem(SignalLogger(settings['state_dim']))
x_logger._DeclarePeriodicPublish(0.033333, 0.0)
builder.Connect(plant_system.get_output_port(0), x_logger.get_input_port(0))
u_logger = builder.AddSystem(SignalLogger(1))
u_logger._DeclarePeriodicPublish(0.033333, 0.0)
builder.Connect(vi_policy.get_output_port(0), u_logger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_publish_every_time_step(False)
state = simulator.get_mutable_context().get_mutable_continuous_state_vector()
if ic is None:
ic = settings['initial_state']
state.SetFromVector(ic)
return simulator, tree, x_logger, u_logger
# And also log
signalLogRate = 60
signalLogger = builder.AddSystem(SignalLogger(nx))
signalLogger.DeclarePeriodicPublish(1. / signalLogRate, 0.0)
builder.Connect(rbplant_sys.get_output_port(0),
signalLogger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
# TODO(russt): Clean up state vector access below.
state = simulator.get_mutable_context().get_mutable_state()\
.get_mutable_continuous_state().get_mutable_vector()
if set_initial_state:
initial_state = np.zeros((nx, 1))
initial_state[0] = 1.0
state.SetFromVector(initial_state)
simulator.StepTo(args.duration)
if (args.animate):
# Generate an animation of whatever happened
ani = visualizer.animate(signalLogger)
if meshcat_server_p is not None:
meshcat_server_p.kill()
meshcat_server_p.wait()