def __init__(self, caption="Simulation"):
"""Initialisiert das Fenster."""
# Fenster öffnen
try:
config = Config(sample_buffers=1, samples=1, double_buffer=True)
super(PendulumWindow, self).__init__(config=config,
resizable=True,
caption=caption)
except:
super(PendulumWindow, self).__init__(resizable=True,
caption=caption)
# Schrift laden
self.font_sans = font.load(None, 16)
# FPS
self.fps_display = clock.ClockDisplay()
# Pendel aufhängen
self.single_pendulum = SinglePendulum(length=9.0, phi=pi / 5)
self.double_pendulum = DoublePendulum()
# Einstellungsfenster
self.settings_window = SettingsWindow(self,
pendulum=self.double_pendulum,
position=(15, 15))
self.settings_window.reset()
def test_radius2():
a = DoublePendulum()
a.solve((0, 0.1, 3, 0.5), 10, 101)
r2 = a.x2**2 + a.y2**2
assert np.isclose(1**2, np.all(r2))
def test_stationary():
"""Test if stationary initial state result in stationary behavior."""
var = DoublePendulum(2, 2, 2, 2)
y0 = [0, 0, 0, 0]
y = var(0, y0)
nt.assert_equal(y, (0.0, 0.0, 0.0, 0.0))
import torch
import numpy as np
from feedback_linearization import FeedbackLinearization
from double_pendulum import DoublePendulum
dyn = DoublePendulum(1.0, 1.0, 1.0, 1.0, time_step=0.01)
fb = FeedbackLinearization(dyn, 3, 5, torch.nn.ReLU(), 0.1)
u = np.array([1.0, 1.0])
x = np.array([1.0, 1.0, 1.0, 1.0])
v = np.array([1.0, 1.0])
#M1 = fb._M1(x)
#print("m1: ", M1)
#M2 = fb._M2(x)
#print("m2: ", M2)
#w1 = fb._w1(x)
#print("w1: ", w1)
#w2 = fb._w2(x)
#print("w2: ", w2)
#mu = fb.feedback(x, v)
#nu = fb.noisy_feedback(x, v)
#print("mean u = ", mu)
#print("noisy u = ", nu)
class PendulumWindow(window.Window):
def __init__(self, caption="Simulation"):
"""Initialisiert das Fenster."""
# Fenster öffnen
try:
config = Config(sample_buffers=1, samples=1, double_buffer=True)
super(PendulumWindow, self).__init__(config=config,
resizable=True,
caption=caption)
except:
super(PendulumWindow, self).__init__(resizable=True,
caption=caption)
# Schrift laden
self.font_sans = font.load(None, 16)
# FPS
self.fps_display = clock.ClockDisplay()
# Pendel aufhängen
self.single_pendulum = SinglePendulum(length=9.0, phi=pi / 5)
self.double_pendulum = DoublePendulum()
# Einstellungsfenster
self.settings_window = SettingsWindow(self,
pendulum=self.double_pendulum,
position=(15, 15))
self.settings_window.reset()
def on_resize(self, width, height):
"""Auf Veränderung der Fenstergröße reagieren."""
glViewport(0, 0, width, height)
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
glOrtho(0, width, 0, height, -1, 1)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
def run(self):
"""Mainloop. Arbeitet Ereignisse ab."""
while not self.has_exit:
self.dispatch_events()
self.update()
self.draw()
self.settings_window.draw()
self.flip()
def update(self):
"""Aktualisiert den Versuchsaufbau."""
dt = clock.tick()
self.single_pendulum.update(dt)
# nur aktualisieren, wenn Simulation läuft
if not self.settings_window.stopped:
self.double_pendulum.update(dt)
def draw(self):
"""Zeichnet das Fenster."""
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glLoadIdentity()
glPushMatrix()
# oben und unten vertauschen (pyglet geht von linker unterer
# Ecke als Nullpunkt aus)
glTranslatef(0, self.height, 0)
glScalef(1, -1, 1)
# Koordinatensystem skalieren
scale = 0.2 * self.height / 2 - 30
# Einfachpendel links
glPushMatrix()
glTranslatef(0.25 * self.width, self.height / 2, 0)
glScalef(scale, scale, scale)
self.single_pendulum.draw()
glPopMatrix()
# Doppelpendel rechts
glPushMatrix()
glTranslatef(0.75 * self.width, self.height / 2, 0)
glScalef(scale, scale, scale)
self.double_pendulum.draw()
glPopMatrix()
glPopMatrix()
# Text darstellen
t = font.Text(self.font_sans,
text="E = %.2f J" % self.double_pendulum.energy,
x=self.width - 10, y=10, halign=font.Text.RIGHT)
t.draw()
self.fps_display.draw()
class Simulation:
def __init__(self, res, fps, time, dt, scale, speed, path_depth, ag, mass,
length, theta, d_theta):
self.running = False # Simulation not started yet at this point
# Setting simulation variables
self.width, self.height = res
self.fps = fps
self.time, self.dt = time, dt
self.points_count = self.time / self.dt
self.scale = scale
self.speed = speed
self.path_depth = path_depth
self.path = []
# Setting variables for solve method.
self.time_range = np.arange(0, self.time + self.dt,
self.dt) # Range of time points
self.ag = ag # Gravitational acceleration
# Creating display and pendulum objects; Solving equations
self.pendulum = DoublePendulum(mass, length, theta, d_theta)
self.pendulum.solve(self.time_range, self.ag)
self.display = Display(res)
def run(self):
""" Simulation loop. """
# Start the simulation
self.running = True
i = 0 # Iterable for simulation frames
di = int((1 / self.fps / self.dt) *
self.speed) # Step size based on time density and frame rate
# Loop
while self.running:
# Getting start point and all of the pendulum positions
x_start, y_start = (int(self.width / 2), int(self.height / 3))
x_1_scaled, y_1_scaled, x_2_scaled, y_2_scaled = self.pendulum.get_scaled_positions(
i, self.scale)
start_pos = (x_start, y_start)
x_1_pos = (x_start + x_1_scaled, y_start - y_1_scaled)
x_2_pos = (x_start + x_2_scaled, y_start - y_2_scaled)
# Getting raw positions and velocities to show live info
x_1_raw, y_1_raw, x_2_raw, y_2_raw = self.pendulum.get_positions(i)
angle_1, angle_2 = self.pendulum.get_angles(i)
velocity_1, velocity_2 = self.pendulum.get_angular_velocities(i)
# Setting strings ready to be drawn on display
info_str = 'Mass {} X: {:05.2f}m, Y: {:05.2f}m, Angle: {:05.1f}deg, Velocity: {:06.1f}deg/s'
m_1_info_str = info_str.format(1, x_1_raw, y_1_raw, angle_1 % 360,
velocity_1)
m_2_info_str = info_str.format(2, x_2_raw, y_2_raw, angle_2 % 360,
velocity_2)
# Drawing rods
self.display.draw_rod(start_pos, x_1_pos)
self.display.draw_rod(x_1_pos, x_2_pos)
self.display.draw_path(self.path, self.path_depth) # Drawing path
# Drawing bodies
self.display.draw_body(x_1_pos)
self.display.draw_body(x_2_pos)
# Drawing live info
self.display.draw_info(m_1_info_str, (10, 10))
self.display.draw_info(m_2_info_str, (10, 38))
# Handling events and transition into next iteration
self.display.next_frame(int(1000 / self.fps))
self.display.handle_events()
# Inserting current second pendulum position into path
self.path.append(x_2_pos)
# Iterating and stopping the simulation at the end of solved time range
i += di
if i > self.points_count:
self.running = False
xlim=(
-longest_double_pendulum.max_length,
longest_double_pendulum.max_length
),
ylim=(
-longest_double_pendulum.max_length,
longest_double_pendulum.max_length
),
)
ax.set_aspect('equal')
ax.grid()
return ax
if __name__ == "__main__":
# Create the pendula
fig = plt.figure()
pendula = DoublePendulum.create_multiple_double_pendula(num_pendula=10)
axes = create_axes(fig=fig, pendula=pendula)
pendula_axes = list(zip(pendula, axes))
ani = animation.FuncAnimation(
fig,
animate,
np.arange(1, len(pendula[0].y)),
interval=25,
blit=True,
)
plt.show()
nominal=1 truedyn=1 T=500 to_render=1 check_energy=0 speed=0.001 path=0 # Create a double pendulum. And a bad one mass1 = 1.0 mass2 = 1.0 length1 = 1.0 length2 = 1.0 time_step = 0.01 friction_coeff=0.5 dyn = DoublePendulum(mass1, mass2, length1, length2, time_step, friction_coeff) bad_dyn = DoublePendulum(0.33*mass1, 0.33*mass2, 0.33*length1, 0.33*length2, time_step,friction_coeff) # LQR Parameters and dynamics q=200.0 r=1.0 A,B=get_Linear_System() Q=q*np.diag([1,0,1.0,0]) R=r*np.eye(2) #Get Linear Feedback policies K=solve_lqr(A,B,Q,R) #Get random initial state
def test_theta2(): pen = DoublePendulum() pen.theta2
def test_t(): pen = DoublePendulum() pen.t
class DoublePendulumEnv(gym.Env):
def __init__(self, stepsPerRollout, rewardScaling, dynamicsScaling,
preprocessState, uscaling, largerQ):
#calling init method of parent class
super(DoublePendulumEnv, self).__init__()
#setting local parameters
self._preprocess_state = preprocessState
self._num_steps_per_rollout = stepsPerRollout
self._reward_scaling = rewardScaling
self._norm = 2
self._time_step = 0.01
self._uscaling = uscaling
self._largerQ = largerQ
#setting action space dimensions so agent knows output size
self.action_space = spaces.Box(low=-50,
high=50,
shape=(6, ),
dtype=np.float32)
#setting observation space dimensions so agent knows input size
if (self._preprocess_state):
self.observation_space = spaces.Box(low=-100,
high=100,
shape=(6, ),
dtype=np.float32)
else:
self.observation_space = spaces.Box(low=-100,
high=100,
shape=(4, ),
dtype=np.float32)
# TODO: these should match what we get from system identification, once
# we have reliable numbers there.
#setting parameters of quadrotor and creating dynamics object
mass1 = 1.0
mass2 = 1.0
length1 = 1.0
length2 = 1.0
self._dynamics = DoublePendulum(mass1, mass2, length1, length2,
self._time_step)
#creating bad dynamics object
scaling = 0.000001 + dynamicsScaling
self._bad_dynamics = DoublePendulum(scaling * mass1, scaling * mass2,
scaling * length1,
scaling * length2, self._time_step)
#setting other local variables
self.A, self.B, C = self._dynamics.linearized_system()
self._count = 0
self._xdim = self._dynamics.xdim
self._udim = self._dynamics.udim
self._M1, self._f1 = self._bad_dynamics.feedback_linearize()
self._iter_count = 0
def step(self, u):
#compute v based on basic control law
diff = self._dynamics.linear_system_state_delta(
self._reference[self._count], self._current_y)
v = -self._K @ (diff)
#output of neural network
m2, f2 = np.split(self._uscaling * u, [4])
M = self._bad_dynamics._M_q(self._state) + np.reshape(
m2, (self._udim, self._udim))
f = self._bad_dynamics._f_q(self._state) + np.reshape(
f2, (self._udim, 1))
z = np.dot(M, v) + f
self._state = self._dynamics.integrate(self._state, z, self._time_step)
self._current_y = self._dynamics.linearized_system_state(self._state)
reward = self.computeReward(self._y_desired[self._count],
self._current_y)
#Increasing count
self._count += 1
#computing observations, rewards, done, info???
done = False
if (self._count >= self._num_steps_per_rollout):
done = True
#formatting observation
list = []
for x in self._state:
list.append(x[0])
observation = list
#preprocessing observation
if (self._preprocess_state):
observation = self.preprocess_state(observation)
#returning stuff
return np.array(observation), reward, done, {}
def reset(self):
#(0) Sample state using state smapler method
self._state = self.initial_state_sampler()
# (1) Generate a time series for v and corresponding y.
self._reference, self._K = self._generate_reference(self._state)
self._y_desired = self._generate_ys(self._state, self._reference,
self._K)
self._current_y = self._dynamics.linearized_system_state(self._state)
#reset internal count
self._count = 0
#formatting observation
list = []
for x in self._state:
list.append(x[0])
observation = list
#preprocessing state
if (self._preprocess_state):
observation = self.preprocess_state(observation)
return np.array(observation)
def seed(self, s):
np.random.seed(np.random.randomint())
def render(self):
# TODO!
pass
def initial_state_sampler(self):
lower = np.array([[-np.pi], [-0.5], [-np.pi], [-0.5]])
upper = -lower
return np.random.uniform(lower, upper)
def _generate_reference(self, x0):
"""
Use sinusoid with random frequency, amplitude, and bias:
``` vi(k) = a * sin(2 * pi * f * k) + b ```
"""
MAX_CONTINUOUS_TIME_FREQ = 0.1
MAX_DISCRETE_TIME_FREQ = MAX_CONTINUOUS_TIME_FREQ * self._dynamics._time_step
linsys_xdim = self.A.shape[0]
linsys_udim = self.B.shape[1]
#random scaling factor for Q based on how many iterations have been done
if (self._largerQ):
Q = 50 * (np.random.uniform() + 0.1) * np.eye(linsys_xdim)
else:
Q = 10 * np.eye(linsys_xdim)
#fixed Q scaling
# Q = 1.0 * np.diag([1.0, 0.0, 0.0, 0.0,
# 1.0, 0.0, 0.0, 0.0,
# 1.0, 0.0, 0.0, 0.0,
# 1.0, 0.0])
#fixed R scaling
R = 1.0 * np.eye(linsys_udim)
# Initial y.
y0 = self._dynamics.linearized_system_state(x0)
y = np.empty((linsys_xdim, self._num_steps_per_rollout))
for ii in range(linsys_xdim):
y[ii, :] = np.linspace(
0, self._num_steps_per_rollout * self._dynamics._time_step,
self._num_steps_per_rollout)
y[ii, :] = y0[ii, 0] + 1.0 * np.random.uniform() * (1.0 - np.cos(
2.0 * np.pi * MAX_DISCRETE_TIME_FREQ * \
np.random.uniform() * y[ii, :])) #+ 0.1 * np.random.normal()
# Ensure that y ref starts at y0.
assert (np.allclose(y[:, 0].flatten(), y0.flatten(), 1e-5))
P = solve_continuous_are(self.A, self.B, Q, R)
K = np.linalg.inv(R) @ self.B.T @ P
return (np.split(y,
indices_or_sections=self._num_steps_per_rollout,
axis=1), K)
def _generate_ys(self, x0, refs, K):
"""
Compute desired output sequence given initial state and input sequence.
This is computed by applying the true dynamics' feedback linearization.
"""
x = x0.copy()
y = self._dynamics.linearized_system_state(x)
ys = []
for r in refs:
diff = self._dynamics.linear_system_state_delta(r, y)
v = -K @ diff
u = self._dynamics.feedback(x, v)
x = self._dynamics.integrate(x, u)
y = self._dynamics.linearized_system_state(x)
ys.append(y.copy())
return ys
def computeReward(self, y_desired, y):
return -self._reward_scaling * self._dynamics.observation_distance(
y_desired, y, self._norm)
def close(self):
pass
def preprocess_state(self, x):
return self._dynamics.preprocess_state(x)
import torch
import numpy as np
from double_pendulum import DoublePendulum
dyn = DoublePendulum(1.0, 1.0, 1.0, 1.0, time_step=0.01)
u = np.array([[0.0], [0.0]])
x = np.array([[0.1], [0.0], [0.1], [0.0]])
current_x = x.copy()
for ii in range(10):
current_x = dyn.integrate(current_x, u)
print(current_x.T)