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