Python scipy.linalg.solve_discrete_are() Examples

def dlqr(a, b, q, r):
    """ Get the feedback controls from linearized system at the current time step

    for a discrete time system Ax+Bu
    find the infinite horizon optimal feedback controller
    to steer the system to the origin
    with
    u = -K*x
    """
    x = np.matrix(sLa.solve_discrete_are(a, b, q, r))

    k = np.matrix(sLa.inv(b.T * x * b + r) * (b.T * x * a))

    eigVals, eigVecs = sLa.eig(a - b * k)

    return np.asarray(k), np.asarray(x), eigVals 

def dare(A, B, Q, R, S=None, E=None, stabilizing=True):
    """ (X,L,G) = dare(A,B,Q,R) solves the discrete-time algebraic Riccati
    equation

        :math:`A^T X A - X - A^T X B (B^T X B + R)^{-1} B^T X A + Q = 0`

    where A and Q are square matrices of the same dimension. Further, Q
    is a symmetric matrix. The function returns the solution X, the gain
    matrix G = (B^T X B + R)^-1 B^T X A and the closed loop eigenvalues L,
    i.e., the eigenvalues of A - B G.

    (X,L,G) = dare(A,B,Q,R,S,E) solves the generalized discrete-time algebraic
    Riccati equation

        :math:`A^T X A - E^T X E - (A^T X B + S) (B^T X B + R)^{-1} (B^T X A + S^T) + Q = 0`

    where A, Q and E are square matrices of the same dimension. Further, Q and
    R are symmetric matrices. The function returns the solution X, the gain
    matrix :math:`G = (B^T X B + R)^{-1} (B^T X A + S^T)` and the closed loop
    eigenvalues L, i.e., the eigenvalues of A - B G , E.
    """
    if S is not None or E is not None or not stabilizing:
        return dare_old(A, B, Q, R, S, E, stabilizing)
    else:
        Rmat = _ssmatrix(R)
        Qmat = _ssmatrix(Q)
        X = solve_discrete_are(A, B, Qmat, Rmat)
        G = solve(B.T.dot(X).dot(B) + Rmat, B.T.dot(X).dot(A))
        L = eigvals(A - B.dot(G))
        return _ssmatrix(X), L, _ssmatrix(G) 

def control(self, arm, x_des=None):
        """Generates a control signal to move the 
        arm to the specified target.
            
        arm Arm: the arm model being controlled
        des list: the desired system position
        x_des np.array: desired task-space force, 
                        system goes to self.target if None
        """
        if self.u is None:
            self.u = np.zeros(arm.DOF)

        self.Q = np.zeros((arm.DOF*2, arm.DOF*2))
        self.Q[:arm.DOF, :arm.DOF] = np.eye(arm.DOF) * 1000.0 
        self.R = np.eye(arm.DOF) * 0.001 

        # calculate desired end-effector acceleration
        if x_des is None:
            self.x = arm.x 
            x_des = self.x - self.target 

        self.arm, state = self.copy_arm(arm)
        A, B = self.calc_derivs(state, self.u)

        if self.solve_continuous is True:
            X = sp_linalg.solve_continuous_are(A, B, self.Q, self.R)
            K = np.dot(np.linalg.pinv(self.R), np.dot(B.T, X))
        else: 
            X = sp_linalg.solve_discrete_are(A, B, self.Q, self.R)
            K = np.dot(np.linalg.pinv(self.R + np.dot(B.T, np.dot(X, B))), np.dot(B.T, np.dot(X, A)))

        # transform the command from end-effector space to joint space
        J = self.arm.gen_jacEE()

        u = np.hstack([np.dot(J.T, x_des), arm.dq])

        self.u = -np.dot(K, u)

        if self.write_to_file is True:
            # feed recorders their signals
            self.u_recorder.record(0.0, self.u)
            self.xy_recorder.record(0.0, self.x)
            self.dist_recorder.record(0.0, self.target - self.x)

        # add in any additional signals 
        for addition in self.additions:
            self.u += addition.generate(self.u, arm)

        return self.u 

def test_random_vs_lqr(self):
        """
        Test random actions vs. a LQR controller. LQR controller should perform
        much better than random actions in the linear dynamics environment.
        """
        env = EnvFactory.make("LinearDynamics-v0")
        num_test_episodes = 500

        def random_policy(env, state):
            return np.random.uniform(
                env.action_space.low, env.action_space.high, env.action_dim
            )

        def lqr_policy(env, state):
            # Four matrices that characterize the environment
            A, B, Q, R = env.A, env.B, env.Q, env.R
            # Solve discrete algebraic Riccati equation:
            M = linalg.solve_discrete_are(A, B, Q, R)
            K = np.dot(
                linalg.inv(np.dot(np.dot(B.T, M), B) + R), (np.dot(np.dot(B.T, M), A))
            )
            state = state.reshape((-1, 1))
            action = -K.dot(state).squeeze()
            return action

        mean_acc_rws_random = self.run_n_episodes(env, num_test_episodes, random_policy)
        mean_acc_rws_lqr = self.run_n_episodes(env, num_test_episodes, lqr_policy)
        logger.info(f"Mean acc. reward of random policy: {mean_acc_rws_random}")
        logger.info(f"Mean acc. reward of LQR policy: {mean_acc_rws_lqr}")
        assert mean_acc_rws_lqr > mean_acc_rws_random 

