def compute_controls(self, wx=1., wu=1e-3):
"""
Compute the series of controls that minimizes the preview QP.
Parameters
----------
wx : scalar, optional
Weight on (cumulated or terminal) state costs.
wu : scalar, optional
Weight on cumulated control costs.
Note
----
This function should be called after ``compute_dynamics()``.
"""
assert self.controller is not None, "Call compute_dynamics() first"
wu = VectorXd.Ones(self.u_dim) * wu
wx = VectorXd.Ones(self.x_dim) * wx
self.controller.weights(wx, wu)
ret = self.controller.solve()
if not ret:
raise Exception("MPC failed to solve QP")
U = VectorXd_to_array(self.controller.control())
self.U = U.reshape((self.nb_steps, self.u_dim))
self.solve_time = self.controller.solveTime().wall # in [ns]
self.solve_and_build_time = self.controller.solveAndBuildTime().wall
self.solve_time *= 1e-9 # in [s]
self.solve_and_build_time *= 1e-9 # in [s]
def __init__(self,
A,
B,
x_init,
x_goal,
nb_steps,
C=None,
d=None,
solver=SolverFlag.QuadProgDense):
self.A = array_to_MatrixXd(A)
self.B = array_to_MatrixXd(B)
self.C = array_to_MatrixXd(C)
self.c = VectorXd.Zero(A.shape[0]) # no bias term for now
self.controller = None
self.d = array_to_VectorXd(d)
self.nb_steps = nb_steps
self.ps = None
self.solver = solver
self.u_dim = B.shape[1]
self.x_dim = A.shape[1]
self.x_goal = array_to_VectorXd(x_goal)
self.x_init = array_to_VectorXd(x_init)
def array_to_VectorXd(v):
V = VectorXd.Zero(v.shape[0])
for i in xrange(v.shape[0]):
V[i] = v[i]
return V