Add comments/ delete spaces

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)