def test_are_validate_args():

    def test_square_shape():
        nsq = np.ones((3, 2))
        sq = np.eye(3)
        for x in (solve_continuous_are, solve_discrete_are):
            assert_raises(ValueError, x, nsq, 1, 1, 1)
            assert_raises(ValueError, x, sq, sq, nsq, 1)
            assert_raises(ValueError, x, sq, sq, sq, nsq)
            assert_raises(ValueError, x, sq, sq, sq, sq, nsq)

    def test_compatible_sizes():
        nsq = np.ones((3, 2))
        sq = np.eye(4)
        for x in (solve_continuous_are, solve_discrete_are):
            assert_raises(ValueError, x, sq, nsq, 1, 1)
            assert_raises(ValueError, x, sq, sq, sq, sq, sq, nsq)
            assert_raises(ValueError, x, sq, sq, np.eye(3), sq)
            assert_raises(ValueError, x, sq, sq, sq, np.eye(3))
            assert_raises(ValueError, x, sq, sq, sq, sq, np.eye(3))

    def test_symmetry():
        nsym = np.arange(9).reshape(3, 3)
        sym = np.eye(3)
        for x in (solve_continuous_are, solve_discrete_are):
            assert_raises(ValueError, x, sym, sym, nsym, sym)
            assert_raises(ValueError, x, sym, sym, sym, nsym)

    def test_singularity():
        sing = 1e12 * np.ones((3, 3))
        sing[2, 2] -= 1
        sq = np.eye(3)
        for x in (solve_continuous_are, solve_discrete_are):
            assert_raises(ValueError, x, sq, sq, sq, sq, sing)

        assert_raises(ValueError, solve_continuous_are, sq, sq, sq, sing)

    def test_finiteness():
        nm = np.ones((2, 2)) * np.nan
        sq = np.eye(2)
        for x in (solve_continuous_are, solve_discrete_are):
            assert_raises(ValueError, x, nm, sq, sq, sq)
            assert_raises(ValueError, x, sq, nm, sq, sq)
            assert_raises(ValueError, x, sq, sq, nm, sq)
            assert_raises(ValueError, x, sq, sq, sq, nm)
            assert_raises(ValueError, x, sq, sq, sq, sq, nm)
            assert_raises(ValueError, x, sq, sq, sq, sq, sq, nm) 

def control_systems(request):
    ct_sys, ref = request.param
    Ac, Bc, Cc = ct_sys.data
    Dc = np.zeros((Cc.shape[0], 1))

    Q = np.eye(Ac.shape[0])
    R = np.eye(Bc.shape[1] if len(Bc.shape) > 1 else 1)

    Sc = linalg.solve_continuous_are(Ac, Bc.reshape(-1, 1), Q, R,)
    Kc = linalg.solve(R, Bc.T @ Sc).reshape(1, -1)
    ct_ctr = LTISystem(Kc)

    evals = np.sort(np.abs(
        linalg.eig(Ac, left=False, right=False, check_finite=False)
    ))
    dT = 1/(2*evals[-1])

    Tsim = (8/np.min(evals[~np.isclose(evals, 0)])
            if np.sum(np.isclose(evals[np.nonzero(evals)], 0)) > 0
            else 8
            )

    dt_data = signal.cont2discrete((Ac, Bc.reshape(-1, 1), Cc, Dc), dT)
    Ad, Bd, Cd, Dd = dt_data[:-1]
    Sd = linalg.solve_discrete_are(Ad, Bd.reshape(-1, 1), Q, R,)
    Kd = linalg.solve(Bd.T @ Sd @ Bd + R, Bd.T @ Sd @ Ad)

    dt_sys = LTISystem(Ad, Bd, dt=dT)
    dt_sys.initial_condition = ct_sys.initial_condition
    dt_ctr = LTISystem(Kd, dt=dT)

    yield ct_sys, ct_ctr, dt_sys, dt_ctr, ref, Tsim 

def solve(env):
  """Returns the optimal value and policy for LQR problem.

  Args:
    env: An instance of `control.EnvironmentV2` with LQR level.

  Returns:
    p: A numpy array, the Hessian of the optimal total cost-to-go (value
      function at state x) is V(x) = .5 * x' * p * x.
    k: A numpy array which gives the optimal linear policy u = k * x.
    beta: The maximum eigenvalue of (a + b * k). Under optimal policy, at
      timestep n the state tends to 0 like beta^n.

  Raises:
    RuntimeError: If the controlled system is unstable.
  """
  n = env.physics.model.nq  # number of DoFs
  m = env.physics.model.nu  # number of controls

  # Compute the mass matrix.
  mass = np.zeros((n, n))
  wrapper.mjbindings.mjlib.mj_fullM(env.physics.model.ptr, mass,
                                    env.physics.data.qM)

  # Compute input matrices a, b, q and r to the DARE solvers.
  # State transition matrix a.
  stiffness = np.diag(env.physics.model.jnt_stiffness.ravel())
  damping = np.diag(env.physics.model.dof_damping.ravel())
  dt = env.physics.model.opt.timestep

  j = np.linalg.solve(-mass, np.hstack((stiffness, damping)))
  a = np.eye(2 * n) + dt * np.vstack(
      (dt * j + np.hstack((np.zeros((n, n)), np.eye(n))), j))

  # Control transition matrix b.
  b = env.physics.data.actuator_moment.T
  bc = np.linalg.solve(mass, b)
  b = dt * np.vstack((dt * bc, bc))

  # State cost Hessian q.
  q = np.diag(np.hstack([np.ones(n), np.zeros(n)]))

  # Control cost Hessian r.
  r = env.task.control_cost_coef * np.eye(m)

  # Solve the discrete algebraic Riccati equation.
  p = scipy_linalg.solve_discrete_are(a, b, q, r)
  k = -np.linalg.solve(b.T.dot(p.dot(b)) + r, b.T.dot(p.dot(a)))

  # Under optimal policy, state tends to 0 like beta^n_timesteps
  beta = np.abs(np.linalg.eigvals(a + b.dot(k))).max()
  if beta >= 1.0:
    raise RuntimeError('Controlled system is unstable.')
  return p, k, beta