From 9f6c91efe86f7ac10582ac1dd477187bd9561f80 Mon Sep 17 00:00:00 2001
From: Tue Herlau <tuhe@dtu.dk>
Date: Thu, 6 Mar 2025 12:49:44 +0100
Subject: [PATCH] Solutions for week 4 and 5
---
solutions/ex04/discrete_kuramoto_TODO_1.py | 1 +
solutions/ex04/discrete_kuramoto_TODO_2.py | 1 +
solutions/ex04/discrete_kuramoto_TODO_3.py | 1 +
solutions/ex04/model_pendulum_TODO_1.py | 1 +
solutions/ex04/model_pendulum_TODO_2.py | 1 +
solutions/ex04/pid_TODO_1.py | 6 ++++++
solutions/ex04/pid_TODO_2.py | 1 +
solutions/ex04/pid_car_TODO_1.py | 2 ++
solutions/ex04/pid_car_TODO_2.py | 2 ++
solutions/ex04/pid_locomotive_agent_TODO_1.py | 1 +
solutions/ex04/pid_locomotive_agent_TODO_2.py | 1 +
solutions/ex04/pid_lunar_TODO_1.py | 2 ++
solutions/ex04/pid_pendulum_TODO_1.py | 2 ++
solutions/ex04/pid_pendulum_TODO_2.py | 1 +
solutions/ex04/pid_pendulum_TODO_3.py | 1 +
solutions/ex05/direct_TODO_1.py | 1 +
solutions/ex05/direct_TODO_10.py | 1 +
solutions/ex05/direct_TODO_2.py | 2 ++
solutions/ex05/direct_TODO_3.py | 2 ++
solutions/ex05/direct_TODO_4.py | 2 ++
solutions/ex05/direct_TODO_5.py | 2 ++
solutions/ex05/direct_TODO_6.py | 1 +
solutions/ex05/direct_TODO_7.py | 1 +
solutions/ex05/direct_TODO_8.py | 1 +
solutions/ex05/direct_TODO_9.py | 3 +++
solutions/ex05/direct_agent_TODO_1.py | 1 +
solutions/ex05/direct_agent_TODO_2.py | 7 +++++++
solutions/ex05/direct_cartpole_kelly_TODO_1.py | 2 ++
solutions/ex05/direct_cartpole_kelly_TODO_2.py | 2 ++
29 files changed, 52 insertions(+)
create mode 100644 solutions/ex04/discrete_kuramoto_TODO_1.py
create mode 100644 solutions/ex04/discrete_kuramoto_TODO_2.py
create mode 100644 solutions/ex04/discrete_kuramoto_TODO_3.py
create mode 100644 solutions/ex04/model_pendulum_TODO_1.py
create mode 100644 solutions/ex04/model_pendulum_TODO_2.py
create mode 100644 solutions/ex04/pid_TODO_1.py
create mode 100644 solutions/ex04/pid_TODO_2.py
create mode 100644 solutions/ex04/pid_car_TODO_1.py
create mode 100644 solutions/ex04/pid_car_TODO_2.py
create mode 100644 solutions/ex04/pid_locomotive_agent_TODO_1.py
create mode 100644 solutions/ex04/pid_locomotive_agent_TODO_2.py
create mode 100644 solutions/ex04/pid_lunar_TODO_1.py
create mode 100644 solutions/ex04/pid_pendulum_TODO_1.py
create mode 100644 solutions/ex04/pid_pendulum_TODO_2.py
create mode 100644 solutions/ex04/pid_pendulum_TODO_3.py
create mode 100644 solutions/ex05/direct_TODO_1.py
create mode 100644 solutions/ex05/direct_TODO_10.py
create mode 100644 solutions/ex05/direct_TODO_2.py
create mode 100644 solutions/ex05/direct_TODO_3.py
create mode 100644 solutions/ex05/direct_TODO_4.py
create mode 100644 solutions/ex05/direct_TODO_5.py
create mode 100644 solutions/ex05/direct_TODO_6.py
create mode 100644 solutions/ex05/direct_TODO_7.py
create mode 100644 solutions/ex05/direct_TODO_8.py
create mode 100644 solutions/ex05/direct_TODO_9.py
create mode 100644 solutions/ex05/direct_agent_TODO_1.py
create mode 100644 solutions/ex05/direct_agent_TODO_2.py
create mode 100644 solutions/ex05/direct_cartpole_kelly_TODO_1.py
create mode 100644 solutions/ex05/direct_cartpole_kelly_TODO_2.py
diff --git a/solutions/ex04/discrete_kuramoto_TODO_1.py b/solutions/ex04/discrete_kuramoto_TODO_1.py
new file mode 100644
index 0000000..e0e9d22
--- /dev/null
+++ b/solutions/ex04/discrete_kuramoto_TODO_1.py
@@ -0,0 +1 @@
+ f_euler = dmodel.f(x, u, k=0)
\ No newline at end of file
diff --git a/solutions/ex04/discrete_kuramoto_TODO_2.py b/solutions/ex04/discrete_kuramoto_TODO_2.py
new file mode 100644
index 0000000..001427a
--- /dev/null
+++ b/solutions/ex04/discrete_kuramoto_TODO_2.py
@@ -0,0 +1 @@
+ f_euler_derivative, _ = dmodel.f_jacobian(x, u)
\ No newline at end of file
diff --git a/solutions/ex04/discrete_kuramoto_TODO_3.py b/solutions/ex04/discrete_kuramoto_TODO_3.py
new file mode 100644
index 0000000..5b50fea
--- /dev/null
+++ b/solutions/ex04/discrete_kuramoto_TODO_3.py
@@ -0,0 +1 @@
+ next_x, cost, terminated, _, metadata = env.step([u])
\ No newline at end of file
diff --git a/solutions/ex04/model_pendulum_TODO_1.py b/solutions/ex04/model_pendulum_TODO_1.py
new file mode 100644
index 0000000..bbdc073
--- /dev/null
+++ b/solutions/ex04/model_pendulum_TODO_1.py
@@ -0,0 +1 @@
+ x_dot = model.f([1, 2], [0], t=0)
\ No newline at end of file
diff --git a/solutions/ex04/model_pendulum_TODO_2.py b/solutions/ex04/model_pendulum_TODO_2.py
new file mode 100644
index 0000000..59b6a6b
--- /dev/null
+++ b/solutions/ex04/model_pendulum_TODO_2.py
@@ -0,0 +1 @@
+ x_dot_numpy = model.f([1, 2], [0], t=0)
\ No newline at end of file
diff --git a/solutions/ex04/pid_TODO_1.py b/solutions/ex04/pid_TODO_1.py
new file mode 100644
index 0000000..be1769b
--- /dev/null
+++ b/solutions/ex04/pid_TODO_1.py
@@ -0,0 +1,6 @@
+ e = self.target - x
+ # if self.e_prior == 0 and self.I == 0:
+ # self.e_prior = e
+ self.I = self.I + e * self.dt
+ u = self.Kp * e + self.Ki * self.I + self.Kd * (e - self.e_prior)/self.dt
+ self.e_prior = e
\ No newline at end of file
diff --git a/solutions/ex04/pid_TODO_2.py b/solutions/ex04/pid_TODO_2.py
new file mode 100644
index 0000000..7468df3
--- /dev/null
+++ b/solutions/ex04/pid_TODO_2.py
@@ -0,0 +1 @@
+ u = pid.pi(x_cur[0])
\ No newline at end of file
diff --git a/solutions/ex04/pid_car_TODO_1.py b/solutions/ex04/pid_car_TODO_1.py
new file mode 100644
index 0000000..4201c47
--- /dev/null
+++ b/solutions/ex04/pid_car_TODO_1.py
@@ -0,0 +1,2 @@
+ self.pid_angle = PID(dt=env.discrete_model.dt, Kp=1.0, Ki=0, Kd=0, target=0)
+ self.pid_velocity = PID(dt=env.discrete_model.dt, Kp=1.5, Ki=0, Kd=0, target=v_target)
\ No newline at end of file
diff --git a/solutions/ex04/pid_car_TODO_2.py b/solutions/ex04/pid_car_TODO_2.py
new file mode 100644
index 0000000..d7b67d3
--- /dev/null
+++ b/solutions/ex04/pid_car_TODO_2.py
@@ -0,0 +1,2 @@
+ xx = x[5] + x[3] if self.use_both_x5_x3 else x[5]
+ u = np.asarray([self.pid_angle.pi(xx), self.pid_velocity.pi(x[0])])
\ No newline at end of file
diff --git a/solutions/ex04/pid_locomotive_agent_TODO_1.py b/solutions/ex04/pid_locomotive_agent_TODO_1.py
new file mode 100644
index 0000000..e8912a0
--- /dev/null
+++ b/solutions/ex04/pid_locomotive_agent_TODO_1.py
@@ -0,0 +1 @@
+ self.pid = PID(dt=dt, Kp=Kp, Ki=Ki, Kd=Kd, target=target)
\ No newline at end of file
diff --git a/solutions/ex04/pid_locomotive_agent_TODO_2.py b/solutions/ex04/pid_locomotive_agent_TODO_2.py
new file mode 100644
index 0000000..a512e14
--- /dev/null
+++ b/solutions/ex04/pid_locomotive_agent_TODO_2.py
@@ -0,0 +1 @@
+ u = self.pid.pi(x[0])
\ No newline at end of file
diff --git a/solutions/ex04/pid_lunar_TODO_1.py b/solutions/ex04/pid_lunar_TODO_1.py
new file mode 100644
index 0000000..7a4671f
--- /dev/null
+++ b/solutions/ex04/pid_lunar_TODO_1.py
@@ -0,0 +1,2 @@
+ alt_adj = self.pid_alt.pi( -(np.abs(x[0])- x[1]) )
+ ang_adj = self.pid_ang.pi( -((.25 * np.pi) * (x[0] + x[2]) - x[4]) )
\ No newline at end of file
diff --git a/solutions/ex04/pid_pendulum_TODO_1.py b/solutions/ex04/pid_pendulum_TODO_1.py
new file mode 100644
index 0000000..d21532b
--- /dev/null
+++ b/solutions/ex04/pid_pendulum_TODO_1.py
@@ -0,0 +1,2 @@
+ u = self.pid.pi(x[0])
+ u = np.clip(u, self.env.action_space.low, self.env.action_space.high)
\ No newline at end of file
diff --git a/solutions/ex04/pid_pendulum_TODO_2.py b/solutions/ex04/pid_pendulum_TODO_2.py
new file mode 100644
index 0000000..6ae3232
--- /dev/null
+++ b/solutions/ex04/pid_pendulum_TODO_2.py
@@ -0,0 +1 @@
+ agent = PIDPendulumAgent(env, dt=env.dt, Kp=12, Ki=0, Kd=2, target_angle=0)
\ No newline at end of file
diff --git a/solutions/ex04/pid_pendulum_TODO_3.py b/solutions/ex04/pid_pendulum_TODO_3.py
new file mode 100644
index 0000000..388da47
--- /dev/null
+++ b/solutions/ex04/pid_pendulum_TODO_3.py
@@ -0,0 +1 @@
+ agent = PIDPendulumAgent(env, dt=env.dt, Kp=12, Ki=2, Kd=2, target_angle=np.pi/6)
\ No newline at end of file
diff --git a/solutions/ex05/direct_TODO_1.py b/solutions/ex05/direct_TODO_1.py
new file mode 100644
index 0000000..943a62d
--- /dev/null
+++ b/solutions/ex05/direct_TODO_1.py
@@ -0,0 +1 @@
+ guess = {k: solutions[i - 1]['fun'][k] for k in ['t0', 'tF', 'x', 'u'] }
\ No newline at end of file
diff --git a/solutions/ex05/direct_TODO_10.py b/solutions/ex05/direct_TODO_10.py
new file mode 100644
index 0000000..30c2fff
--- /dev/null
+++ b/solutions/ex05/direct_TODO_10.py
@@ -0,0 +1 @@
+ x_interp = xs[:, k] + tau * fs[:, k] + (tau ** 2 / (2 * hk)) * (fs[:, k + 1] - fs[:, k])
\ No newline at end of file
diff --git a/solutions/ex05/direct_TODO_2.py b/solutions/ex05/direct_TODO_2.py
new file mode 100644
index 0000000..93276c3
--- /dev/null
+++ b/solutions/ex05/direct_TODO_2.py
@@ -0,0 +1,2 @@
+ z, z0, z_lb, z_ub = z + xs[k], z0 + list(guess['x'](tk).flat), z_lb + x_low, z_ub + x_high
+ z, z0, z_lb, z_ub = z + us[k], z0 + list(guess['u'](tk).flat), z_lb + u_low, z_ub + u_high
\ No newline at end of file
diff --git a/solutions/ex05/direct_TODO_3.py b/solutions/ex05/direct_TODO_3.py
new file mode 100644
index 0000000..2a48c18
--- /dev/null
+++ b/solutions/ex05/direct_TODO_3.py
@@ -0,0 +1,2 @@
+ z, z0, z_lb, z_ub = z+[t0], z0+[guess['t0']], z_lb+[model.t0_bound().low[0]], z_ub+[model.t0_bound().high[0]]
+ z, z0, z_lb, z_ub = z+[tF], z0+[guess['tF']], z_lb+[model.tF_bound().low[0]], z_ub+[model.tF_bound().high[0]]
\ No newline at end of file
diff --git a/solutions/ex05/direct_TODO_4.py b/solutions/ex05/direct_TODO_4.py
new file mode 100644
index 0000000..a17d399
--- /dev/null
+++ b/solutions/ex05/direct_TODO_4.py
@@ -0,0 +1,2 @@
+ fs.append(model.sym_f(x=xs[k], u=us[k], t=ts[k]))
+ cs.append(cost.sym_c(x=xs[k], u=us[k], t=ts[k]))
\ No newline at end of file
diff --git a/solutions/ex05/direct_TODO_5.py b/solutions/ex05/direct_TODO_5.py
new file mode 100644
index 0000000..4503612
--- /dev/null
+++ b/solutions/ex05/direct_TODO_5.py
@@ -0,0 +1,2 @@
+ hk = (ts[k + 1] - ts[k])
+ J += .5 * hk * (cs[k] + cs[k + 1])
\ No newline at end of file
diff --git a/solutions/ex05/direct_TODO_6.py b/solutions/ex05/direct_TODO_6.py
new file mode 100644
index 0000000..d44ee27
--- /dev/null
+++ b/solutions/ex05/direct_TODO_6.py
@@ -0,0 +1 @@
+ Ieq.append((xs[k+1][j] - xs[k][j]) - 0.5*hk*(fs[k+1][j] + fs[k][j]))
\ No newline at end of file
diff --git a/solutions/ex05/direct_TODO_7.py b/solutions/ex05/direct_TODO_7.py
new file mode 100644
index 0000000..40f3af4
--- /dev/null
+++ b/solutions/ex05/direct_TODO_7.py
@@ -0,0 +1 @@
+ Iineq += model.sym_h(x=xs[k], u=us[k], t=ts[k])
\ No newline at end of file
diff --git a/solutions/ex05/direct_TODO_8.py b/solutions/ex05/direct_TODO_8.py
new file mode 100644
index 0000000..10dcc17
--- /dev/null
+++ b/solutions/ex05/direct_TODO_8.py
@@ -0,0 +1 @@
+ J_jac = sym.lambdify([z], sym.derive_by_array(J, z), modules='numpy')
\ No newline at end of file
diff --git a/solutions/ex05/direct_TODO_9.py b/solutions/ex05/direct_TODO_9.py
new file mode 100644
index 0000000..5ed2dd8
--- /dev/null
+++ b/solutions/ex05/direct_TODO_9.py
@@ -0,0 +1,3 @@
+ for k in range(len(ts) - 1):
+ if ts[k] <= t_new and t_new <= ts[k + 1]:
+ break
\ No newline at end of file
diff --git a/solutions/ex05/direct_agent_TODO_1.py b/solutions/ex05/direct_agent_TODO_1.py
new file mode 100644
index 0000000..dca993f
--- /dev/null
+++ b/solutions/ex05/direct_agent_TODO_1.py
@@ -0,0 +1 @@
+ self.ufun = solutions[-1]['fun']['u']
\ No newline at end of file
diff --git a/solutions/ex05/direct_agent_TODO_2.py b/solutions/ex05/direct_agent_TODO_2.py
new file mode 100644
index 0000000..311c0af
--- /dev/null
+++ b/solutions/ex05/direct_agent_TODO_2.py
@@ -0,0 +1,7 @@
+ t = info['time_seconds']
+ if t > self.ts_grid[-1]:
+ print("Simulation time is", t, "which exceeds the maximal planning horizon t_F =", self.ts_grid[-1])
+ raise Exception("Time exceed agents planning horizon")
+
+ u = self.ufun(t)
+ u = np.asarray(self.env.discrete_model.phi_u(u))
\ No newline at end of file
diff --git a/solutions/ex05/direct_cartpole_kelly_TODO_1.py b/solutions/ex05/direct_cartpole_kelly_TODO_1.py
new file mode 100644
index 0000000..c3da19e
--- /dev/null
+++ b/solutions/ex05/direct_cartpole_kelly_TODO_1.py
@@ -0,0 +1,2 @@
+ Q = np.zeros((4, 4))
+ return SymbolicQRCost(Q=Q, R=np.asarray([[1.0]]) )
\ No newline at end of file
diff --git a/solutions/ex05/direct_cartpole_kelly_TODO_2.py b/solutions/ex05/direct_cartpole_kelly_TODO_2.py
new file mode 100644
index 0000000..11a9b85
--- /dev/null
+++ b/solutions/ex05/direct_cartpole_kelly_TODO_2.py
@@ -0,0 +1,2 @@
+ duration = 2
+ return Box(duration, duration, shape=(1,))
\ No newline at end of file
--
GitLab