Hi, since around 2 months I’ve been receiving a MINSTEP error from the QP solver. This is my current code. I’m trying to implement a swingup and stabilization of an inverted pendulum. I’ve pretty much tried everything I could imagine. Does anyone have an idea? Could it be my solver options or my model itself?
Kind regards
import numpy as np
import casadi as ca
import matplotlib.pyplot as plt
import matplotlib.patches as mpatches
from matplotlib.animation import FuncAnimation
import time
import pyads
import control
from scipy.signal import cont2discrete
from datetime import datetime
from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
from scipy.signal import cont2discrete
import control
import time
import os
import shutil
from ekf_observer import EKF
import sys
# ─────────────────────────────────────────────────────────────────────────────
# DLL VERWALTUNG
# ─────────────────────────────────────────────────────────────────────────────
ACADOS_LIB_DIR = r"C:\Users\mz\Desktop\acados\lib"
C_GEN_DIR = "c_generated_code"
REQUIRED_DLLS = ["acados.dll", "blasfeo.dll", "hpipm.dll", "qpOASES_e.dll"]
def clean_and_prepare_generated_code():
if not os.path.exists(C_GEN_DIR):
os.makedirs(C_GEN_DIR)
else:
for item in os.listdir(C_GEN_DIR):
item_path = os.path.join(C_GEN_DIR, item)
# Alles löschen außer den 4 benötigten DLLs
if os.path.isfile(item_path) and item not in REQUIRED_DLLS:
os.remove(item_path)
elif os.path.isdir(item_path):
shutil.rmtree(item_path)
# Fehlende DLLs kopieren
for dll in REQUIRED_DLLS:
dst = os.path.join(C_GEN_DIR, dll)
if not os.path.exists(dst):
shutil.copy(os.path.join(ACADOS_LIB_DIR, dll), dst)
print(f"Kopiert: {dll}")
clean_and_prepare_generated_code()
# ---------------- ADS / Timing ----------------
AMS_NET_ID = "134.28.49.25.1.1" # deine AMS-NetID
PLC_IP = "134.28.49.25" # IP der Steuerung
AMS_PORT = 851 # Standard-Port für PLC1
# ---------------- Encoder ----------------
ENCODER_CPR_ARM = 8192 # Counts per Revolution
ENCODER_CPR_PENDLE = 4096 # Counts per Revolution
UINT16_MAX = 65536 # 2^16 für UINT16 wrap-around
# ============================================================
# Hilfsfunktionen:
# ============================================================
def handle_encoder_overflow(current, previous, max_value=65536):
diff = current - previous
# Überlauf nach oben (0 -> 65535)
if diff < -max_value / 2:
diff += max_value
# Überlauf nach unten (65535 -> 0)
elif diff > max_value / 2:
diff -= max_value
return diff
def counts_to_rad_delta(diff_counts, cpr=8192):
return (diff_counts / cpr) * 2.0 * np.pi # KEIN modulo!
log_timestamp = datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
log_filename = f"nmpc_log_{log_timestamp}.txt"
class Tee:
def __init__(self, *streams):
self.streams = streams
def write(self, data):
for s in self.streams:
try:
s.write(data)
s.flush()
except (ValueError, OSError):
pass
def flush(self):
for s in self.streams:
try:
s.flush()
except (ValueError, OSError):
pass
log_file = open(log_filename, 'w', encoding='utf-8')
sys.stdout = Tee(sys.__stdout__, log_file)
print(f"=== Log-Datei: {log_filename} ===")
# ─────────────────────────────────────────────────────────────────────────────
# Model parameter
# ─────────────────────────────────────────────────────────────────────────────
m1 = 0.09
m2 = 0.08
mh = 0.10
L_1 = 0.1492
L_2 = 0.1778
g = 9.81
km = 0.0741
# ─────────────────────────────────────────────────────────────────────────────
# NMPC PARAMETER und contrains
# ─────────────────────────────────────────────────────────────────────────────
N = 50
dt = 0.02
T_sim = 5
n_steps = int(T_sim / dt)
U = 5
MODE = "swingup" # oder "swingup"
COST_TYPE = "NONLINEAR_LS" # "EXTERNAL" oder "NONLINEAR_LS"
if MODE == "stabilize":
x0 = np.array([0.0, np.pi, 0.0, 0.0])
x_ref = np.array([0.0, np.pi, 0.0, 0.0])
Q = np.diag([100.0, 150.0, 20.0, 20.0])
R = np.diag([1])
B = np.diag([1])
else: # swingup
x0 = np.array([0.0, 0.0, 0.0, 0.0])
x_ref = np.array([0.0, np.pi, 0.0, 0.0])
Q = np.diag([1.0, 100.0, 2.0, 1.0])
R = np.diag([1.0])
B = np.diag([1e7])
u_ref = 0.0
U = 3.5
Q_new = np.diag([Q[0,0], Q[0,0], Q[1,1], Q[1,1], Q[2,2], Q[3,3]])
# ------------------------------------logging---------------------------
t_history = []
x_history = np.zeros((4, n_steps + 1))
u_history = np.zeros((1, n_steps))
status_history = np.zeros(n_steps)
qp_status_history = np.zeros(n_steps)
u_opt_history = np.zeros((N, n_steps))
fallback_mask = np.zeros(n_steps, dtype=bool) # True = Fallback wurde genutzt
solver_time = []
twincat_time = []
error_history = np.zeros((4, n_steps))
x_ekf_history = np.zeros((4, n_steps + 1))
omega_raw_history = np.zeros((2, n_steps + 1))
# ─────────────────────────────────────────────────────────────────────────────
# Terminal Cost (LQR)
# ─────────────────────────────────────────────────────────────────────────────
A_down = np.array([
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0],
[-77.7, 42.9, 0.0, 0.0],
[57.5, 139.3, 0.0, 0.0]
])
B_down = np.array([[0.0], [0.0], [27.2], [18.4]])
C_mat = np.eye(4)
D_mat = np.zeros((4, 1))
Ad, Bd, _, _, _ = cont2discrete((A_down, B_down, C_mat, D_mat), dt, method="zoh")
_, P, _ = control.dlqr(Ad, Bd, Q, R)
# ─────────────────────────────────────────────────────────────────────────────
# Acados Modell (CasADi symbolisch, nur für den Solver)
# ─────────────────────────────────────────────────────────────────────────────
def build_acados_model():
model = AcadosModel()
model.name = "adip"
x = ca.MX.sym("x", 4)
u = ca.MX.sym("u", 1)
theta, alpha, theta_dot, alpha_dot = x[0], x[1], x[2], x[3]
Force = u[0]
denom = L_1**2 * (4*ca.cos(alpha)**2 * m2 - m1 - 4*m2 - 4*mh)
theta_ddot = (
-(1 / denom) * 2 * (
2*L_1**2 * ca.cos(alpha)*ca.sin(alpha) * theta_dot**2 * m2
- 2*L_1**2 * ca.cos(alpha)*ca.sin(alpha) * theta_dot * alpha_dot * m2
+ L_2*m2*L_1 * theta_dot**2 * ca.sin(alpha)
+ m2*L_2 * alpha_dot * L_1 * theta_dot * ca.sin(alpha)
+ m2*L_2 * alpha_dot**2 * L_1 * ca.sin(alpha)
+ 2*L_1 * ca.cos(alpha)**2 * ca.sin(theta) * g * m2
+ 2*L_1 * ca.cos(alpha)*ca.cos(theta)*ca.sin(alpha) * g * m2
- m1*g*L_1 * ca.sin(theta)
- 2*m2*g*L_1 * ca.sin(theta)
- 2*mh*g*L_1 * ca.sin(theta)
+ 2*Force
)
)
alpha_ddot = (
(1 / (L_2 * denom)) * 2 * (
4*L_1**2*L_2 * ca.cos(alpha)*ca.sin(alpha) * theta_dot**2 * m2
+ 2*L_1**2*L_2 * ca.cos(alpha)*ca.sin(alpha) * alpha_dot**2 * m2
+ L_1**3 * ca.sin(alpha) * theta_dot**2 * m1
+ 4*L_1**3 * ca.sin(alpha) * theta_dot**2 * m2
+ 4*L_1**3 * ca.sin(alpha) * theta_dot**2 * mh
- L_1**3 * ca.sin(alpha) * theta_dot * alpha_dot * m1
- 4*L_1**3 * ca.sin(alpha) * theta_dot * alpha_dot * m2
- 4*L_1**3 * ca.sin(alpha) * theta_dot * alpha_dot * mh
+ L_1*L_2**2 * ca.sin(alpha) * theta_dot**2 * m2
+ L_2**2*m2*L_1 * alpha_dot * theta_dot * ca.sin(alpha)
+ m2*L_2**2 * alpha_dot**2 * L_1 * ca.sin(alpha)
+ 2*L_1*L_2 * ca.cos(alpha)**2 * ca.sin(theta) * g * m2
+ 2*L_1*L_2 * ca.cos(alpha)*ca.cos(theta)*ca.sin(alpha) * g * m2
- L_1**2 * ca.cos(alpha)*ca.sin(theta) * g * m1
+ L_1**2 * ca.cos(theta)*ca.sin(alpha) * g * m1
+ 4*L_1**2 * ca.cos(theta)*ca.sin(alpha) * g * m2
+ 4*L_1**2 * ca.cos(theta)*ca.sin(alpha) * g * mh
- ca.sin(theta)*g*L_1*m1*L_2
- 2*ca.sin(theta)*g*L_1*m2*L_2
- 2*ca.sin(theta)*g*L_1*mh*L_2
+ 4*Force*L_1*ca.cos(alpha)
+ 2*Force*L_2
)
)
f_expl = ca.vertcat(theta_dot, alpha_dot, theta_ddot, alpha_ddot)
xdot = ca.MX.sym("xdot", 4)
u_prev = ca.MX.sym("u_prev", 1)
model.x = x
model.u = u
model.f_expl_expr = f_expl
model.f_impl_expr = xdot - f_expl
model.xdot = xdot
model.p = u_prev
return model
# ─────────────────────────────────────────────────────────────────────────────
# Acados OCP Setup
# ─────────────────────────────────────────────────────────────────────────────
if COST_TYPE == "EXTERNAL":
def create_ocp_solver(x_ref, u_ref):
ocp = AcadosOcp() # create ocp object to formulate the OCP
model = build_acados_model() # set model
ocp.model = model # set model
nx = 4 # 4 states
nu = 1 # 1 input
x = ocp.model.x
u = ocp.model.u
u_prev = ocp.model.p
ocp.solver_options.N_horizon = N
ocp.parameter_values = np.array([0.0])
ocp.cost.cost_type = "EXTERNAL"
ocp.cost.cost_type_e = "EXTERNAL"
error_theta_cos = ca.cos(x[0]) - ca.cos(x_ref[0])
error_theta_sin = ca.sin(x[0]) - ca.sin(x_ref[0])
error_alpha_cos = ca.cos(x[1]) - ca.cos(x_ref[1])
error_alpha_sin = ca.sin(x[1]) - ca.sin(x_ref[1])
error_omega_1 = x[2] - x_ref[2]
error_omega_2 = x[3] - x_ref[3]
error_x = ca.vertcat(
error_theta_cos,
error_theta_sin,
error_alpha_cos,
error_alpha_sin,
error_omega_1,
error_omega_2)
P_new = np.zeros((6, 6))
P_new[0, 0] = P[0, 0] # cos_theta ← P[0,0], nicht P[2,2]
P_new[1, 1] = P[0, 0] # sin_theta
P_new[2, 2] = P[1, 1] # cos_alpha ← P[1,1], nicht P[3,3]
P_new[3, 3] = P[1, 1] # sin_alpha
P_new[4, 4] = P[2, 2] # omega1
P_new[5, 5] = P[3, 3] # omega2
error_u = u - u_ref
change_of_u = u - u_prev
model.cost_expr_ext_cost = error_x.T @ Q_new @ error_x + error_u.T @ R @ error_u + change_of_u.T @ B @ change_of_u
model.cost_expr_ext_cost_e = error_x.T @ P_new @ error_x
ocp.constraints.x0 = x0 # Anfangsbedingung
ocp.constraints.lbu = np.array([-U* km])
ocp.constraints.ubu = np.array([U* km])
ocp.constraints.idxbu = np.array([0])
ocp.solver_options.tf = N * dt # istdas richitg für mich ?
ocp.solver_options.integrator_type = "IRK" # ERK, IRK, GNSF, DISCRETE # Explicit Runge-Kutta # Implicit Runge-Kutta -> IRK benutzen
ocp.solver_options.num_stages = 4 # 1, 2, 3, 4 (Runge-Kutta Stufen)
ocp.solver_options.num_steps = 1 # 1 default, 2, 3, ... (mehr = genauer, langsamer) Number of steps in the integrator.
ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" # PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES
ocp.solver_options.hpipm_mode = "BALANCE" # SPEED_ABS, SPEED, BALANCE, ROBUST
ocp.solver_options.qp_solver_iter_max = 400 # 50, 100, 200, 400, ...
ocp.solver_options.nlp_solver_type = "SQP_RTI" # SQP, SQP_RTI
ocp.solver_options.nlp_solver_max_iter = 1 # 1 (RTI), 50, 100, 200 (SQP)
ocp.solver_options.hessian_approx = "EXACT" # GAUSS_NEWTON, EXACT
ocp.solver_options.regularize_method = "CONVEXIFY" # NO_REGULARIZE, MIRROR, PROJECT, PROJECT_REDUC_HESS, CONVEXIFY, GERSHGORIN_LEVENBERG_MARQUARDT # nochmka nachguckebn
ocp.solver_options.levenberg_marquardt = 0.0 # adds an constant term to the hessain
ocp.solver_options.reg_epsilon = 1e-2 # 1e-3, 1e-4, 1e-6, ...
ocp.solver_options.qp_tol = 1e-2 # 1e-3, 1e-4, 1e-6, 1e-8
ocp.solver_options.tol = 1e-2 # 1e-3, 1e-4, 1e-6, 1e-8
ocp.solver_options.print_level = 0 # 0 (still), 1 (iter), 2 (verbose)
ocp.solver_options.qp_solver_warm_start = 2
solver = AcadosOcpSolver(ocp, json_file="adip_ocp.json")
return solver
if COST_TYPE == "NONLINEAR_LS":
def create_ocp_solver(x_ref, u_ref):
ocp = AcadosOcp() # create ocp object to formulate the OCP
model = build_acados_model() # set model
ocp.model = model # set model
nx = 4 # 4 states
nu = 1 # 1 input
x = ocp.model.x
u = ocp.model.u
u_prev = ocp.model.p
ocp.solver_options.N_horizon = N
ocp.parameter_values = np.array([0.0])
ocp.cost.cost_type = "NONLINEAR_LS"
ocp.cost.cost_type_e = "NONLINEAR_LS"
error_theta_cos = ca.cos(x[0]) - ca.cos(x_ref[0])
error_theta_sin = ca.sin(x[0]) - ca.sin(x_ref[0])
error_alpha_cos = ca.cos(x[1]) - ca.cos(x_ref[1])
error_alpha_sin = ca.sin(x[1]) - ca.sin(x_ref[1])
error_omega_1 = x[2] - x_ref[2]
error_omega_2 = x[3] - x_ref[3]
error_x = ca.vertcat(
error_theta_cos,
error_theta_sin,
error_alpha_cos,
error_alpha_sin,
error_omega_1,
error_omega_2)
P_new = np.zeros((6, 6))
P_new[0, 0] = P[0, 0] # cos_theta ← P[0,0], nicht P[2,2]
P_new[1, 1] = P[0, 0] # sin_theta
P_new[2, 2] = P[1, 1] # cos_alpha ← P[1,1], nicht P[3,3]
P_new[3, 3] = P[1, 1] # sin_alpha
P_new[4, 4] = P[2, 2] # omega1
P_new[5, 5] = P[3, 3] # omega2
error_u = u - u_ref
change_of_u = u - u_prev
y_expr = ca.vertcat(
error_x, # 6 Einträge
error_u, # 1 Eintrag
change_of_u # 1 Eintrag
)
y_expr_e = error_x # 6 Einträge am Endknoten
# Gewichtungsmatrix W (8x8) — Blockdiagonal aus Q_new, R, B
W = np.zeros((8, 8))
W[0:6, 0:6] = Q_new
W[6:7, 6:7] = R
W[7:8, 7:8] = B
# Terminal-Gewichtung (6x6)
W_e = P_new
# In das Modell hineinreichen
model.cost_y_expr = y_expr
model.cost_y_expr_e = y_expr_e
ocp.cost.W = W
ocp.cost.W_e = W_e
ocp.cost.yref = np.zeros(8) # Referenz = 0, weil error_x schon zentriert ist
ocp.cost.yref_e = np.zeros(6)
ocp.constraints.x0 = x0 # Anfangsbedingung
ocp.constraints.lbu = np.array([-U* km])
ocp.constraints.ubu = np.array([U* km])
ocp.constraints.idxbu = np.array([0])
ocp.solver_options.tf = N * dt # istdas richitg für mich ?
ocp.solver_options.integrator_type = "IRK" # ERK, IRK, GNSF, DISCRETE # Explicit Runge-Kutta # Implicit Runge-Kutta -> IRK benutzen
ocp.solver_options.num_stages = 4 # 1, 2, 3, 4 (Runge-Kutta Stufen)
ocp.solver_options.num_steps = 1 # 1 default, 2, 3, ... (mehr = genauer, langsamer) Number of steps in the integrator.
ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" # PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES
ocp.solver_options.hpipm_mode = "BALANCE" # SPEED_ABS, SPEED, BALANCE, ROBUST
ocp.solver_options.qp_solver_iter_max = 400 # 50, 100, 200, 400, ...
ocp.solver_options.nlp_solver_type = "SQP_RTI" # SQP, SQP_RTI
ocp.solver_options.nlp_solver_max_iter = 1 # 1 (RTI), 50, 100, 200 (SQP)
ocp.solver_options.hessian_approx = "GAUSS_NEWTON" # GAUSS_NEWTON, EXACT
ocp.solver_options.regularize_method = "CONVEXIFY" # NO_REGULARIZE, MIRROR, PROJECT, PROJECT_REDUC_HESS, CONVEXIFY, GERSHGORIN_LEVENBERG_MARQUARDT # nochmka nachguckebn
ocp.solver_options.levenberg_marquardt = 0.0 # adds an constant term to the hessain
ocp.solver_options.reg_epsilon = 1e-2 # 1e-3, 1e-4, 1e-6, ...
ocp.solver_options.qp_tol = 1e-2 # 1e-3, 1e-4, 1e-6, 1e-8
ocp.solver_options.tol = 1e-2 # 1e-3, 1e-4, 1e-6, 1e-8
ocp.solver_options.print_level = 0 # 0 (still), 1 (iter), 2 (verbose)
ocp.solver_options.qp_solver_warm_start = 2
solver = AcadosOcpSolver(ocp, json_file="adip_ocp.json")
return solver
# ─────────────────────────────────────────────────────────────────────────────
# Hilfsfunktionen: Lösung auslesen / Warm-Start setzen
# ─────────────────────────────────────────────────────────────────────────────
def get_solution(solver):
"""Komplette Trajektorie aus dem Solver auslesen – analog zu sol.value(X/U)."""
x_opt = np.array([solver.get(i, "x") for i in range(N + 1)]).T # (4, N+1)
u_opt = np.array([solver.get(i, "u") for i in range(N)]).T # (1, N)
return u_opt, x_opt
def set_warm_start(solver, x_init, u_init):
"""Warm-Start setzen – analog zu opti.set_initial(X, x_init)."""
for i in range(N):
solver.set(i, "x", x_init[:, i])
solver.set(i, "u", u_init[:, i])
solver.set(N, "x", x_init[:, N])
# ----------------Initialisierung ----------------
plc = pyads.Connection(AMS_NET_ID, pyads.PORT_TC3PLC1)
plc.open()
#encoder resetten
print("resetting values ...")
plc.write_by_name("GVL.Reset_ENC_1", True)
plc.write_by_name("GVL.Reset_ENC_2", True)
time.sleep(0.05)
plc.write_by_name("GVL.Reset_ENC_1", False)
plc.write_by_name("GVL.Reset_ENC_2", False)
time.sleep(0.05)
#enc = plc.read_by_name("GVL.Enc")
#prev_EncCounter_Ch1, prev_EncCounter_Ch2 = enc[0], enc[1]
#plc.write_by_name("GVL.Motor_value", 0) # Motor auf 0 setzen
# ─────────────────────────────────────────────────────────────────────────────
# MPC Closed-Loop Simulation
# ─────────────────────────────────────────────────────────────────────────────
print("Starte MPC-Simulation ...")
solver = create_ocp_solver(x_ref, u_ref)
counter = 0
t0_prev = 0
dt_actual = 0.02
# anfangsstate und u für den solver
planned_u = np.zeros((1, N))
planned_x = np.zeros((4, N + 1))
for i in range(N + 1):
planned_x[:, i] = x0
#planned_u = np.load("u_init_from_sim.npy")
#planned_x = np.load("x_init_from_sim.npy")
# ─── Warm-Start (DFKI-Style) ───
#print("Warm-Start: 10 Iterationen am Startpunkt ...")
#for i in range(10):
# solver.set(0, "lbx", x0)
# solver.set(0, "ubx", x0)
# solver.solve()
# status = solver.status
# print(f" Iter {i+1}/10, status={status}")
#print("Warm-Start abgeschlossen.")
# iteration = 8000
# print("Warm-Start: Pre-Iterationen ...")
# x0_warm = x0.copy()
# for i in range(iteration):
# solver.set(0, "lbx", x0_warm)
# solver.set(0, "ubx", x0_warm)
# solver.solve()
# x0_warm = solver.get(1, "x")
# if i % 100 == 0:
# status = solver.status
# print(f" Iter {i}/iteration, status={status}", end="\r")
# print("\nWarm-Start abgeschlossen.")
arm = x0[0]
pendle = x0[1]
omega1 = x0[2]
omega2 = x0[3]
ekf = EKF(dt=dt, x0=x0)
u_prev = 0.0 # letzter angelegter Input für EKF-Predict
try:
while counter < n_steps:
# getting loop time
t0 = time.perf_counter()
if counter > 1:
dt_actual = t0 - t0_prev
t_history.append(dt_actual)
t0_prev = t0
#getting encoder values and time
time_twincat_1 = time.perf_counter()
enc = plc.read_by_name("GVL.Enc")
EncCounter_Ch1, EncCounter_Ch2 = enc[0], enc[1] # raw value
time_twincat_2 = time.perf_counter()
twincat_time.append(time_twincat_2 - time_twincat_1) # ← das hier fehlt
# # Differenz mit Überlauf-Behandlung berechnen
# diff_EncCounter_Ch1 = handle_encoder_overflow(EncCounter_Ch1, prev_EncCounter_Ch1, UINT16_MAX)
# diff_EncCounter_Ch2 = handle_encoder_overflow(EncCounter_Ch2, prev_EncCounter_Ch2, UINT16_MAX)
# # Werte berechnen
# arm = EncCounter_Ch1 * 2*np.pi/ENCODER_CPR_ARM
# pendle = EncCounter_Ch2 * 2*np.pi/ENCODER_CPR_PENDLE
# #arm = np.arctan2(np.sin(arm), np.cos(arm))
# #pendle = np.arctan2(np.sin(pendle), np.cos(pendle))
# if counter > 1:
# arm += counts_to_rad_delta(diff_EncCounter_Ch1, ENCODER_CPR_ARM)
# pendle += counts_to_rad_delta(diff_EncCounter_Ch2, ENCODER_CPR_PENDLE)
# omega1 = counts_to_rad_delta(diff_EncCounter_Ch1, ENCODER_CPR_ARM) / dt_actual
# omega2 = counts_to_rad_delta(diff_EncCounter_Ch2, ENCODER_CPR_PENDLE) / dt_actual
if counter > 0:
diff_EncCounter_Ch1 = handle_encoder_overflow(EncCounter_Ch1, prev_EncCounter_Ch1, UINT16_MAX)
diff_EncCounter_Ch2 = handle_encoder_overflow(EncCounter_Ch2, prev_EncCounter_Ch2, UINT16_MAX)
arm += counts_to_rad_delta(diff_EncCounter_Ch1, ENCODER_CPR_ARM)
pendle += counts_to_rad_delta(diff_EncCounter_Ch2, ENCODER_CPR_PENDLE)
omega1 = counts_to_rad_delta(diff_EncCounter_Ch1, ENCODER_CPR_ARM) / dt_actual
omega2 = counts_to_rad_delta(diff_EncCounter_Ch2, ENCODER_CPR_PENDLE) / dt_actual
x_hat = ekf.update(u_prev, arm, pendle)
x_ekf_history[:, counter - 1] = x_hat
omega_raw_history[:, counter - 1] = [omega1, omega2]
# Aktueller Zustand
x_current = np.array([arm, pendle, x_hat[2], x_hat[3]])
# Aktueller Zustand
#x_current = np.array([arm, pendle, omega1, omega2])
x_history[:, counter - 1] = x_current
error_x = x_current - x_ref
error_history[:, counter - 1] = error_x
# Anfangsbedingung setzen
solver.set(0, "lbx", x_current)
solver.set(0, "ubx", x_current)
for i in range(N):
solver.set(i, "p", np.array([planned_u[0, i]]))
solver_1 = time.perf_counter()
solver.solve()
solver_2 = time.perf_counter()
solver_time_current = solver_2 - solver_1
solver_time.append(solver_time_current)
status = solver.status
qp_status = solver.get_stats('qp_stat')
if status == 0:
# Erfolgreich: Neue Lösung auslesen und anwenden
u_opt, x_opt = get_solution(solver)
u_apply = np.clip(float(u_opt[0, 0]) / km, -U, U)
input = -1 * u_apply * 555.556
motor_value = int(input)
plc.write_by_name("GVL.Motor_value", motor_value)
# Trajektorie für den NÄCHSTEN Schritt vorbereiten
planned_u = np.hstack([u_opt[:, 1:], u_opt[:, -1:]])
planned_x = np.hstack([x_opt[:, 1:], x_opt[:, -1:]])
# Warm-Start und Logging nur bei Erfolg
u_opt_history[:, counter - 1] = u_opt[0, :] / km
else:
u_apply = np.clip(float(planned_u[0, 0]) / km, -U, U)
input = -1 * u_apply * 555.556
motor_value = int(input)
plc.write_by_name("GVL.Motor_value", motor_value)
print(f"Schritt {counter}: Solver Status = {status} -> Nutze Fallback! u_apply = {u_apply} " )
# Gespeicherte Trajektorie weiter nach links verschieben
planned_u = np.hstack([planned_u[:, 1:], planned_u[:, -1:]])
planned_x = np.hstack([planned_x[:, 1:], planned_x[:, -1:]])
fallback_mask[counter - 1] = True
u_opt_history[:, counter - 1] = planned_u[0, :]
# Status loggen
status_history[counter - 1] = status
qp_status_history[counter - 1] = qp_status[-1]
u_history[:, counter - 1] = u_apply
# Warm-Start für nächsten Schritt setzen
set_warm_start(solver, planned_x, planned_u)
# ---------- loggen ----------
print(
f"Schritt {counter} | Status={status} | "
f"input = {u_opt[0, 0]/ km}, u={u_apply}| motorvalue= {motor_value:6d} | "
f"θ1={np.rad2deg(arm):+7.2f} deg, "
f"θ2={np.rad2deg(pendle):+7.2f} deg, "
f"ω1={omega1:+7.2f} rad/s, "
f"ω2={omega2:+7.2f} rad/s | "
f"e_θ={np.rad2deg(error_x[0]):+7.2f}°, "
f"e_α={np.rad2deg(error_x[1]):+7.2f}°, "
)
# Update für nächste Iteration
prev_EncCounter_Ch1 = EncCounter_Ch1
prev_EncCounter_Ch2 = EncCounter_Ch2
counter = counter + 1
u_prev = u_apply * km
while time.perf_counter() - t0 < dt:
pass
except KeyboardInterrupt:
print("\nProgramm beendet.")
finally:
plc.write_by_name("GVL.Motor_value", 0) # Motor ausschalten
plc.close()
print("PLC-Verbindung geschlossen.")
sys.stdout = sys.__stdout__
log_file.close()
# ─────────────────────────────────────────────────────────────────────────────
# Plots (3x3 Grid)
# ─────────────────────────────────────────────────────────────────────────────
n_actual = len(t_history)
t_vec = np.arange(n_actual + 1) * dt
t_u_vec = np.arange(n_actual) * dt
T_arr = np.array(t_history)
u_current = u_history.flatten()[:n_actual]
u_force = u_current * km
fig, axes = plt.subplots(4, 3, figsize=(16, 16))
fig.suptitle('NMPC – ADIP Reales System', fontsize=16)
# ── NEU: Matrizen als Text-Box einfügen ──────────────────────────────────────
q_diag = np.diag(Q)
r_diag = np.diag(R)
b_diag = np.diag(B)
matrix_text = (
f"Q = [{q_diag[0]:.2f}, {q_diag[1]:.0f}, {q_diag[2]:.2f}, {q_diag[3]:.1f}]\n"
f"R = [{r_diag[0]:.0f}]\n"
f"B = [{b_diag[0]:.0f}]"
)
fig.text(
0.5, 0.950,
matrix_text,
ha='center', va='top',
fontsize=9, family='monospace',
bbox=dict(boxstyle='round,pad=0.3', facecolor='lightyellow',
edgecolor='gray', alpha=0.8),
multialignment='center' # ← sorgt dafür, dass jede Zeile für sich zentriert ist
)
# ================= LINKE SPALTE =================
# --- [0,0]: Arm-Winkel theta ---
ax = axes[0, 0]
ax.plot(t_vec[:n_actual+1], np.rad2deg(x_history[0, :n_actual+1]), label=r'$\theta_1$ Arm', color='steelblue')
ax.axhline(np.rad2deg(x_ref[0]), linestyle='--', linewidth=1, color='steelblue', label=r'$\theta_{1,ref}$')
ax.set_xlabel('Zeit [s]')
ax.set_ylabel('Winkel [°]')
ax.set_title(r'Arm-Winkel $\theta$')
ax.grid(True)
ax.legend()
# --- [1,0]: Pendel-Winkel alpha ---
ax = axes[1, 0]
ax.plot(t_vec[:n_actual+1], np.rad2deg(x_history[1, :n_actual+1]), label=r'$\theta_2$ Pendel', color='orange')
ax.axhline(np.rad2deg(x_ref[1]), linestyle='--', linewidth=1, color='orange', label=r'$\theta_{2,ref}$')
ax.set_xlabel('Zeit [s]')
ax.set_ylabel('Winkel [°]')
ax.set_title(r'Pendel-Winkel $\alpha$')
ax.grid(True)
ax.legend()
# --- [2,0]: Winkelgeschwindigkeiten ---
ax = axes[2, 0]
ax.plot(t_vec[:n_actual+1], x_history[2, :n_actual+1], label=r'$\dot{\theta}_1$ Arm')
ax.plot(t_vec[:n_actual+1], x_history[3, :n_actual+1], label=r'$\dot{\theta}_2$ Pendel')
ax.axhline(0.0, linestyle='--', linewidth=1, label='ref = 0')
ax.set_xlabel('Zeit [s]')
ax.set_ylabel('Winkelgeschwindigkeit [rad/s]')
ax.set_title('Winkelgeschwindigkeiten')
ax.grid(True)
ax.legend()
# ================= MITTLERE SPALTE =================
# --- [0,1]: Haupt-Solver Status ---
ax = axes[0, 1]
ax.step(t_u_vec[:n_actual], status_history[:n_actual], where='post', color='red', label='NLP Status')
ax.set_yticks(range(9))
ax.set_yticklabels([
'0: Success', '1: NaN', '2: MaxIter', '3: MinStep',
'4: QP Fail', '5: Ready', '6: Unbounded', '7: Timeout', '8: QP Bounds'
], fontsize=8)
ax.set_ylim(-0.5, 8.5)
ax.set_xlabel('Zeit [s]')
ax.set_ylabel('Status Code')
ax.set_title('Haupt-Solver Status (acados)')
ax.grid(True, axis='y', linestyle='--', alpha=0.7)
ax.legend(loc='upper right')
# --- [1,1]: QP Solver Status ---
ax = axes[1, 1]
ax.step(t_u_vec[:n_actual], qp_status_history[:n_actual], where='post', color='purple', label='QP Status')
ax.set_yticks([-1, 0, 1, 2, 3, 9])
ax.set_yticklabels([
'-1: Unknown', '0: Success', '1: NaN',
'2: MaxIter', '3: MinStep', '9: Infeasible'
], fontsize=8)
ax.set_ylim(-1.5, 9.5)
ax.set_xlabel('Zeit [s]')
ax.set_ylabel('QP Status Code')
ax.set_title('Unter-Solver Status (HPIPM)')
ax.grid(True, axis='y', linestyle='--', alpha=0.7)
ax.legend(loc='upper right')
# --- [2,1]: Timing ---
ax = axes[2, 1]
solver_arr = np.array(solver_time[:n_actual]) * 1000.0
twincat_arr = np.array(twincat_time[:n_actual]) * 1000.0
ax.plot(t_u_vec[:n_actual], T_arr * 1000.0, label='Loop time', color='steelblue', linewidth=1.2)
ax.plot(t_u_vec[:n_actual], solver_arr, label='Solver time', color='green', linewidth=1.2)
ax.plot(t_u_vec[:n_actual], twincat_arr, label='TwinCAT (ADS)', color='darkorange', linewidth=1.2)
ax.axhline(dt * 1000.0, linestyle='--', color='red', linewidth=1, label=f'dt = {dt*1000:.0f} ms (Deadline)')
ax.set_xlabel('Zeit [s]')
ax.set_ylabel('Zeit [ms]')
ax.set_title('Timing-Aufschlüsselung pro Schritt')
ax.grid(True)
ax.legend(fontsize=8)
# ================= RECHTE SPALTE =================
# --- [0,2]: Stellgröße Force ---
ax = axes[0, 2]
ax.step(t_u_vec[:n_actual], u_force, where='post', label='Force [N]', color='steelblue', linewidth=1.5)
ax.axhline( U * km, linestyle='--', color='red', linewidth=1, label='U_max')
ax.axhline(-U * km, linestyle='--', color='blue', linewidth=1, label='U_min')
ax.set_xlabel('Zeit [s]')
ax.set_ylabel('Force [N]')
ax.set_title('Stellgröße u (Force)')
ax.grid(True, alpha=0.4)
ax.legend(fontsize=8)
# --- [1,2]: Stellgröße Strom ---
ax = axes[1, 2]
ax.step(t_u_vec[:n_actual], u_current, where='post', label='Strom [A]', color='orange', linewidth=1.5)
ax.axhline( U, linestyle='--', color='red', linewidth=1, label='I_max')
ax.axhline(-U, linestyle='--', color='blue', linewidth=1, label='I_min')
ax.set_xlabel('Zeit [s]')
ax.set_ylabel('Strom [A]')
ax.set_title('Stellgröße u (Strom)')
ax.grid(True, alpha=0.4)
ax.legend(fontsize=8)
# --- [2,2]: Fehler-Plot (Winkel) mit Fallback-Markierung ---
ax = axes[2, 2]
ax.plot(t_u_vec[:n_actual], np.rad2deg(error_history[0, :n_actual]),
label=r'$e_{\theta}$ (Arm)', color='steelblue')
ax.plot(t_u_vec[:n_actual], np.rad2deg(error_history[1, :n_actual]),
label=r'$e_{\alpha}$ (Pendel)', color='orange')
ax.axhline(0, color='black', linestyle='--', linewidth=0.8)
# Fallback-Bereiche markieren
i = 0
while i < n_actual:
if fallback_mask[i]:
start = i
while i < n_actual and fallback_mask[i]:
i += 1
ax.axvspan(start * dt, i * dt, color='red', alpha=0.15, zorder=0)
else:
i += 1
from matplotlib.patches import Patch
fallback_patch = Patch(facecolor='red', alpha=0.15, label='Fallback aktiv')
ax.legend(handles=ax.get_legend_handles_labels()[0] + [fallback_patch], fontsize=8)
ax.set_xlabel('Zeit [s]')
ax.set_ylabel('Fehler [°]')
ax.set_title('Regelfehler (Winkel)')
ax.grid(True, alpha=0.3)
ax.legend(fontsize=8)
# ═════════════ ZEILE 4: EKF vs. Encoder Vergleich ═════════════
# --- [3,0]: Winkel (beide) — Encoder vs. EKF ---
ax = axes[3, 0]
ax.plot(t_vec[:n_actual+1], np.rad2deg(x_history[0, :n_actual+1]),
label=r'$\theta$ Encoder', color='steelblue', linewidth=1.5)
ax.plot(t_vec[:n_actual+1], np.rad2deg(x_ekf_history[0, :n_actual+1]),
label=r'$\theta$ EKF', color='steelblue',
linestyle='--', linewidth=1.2, alpha=0.7)
ax.plot(t_vec[:n_actual+1], np.rad2deg(x_history[1, :n_actual+1]),
label=r'$\alpha$ Encoder', color='orange', linewidth=1.5)
ax.plot(t_vec[:n_actual+1], np.rad2deg(x_ekf_history[1, :n_actual+1]),
label=r'$\alpha$ EKF', color='orange',
linestyle='--', linewidth=1.2, alpha=0.7)
ax.axhline(0, linestyle=':', linewidth=1, color='black', alpha=0.5)
ax.set_xlabel('Zeit [s]')
ax.set_ylabel('Winkel [°]')
ax.set_title('Winkel: Encoder vs. EKF')
ax.grid(True)
ax.legend(fontsize=7, ncol=2)
# --- [3,1]: ω1 (Arm) — Encoder vs. EKF ---
ax = axes[3, 1]
ax.plot(t_vec[:n_actual+1], omega_raw_history[0, :n_actual+1],
label='Encoder', color='steelblue', linewidth=2.0)
ax.plot(t_vec[:n_actual+1], x_ekf_history[2, :n_actual+1],
label='EKF', color='darkblue', linewidth=1.5, linestyle='--')
ax.axhline(0.0, linestyle=':', linewidth=1, color='red', alpha=0.5)
ax.set_xlabel('Zeit [s]')
ax.set_ylabel(r'$\dot{\theta}_1$ [rad/s]')
ax.set_title(r'Arm-Geschwindigkeit $\dot{\theta}_1$')
ax.grid(True)
ax.legend(fontsize=8)
# --- [3,2]: ω2 (Pendel) — Encoder vs. EKF ---
ax = axes[3, 2]
ax.plot(t_vec[:n_actual+1], omega_raw_history[1, :n_actual+1],
label='Encoder', color='orange', linewidth=2.0)
ax.plot(t_vec[:n_actual+1], x_ekf_history[3, :n_actual+1],
label='EKF', color='darkorange', linewidth=1.5, linestyle='--')
ax.axhline(0.0, linestyle=':', linewidth=1, color='red', alpha=0.5)
ax.set_xlabel('Zeit [s]')
ax.set_ylabel(r'$\dot{\theta}_2$ [rad/s]')
ax.set_title(r'Pendel-Geschwindigkeit $\dot{\theta}_2$')
ax.grid(True)
ax.legend(fontsize=8)
plt.tight_layout(rect=[0, 0, 1, 0.945]) # Platz für den Text oben freilassen
timestamp = datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
plt.savefig(rf"C:\Users\mz\Desktop\exp2-adip\Bilder\NMPC_control_pictures\nmpc_adip_{timestamp}.png", dpi=150, bbox_inches='tight')
plt.show()
SQP_RTI: QP solver returned error status 3 (ACADOS_MINSTEP) QP iteration 1.
Schritt 243: Solver Status = 4 -> Nutze Fallback! u_apply = 3.4999999999999996
Schritt 243 | Status=4 | input = 3.4999999999999996, u=3.4999999999999996| motorvalue= -1944 | θ1= +51.99 deg, θ2=+271.85 deg, ω1= -0.27 rad/s, ω2= -14.34 rad/s | e_θ= +51.99°, e_α= +91.85°,
SQP_RTI: QP solver returned error status 3 (ACADOS_MINSTEP) QP iteration 3.
Schritt 244: Solver Status = 4 -> Nutze Fallback! u_apply = 3.4999999999999996
Schritt 244 | Status=4 | input = 3.4999999999999996, u=3.4999999999999996| motorvalue= -1944 | θ1= +50.27 deg, θ2=+259.72 deg, ω1= -1.50 rad/s, ω2= -10.58 rad/s | e_θ= +50.27°, e_α= +79.72°,
SQP_RTI: QP solver returned error status 3 (ACADOS_MINSTEP) QP iteration 3.
Schritt 245: Solver Status = 4 -> Nutze Fallback! u_apply = 3.4999999999999996
Schritt 245 | Status=4 | input = 3.4999999999999996, u=3.4999999999999996| motorvalue= -1944 | θ1= +47.81 deg, θ2=+250.84 deg, ω1= -2.15 rad/s, ω2= -7.75 rad/s | e_θ= +47.81°, e_α= +70.84°,
SQP_RTI: QP solver returned error status 3 (ACADOS_MINSTEP) QP iteration 2.
Schritt 246: Solver Status = 4 -> Nutze Fallback! u_apply = 3.4999999999999996
Schritt 246 | Status=4 | input = 3.4999999999999996, u=3.4999999999999996| motorvalue= -1944 | θ1= +44.21 deg, θ2=+243.28 deg, ω1= -3.14 rad/s, ω2= -6.60 rad/s | e_θ= +44.21°, e_α= +63.28°,
SQP_RTI: QP solver returned error status 3 (ACADOS_MINSTEP) QP iteration 1.
Schritt 247: Solver Status = 4 -> Nutze Fallback! u_apply = 3.4999999999999996
Schritt 247 | Status=4 | input = 3.4999999999999996, u=3.4999999999999996| motorvalue= -1944 | θ1= +40.08 deg, θ2=+238.10 deg, ω1= -3.60 rad/s, ω2= -4.52 rad/s | e_θ= +40.08°, e_α= +58.10°,