From 540bd5a68f5df2c4ce46cf7569b26e9046c14894 Mon Sep 17 00:00:00 2001
From: rachelmoan <moanrachel516@gmail.com>
Date: Thu, 2 Jan 2025 13:14:16 -0600
Subject: [PATCH] Add comments/ delete spaces
---
guided_mrmp/controllers/mpc.py | 8 ++++++--
1 file changed, 6 insertions(+), 2 deletions(-)
diff --git a/guided_mrmp/controllers/mpc.py b/guided_mrmp/controllers/mpc.py
index dec7a8a..91edd90 100644
--- a/guided_mrmp/controllers/mpc.py
+++ b/guided_mrmp/controllers/mpc.py
@@ -2,8 +2,6 @@ import numpy as np
import casadi as ca
from guided_mrmp.optimizer import Optimizer
-np.seterr(divide="ignore", invalid="ignore")
-
class MPC:
def __init__(self, model, T, DT, state_cost, final_state_cost, input_cost, input_rate_cost, settings):
"""
@@ -61,11 +59,17 @@ class MPC:
# Cost function
cost = 0
for k in range(self.control_horizon):
+
+ # difference between the current state and the target state
cost += ca.mtimes([(X[:, k+1] - target[:, k]).T, self.Q, X[:, k+1] - target[:, k]])
+
+ # control effort
cost += ca.mtimes([U[:, k].T, self.R, U[:, k]])
if k > 0:
+ # Penalize large changes in control
cost += ca.mtimes([(U[:, k] - U[:, k-1]).T, self.P, U[:, k] - U[:, k-1]])
+ # Final state cost
cost += ca.mtimes([(X[:, -1] - target[:, -1]).T, self.Qf, X[:, -1] - target[:, -1]])
opti.minimize(cost)
--
GitLab