diff --git a/guided_mrmp/controllers/mpc.py b/guided_mrmp/controllers/mpc.py index dec7a8a1d961766361b221032ac4549a3fee56f8..91edd903c56df399f5a01b1e539a3679d8a456d2 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)