Python scipy.linalg.solve_continuous_are() Examples

def LQR(v_target, wheelbase, Q, R):
    # print(v_target,wheelbase,Q,R)
    A= np.matrix([[0, v_target*(5./18.)], [0, 0]])
    B = np.matrix([[0], [(v_target/wheelbase)*(5./18.)]])
    V = np.matrix(linalg.solve_continuous_are(A, B, Q, R))
    K = np.matrix(linalg.inv(R) * (B.T * V))
    return K

# create the Robot instance. 

def build_care(feedback_linearizable_output, Q):
        """Build a quadratic CLF from a FeedbackLinearizableOutput with auxilliary control gain matrix, by solving the continuous algebraic Riccati equation (CARE).

        CARE is

        F'P + PF - PGG'P = -Q

        for specified Q.

        Outputs a QuadraticControlLyapunovFunction.

        Inputs:
        Positive definite matrix for CTLE, Q: numpy array (p, p)
        """

        F = feedback_linearizable_output.F
        G = feedback_linearizable_output.G
        R = identity(G.shape[1])
        P = solve_continuous_are(F, G, Q, R)
        alpha = min(eigvals(Q)) / max(eigvals(P))
        return QuadraticControlLyapunovFunction(feedback_linearizable_output, P, alpha) 

def continuous_LQR(speed, Q, R, wheelbase=2.995):
    A= np.matrix([[0, speed], [0, 0]])
    B = np.matrix([[0], [(speed/wheelbase)]])
    V = np.matrix(linalg.solve_continuous_are(A, B, Q, R))
    K = np.matrix(linalg.inv(R) * (B.T * V))
    return K 

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_solve_generalized_continuous_are():
    cases = [
        # Two random examples differ by s term
        # in the absence of any literature for demanding examples.
        (np.array([[2.769230e-01, 8.234578e-01, 9.502220e-01],
                   [4.617139e-02, 6.948286e-01, 3.444608e-02],
                   [9.713178e-02, 3.170995e-01, 4.387444e-01]]),
         np.array([[3.815585e-01, 1.868726e-01],
                   [7.655168e-01, 4.897644e-01],
                   [7.951999e-01, 4.455862e-01]]),
         np.eye(3),
         np.eye(2),
         np.array([[6.463130e-01, 2.760251e-01, 1.626117e-01],
                   [7.093648e-01, 6.797027e-01, 1.189977e-01],
                   [7.546867e-01, 6.550980e-01, 4.983641e-01]]),
         np.zeros((3, 2)),
         None),
        (np.array([[2.769230e-01, 8.234578e-01, 9.502220e-01],
                   [4.617139e-02, 6.948286e-01, 3.444608e-02],
                   [9.713178e-02, 3.170995e-01, 4.387444e-01]]),
         np.array([[3.815585e-01, 1.868726e-01],
                   [7.655168e-01, 4.897644e-01],
                   [7.951999e-01, 4.455862e-01]]),
         np.eye(3),
         np.eye(2),
         np.array([[6.463130e-01, 2.760251e-01, 1.626117e-01],
                   [7.093648e-01, 6.797027e-01, 1.189977e-01],
                   [7.546867e-01, 6.550980e-01, 4.983641e-01]]),
         np.ones((3, 2)),
         None)
        ]

    min_decimal = (10, 10)

    def _test_factory(case, dec):
        """Checks if X = A'XA-(A'XB)(R+B'XB)^-1(B'XA)+Q) is true"""
        a, b, q, r, e, s, knownfailure = case
        if knownfailure:
            pytest.xfail(reason=knownfailure)

        x = solve_continuous_are(a, b, q, r, e, s)
        res = a.conj().T.dot(x.dot(e)) + e.conj().T.dot(x.dot(a)) + q
        out_fact = e.conj().T.dot(x).dot(b) + s
        res -= out_fact.dot(solve(np.atleast_2d(r), out_fact.conj().T))
        assert_array_almost_equal(res, np.zeros_like(res), decimal=dec)

    for ind, case in enumerate(cases):
        _test_factory(case, min_decimal[ind]) 

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