Пример #1
0
    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]
Пример #2
0
 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)
Пример #3
0
def array_to_VectorXd(v):
    V = VectorXd.Zero(v.shape[0])
    for i in xrange(v.shape[0]):
        V[i] = v[i]
    return V