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)
if (v >= 0):
thetadot[i] = math.sqrt(v)
else:
thetadot[i] = float("nan")
ax.plot(theta, thetadot, color=[.6, .6, .6])
ax.plot(theta, -thetadot, color=[.6, .6, .6])
return ax
if __name__ == "__main__":
builder = DiagramBuilder()
pendulum = builder.AddSystem(PendulumPlant())
ax = PhasePlot(pendulum)
saturation = builder.AddSystem(Saturation(min_value=[-3], max_value=[3]))
builder.Connect(saturation.get_output_port(0), pendulum.get_input_port(0))
controller = builder.AddSystem(SwingUpAndBalanceController())
builder.Connect(pendulum.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), saturation.get_input_port(0))
# visualizer = builder.AddSystem(PendulumVisualizer())
# builder.Connect(pendulum.get_output_port(0), visualizer.get_input_port(0))
logger = builder.AddSystem(SignalLogger(2))
builder.Connect(pendulum.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
E_error = Ec - Ed
u_bar = E_error * theta1_dot
u = -self.k1 * theta2 - self.k2 * theta2_dot + self.k3 * u_bar
tau = m22_bar * u + bias_bar
control_vec.SetFromVector(np.array([tau]))
if __name__ == "__main__":
builder = DiagramBuilder()
acrobot = builder.AddSystem(AcrobotPlant())
saturation = builder.AddSystem(Saturation(min_value=[-20], max_value=[20]))
builder.Connect(saturation.get_output_port(0), acrobot.get_input_port(0))
wrapangles = WrapToSystem(4)
wrapangles.set_interval(0, 0, 2. * np.pi)
wrapangles.set_interval(1, -np.pi, np.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))
controller = builder.AddSystem(EnergyShapingControl(k1=50, k2=5, k3=5.0))
builder.Connect(wrapto.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
saturation.get_input_port(0))
u_0 = np.array([
self.m * self.g / 4.0,
] * 4)
output = u_0 + u_bar
control_vec.SetFromVector(output)
if __name__ == "__main__":
builder = DiagramBuilder()
quadroter = builder.AddSystem(QuadrotorPlant())
limit = 10
saturation = builder.AddSystem(
Saturation(min_value=[
-limit,
] * 4, max_value=[
limit,
] * 4))
builder.Connect(saturation.get_output_port(0), quadroter.get_input_port(0))
controller = builder.AddSystem(QuadLQRController())
builder.Connect(quadroter.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
saturation.get_input_port(0))
logger = builder.AddSystem(SignalLogger(12))
builder.Connect(quadroter.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()