def testLQR(self, siso):
"""Call lqr()"""
(K, S, E) = lqr(siso.ss1.A, siso.ss1.B, np.eye(2), np.eye(1))
# Should work if [Q N;N' R] is positive semi-definite
(K, S, E) = lqr(siso.ss2.A, siso.ss2.B, 10 * np.eye(3), np.eye(1),
[[1], [1], [2]])
def newtonKleinman(A, B, Q, K, N):
assert ((la.eig(A - B * K)[0]) < 0).min(
), 'initial gain K is not stabilizing, Newton-Kleinman is not guaranteed to converge!'
Kopt, Popt, Eopt = mtl.lqr(A, B, Q, 1)
assert (
(Eopt < 0).min() or ((la.eig(Popt)[0]) > 0).min()
), 'There is no stabilizing solution to Riccati equation for provided (A,B,Q,R), your efforts are futile!'
# P that corresponds to initial gain K
P = mtl.lyap((A - np.dot(B, K)).transpose(),
(Q + np.dot(K.transpose(), K)))
assert ((la.eig(P)[0]) > 0).min(), 'P iterate is not positive definite!'
errorP = []
errorP.append(la.norm(P - Popt))
for i in range(N):
'''Newton-Kleinman is repeated Lyapunov equation that yields iterates P. Each P that constitutes Lyapunov function x'Px '''
P = mtl.lyap((A - np.dot(B, np.dot(B.transpose(), P))).transpose(),
(Q + np.dot(
np.dot(B.transpose(), P).transpose(),
np.dot(B.transpose(), P))))
assert ((la.eig(P)[0]) >
0).min(), 'P iterate is not positive definite!'
errorP.append(la.norm(P - Popt))
plt.plot(errorP, 'o')
plt.grid()
plt.show()
pass
def calc_input(self, pendulum, reference_z=None):
"""
Parameters
-------------
pendulum : pendulum class
Returns
----------
f : float in [N]
input of the system
"""
# freeze the state
self._freeze_state(pendulum)
self._freeze_weight(pendulum)
K, P, e = lqr(self.A, self.B, self.Q, self.R)
state = np.array([[pendulum.z], [pendulum.th], [pendulum.v_z], [pendulum.v_th]])
if reference_z is not None:
state = np.array([[pendulum.z - reference_z], [pendulum.th], [pendulum.v_z], [pendulum.v_th]])
f = - np.dot(K, state)
return f
def control_input(Alpha, Q, R, X, t): # Generate feasible trajectory Xd, Ud = feasible_trajectory(Alpha, t) x, y, theta = Xd omega_L, omega_R = Ud x = float(x) y = float(y) theta = float(theta) omega_L = float(omega_L) omega_R = float(omega_R) # Find controller using LQR at time t A = np.array([[0, 0, -r/2*np.sin(theta)*(omega_L+omega_R)], [0, 0, r/2*np.cos(theta)*(omega_L+omega_R)], [0, 0, 0]]) B = np.array([[r/2*np.cos(theta), r/2*np.cos(theta)], [r/2*np.sin(theta), r/2*np.sin(theta)], [-r/L, r/L]]) K, _, _ = mt.lqr(A, B, Q, R) # Compute control law U = Ud - K@(X - Xd) # Add input noise # U += np.random.uniform(-1.0, 1.0, size=(2, 1))*25 # Limit the maximum voltage input U = np.clip(U, -50, 50) return U
def __init__(self, pendulum):
# controllers
alpha_0 = pendulum.p_j * (
pendulum.p_m +
pendulum.c_m) + pendulum.c_m * pendulum.p_m * (pendulum.p_l**2)
inerter_mass = pendulum.p_j + pendulum.p_m * (pendulum.p_l**2)
mass = pendulum.p_m + pendulum.c_m
self.A = np.array([[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0],
[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]])
self.A[2, 1] = -(pendulum.p_m**2) * (pendulum.p_l**
2) * pendulum.g / alpha_0
self.A[2, 2] = -pendulum.c_mu * inerter_mass / alpha_0
self.A[2, 3] = pendulum.p_mu * pendulum.p_m * pendulum.p_l / alpha_0
self.A[3,
1] = mass * pendulum.p_m * pendulum.p_l * pendulum.g / alpha_0
self.A[3, 2] = pendulum.c_mu * pendulum.p_m * pendulum.p_l / alpha_0
self.A[3, 3] = -pendulum.p_mu * mass / alpha_0
self.B = np.array([[0.0], [0.0], [0.0], [0.0]])
self.B[2, 0] = inerter_mass / alpha_0
self.B[3, 0] = -pendulum.p_m * pendulum.p_l / alpha_0
self.C = np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0]])
self.R = 100.0
self.Q = np.diag([10, 10, 10, 10])
self.K, self.P, self.e = lqr(self.A, self.B, self.Q, self.R)
def control_input(Alpha, Q, R, X, t):
# Generate feasible trajectory
Xd, Ud = feasible_trajectory(Alpha, t)
x, y, theta, omega_L, omega_R = Xd
V_L, V_R = Ud
x = float(x)
y = float(y)
theta = float(theta)
omega_L = float(omega_L)
omega_R = float(omega_R)
# Find controller using LQR at time t
# Combine a transfer function from Voltage to omega
# with ss representation from omega to state
A = np.array([[
0, 0, -r / 2 * np.sin(theta) * (omega_L + omega_R),
r / 2 * np.cos(theta), r / 2 * np.cos(theta)
],
[
0, 0, r / 2 * np.cos(theta) * (omega_L + omega_R),
r / 2 * np.sin(theta), r / 2 * np.sin(theta)
], [0, 0, 0, -r / L, r / L], [0, 0, 0, -1 / tau, 0],
[0, 0, 0, 0, -1 / tau]])
B = np.array([[0, 0], [0, 0], [0, 0], [K_m / tau, 0], [0, K_m / tau]])
# Check controllability and find controller using LQR only if controllable
AB = np.zeros((A.shape[0], B.shape[1] * (A.shape[0] - 1)))
for i in range(1, A.shape[0]):
AB[:, i:i + 2] = np.linalg.matrix_power(A, i) @ B
ctrb = np.hstack((B, AB))
rank_ctrb = np.sum(np.linalg.svd(ctrb, compute_uv=False) > 1e-10)
K = np.zeros((2, 5))
if rank_ctrb == 5:
K, _, _ = mt.lqr(A, B, Q, R)
# Compute control law
U = Ud - K @ (X - Xd)
# Add input noise
# U += np.random.uniform(-2.0, 2.0, size=(2, 1))
# Limit the maximum voltage input
U = np.clip(U, -12, 12)
return U
def control_input(Alpha, Q, R, X, t):
# Generate feasible trajectory
Xd, Ud = feasible_trajectory(Alpha, t)
x, y, theta = Xd
omega_L, omega_R = Ud
x = float(x)
y = float(y)
theta = float(theta)
omega_L = float(omega_L)
omega_R = float(omega_R)
# Find controller using LQR at time t
A = np.array([[0, 0, -r / 2 * np.sin(theta) * (omega_L + omega_R)],
[0, 0, r / 2 * np.cos(theta) * (omega_L + omega_R)],
[0, 0, 0]])
B = np.array([[r / 2 * np.cos(theta), r / 2 * np.cos(theta)],
[r / 2 * np.sin(theta), r / 2 * np.sin(theta)],
[-r / L, r / L]])
K, _, _ = mt.lqr(A, B, Q, R)
# Compute control law
U = Ud - K @ (X - Xd)
return U
w1_offset_eql = 0
th2_offset_eql = -30
w2_offset_eql = 0
A_lin = np.matrix(A_Lin.A_lin(M1, M2, L1, L2, G, state_eql[0], state_eql[1],
state_eql[2], state_eql[3]),
dtype=np.float)
B_lin = np.matrix([0, 1, 0, 0]).T
C_lin = np.matrix(np.identity(len(A_lin)))
D_lin = np.matrix(np.zeros_like(B_lin))
#sys = ctr.ss(A_lin, B_lin, C_lin, D_lin)
sys = ctr.StateSpace(A_lin, B_lin, C_lin, D_lin)
Q = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1000, 0], [0, 0, 0, 1000]])
R = 1
K, S, E = ctr.lqr(sys, Q, R)
def derivs(state, t):
if Control == True:
T_1 = -K.dot(state - state_eql)
T_1 = T_1[0]
elif Control == False:
T_1 = 0
dydx = np.zeros_like(state)
dydx[0] = state[1]
delta = state[2] - state[0]
den1 = (M1 + M2) * L1 - M2 * L1 * cos(delta) * cos(delta)
dydx[1] = ((M2 * L1 * state[1] * state[1] * sin(delta) * cos(delta) +
'''3D case '''
if case == '3D':
A = np.array([[-1, 2, -1], [2.2, 1.7, -1], [0.5, 0.7, -1]])
A1 = np.array([[-1, 2, -1], [2.2, 1.7, -1], [0.5, 0.7, -1]])
eigs = la.eig(A)
eval = eigs[0]
evec = eigs[1]
print('eigen values of A:\n', eval)
B = np.array([[2], [1.6], [3]])
B1 = np.array([[2], [1.6], [0.9]])
Q = np.array([[5, 0, 0], [0, 8, 0], [0, 0, 1]])
q = np.random.rand(3, 3)
Qrand = np.dot(q.transpose(), q)
#K0,X,E = mtl.lqr(A,B, Qrand,1)
K0, X, E = mtl.lqr(A, B, Q, 1)
#eigs = la.eig(A-np.dot(B,K0)); eval = eigs[0]; evec = eigs[1]; print('eigen values of Acl:\n', eval)
#newtonKleinman(A, B, Q, K0, 10)
#inverseReinfLearning1(A, B, Q, 10)
algorithmProperties = {
'updateP': 'Ricc',
'alpha': 1.0,
'isQDiagonal': False
}
inverseReinfLearning(A, B, Q, 30, p=algorithmProperties, debugMode=True)
#inverseReinfLearning_Lyap1(A,B,Q,50)
print('Main finished')
Cx = np.cos(x[2])
D = m * L * L * (M + m * (1 - Cx**2))
dx = np.zeros(4)
dx[0] = x[1]
dx[1] = (1 / D) * (-(m**2) * (L**2) * g * Cx * Sx + m * (L**2) *
(m * L *
(x[3]**2) * Sx - d * x[1])) + m * L * L * (1 / D) * u
dx[2] = x[3]
dx[3] = (1 / D) * ((m + M) * m * g * L * Sx - m * L * Cx *
(m * L *
(x[3]**2) * Sx - d * x[1])) - m * L * Cx * (1 / D) * u
return dx
#%% Traditional LQR
K = lqr(A, B, Q, R)[0]
#%%
x0 = np.array([[-1], [0], [np.pi], [0]])
perturbation = np.array([
[0],
[0],
[np.random.normal(0, 0.1)], # np.random.normal(0, 0.05)
[0]
])
x = x0 + perturbation
w_r = np.array([[1], [0], [np.pi], [0]])
action_range = np.arange(-500, 500)
def velocity(self):
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=100)
rospy.init_node('controller_kay')
rate = rospy.Rate(100) # 10hz
#controller gains
alpha_v = 0.4
alpha_w = 0.4
lambda_v = 0.2
lambda_w = 0.2
kv1 = 0.1
kv0 = 0.05
kw1 = 0.01
kw0 = 1
pi = np.pi
dt = 0.012
while not rospy.is_shutdown():
#xr = controller.path[0]
#yr = controller.path[1]
#theta_r = controller.path[2]
#refVel = controller.path[3]
#refOmega = controller.path[4]
curPose = [
controller.xs, controller.ys,
np.mod(controller.yaw, 6.28), controller.vs, controller.ws
]
refPose = controller.path[:3]
refVel = np.matrix(controller.path[-2:])
t = (np.matrix([refPose]) - np.matrix([curPose[:3]]))
t = np.transpose(t)
x = t[2, 0]
if (-3.14 > x >= -6.28):
x = x + 6.28
t[2, 0] = x
elif 3.14 < x <= 6.28:
x = x - 6.28
t[2, 0] = x
p_e = np.matrix([[math.cos(curPose[2]),
math.sin(curPose[2]), 0],
[-math.sin(curPose[2]),
math.cos(curPose[2]), 0], [0, 0, 1]]) * t
# kayacan tracking controller
A = np.matrix([[0, curPose[4], 0], [-curPose[4], 0, refVel[0, 0]],
[0, 0, 0]])
B = np.matrix([[1, 0], [0, 0], [0, 1]])
Q = np.diag([1000, 1000, 5])
#[1000,1000,0.0005]
R = np.diag([100, 1])
#100,0.1
K = lqr(A, B, Q, R)
u_b = K[0] * p_e
#Forward learning rate controller
e1_dot = curPose[4] * p_e[1] - curPose[3] + refVel[0, 0]
e2_dot = -curPose[4] * p_e[0] + refVel[0, 0] * p_e[2]
e2_dot2 = -p_e[1] * refVel[0, 1]**2 + refVel[
0, 1] * curPose[3] - refVel[0, 0] * curPose[4]
kv1_dot = alpha_v * refVel[0, 0] * (e1_dot + lambda_v * p_e[0])
kv0_dot = alpha_v * (e1_dot + lambda_v * p_e[0])
kw1_dot = alpha_w * refVel[0, 0] * refVel[0, 1] * (
e2_dot2 + 2 * lambda_w * e2_dot + lambda_w**2 * p_e[1])
kw0_dot = alpha_w * refVel[0, 0] * (
e2_dot2 + 2 * lambda_w * e2_dot + lambda_w**2 * p_e[1])
kv1 = kv1 + kv1_dot * dt
kv0 = kv0 + kv0_dot * dt
kw1 = kw1 + kw1_dot * dt
kw0 = kw0 + kw0_dot * dt
vf = refVel[0, 0] * kv1 + kv0
wf = refVel[0, 1] * kw1 + kw0
v = u_b[0, 0] + vf
w = u_b[1, 0] + wf
v = min(0.3, abs(v))
w = min(max(-3.5, w), 3.5)
vel_msg = Twist()
vel_msg.linear.x = v
vel_msg.angular.z = w
print(t[2, 0], p_e[2, 0])
#rospy.loginfo(vel_msg)
pub.publish(vel_msg)
rate.sleep()
def LTI_CLQE(system,saveComputation):
A = system.A
B = system.B
C = system.C
G = system.G
g = system.g
tol = system.tol
size_x = A.shape[1]
size_u = B.shape[1]
size_y = C.shape[0]
size_l = G.shape[0]
G = svd_suit(G, tol)
N = G.null
R = G.row_space #E = [N, R]
##################################################################
# Derivative-State Constraints and Effective States
#D1*dx/dt + D2*x = 0
dof = int(size_x/2)
D1 = np.hstack([np.eye(dof), np.zeros((dof,dof))])
D2 = np.hstack([np.zeros((dof,dof)), np.eye(dof)])
GG = vertcat(horzcat(G.M, np.zeros((size_l, size_x))),horzcat(D1, D2))
GG = svd_suit(GG, tol)
NA = svd_suit(N.T@A,tol)
temp = svd_suit(np.hstack([NA.null,N,GG.row_space[size_x:,:]]),tol)
R_used = temp.left_null
size_z = N.shape[1]
size_zeta = R_used.shape[1]
size_chi = size_z+size_zeta
E = np.hstack([N, R_used])
N1 = np.hstack([N, np.zeros((size_x, size_zeta))])
##################################################################
class OutStruct:
def __init__(self):
self.matrix_chi = None
self.matrix_u= None
self.matrix_y= None
self.vector = None
self.map= None
class Output:
def __init__(self):
self.sizes = None
self.initialConditions = None
self.desired = None
self.closed_loop = None
self.matrices = None
self.observer = OutStruct()
self.controller = OutStruct()
output = Output()
output.sizes = {'size_x': size_x, 'size_u': size_u, 'size_y': size_y, 'size_l': size_l,
'size_z': size_z, 'size_zeta': size_zeta, 'size_xi': size_chi}
##################################################################
if system.controllerSettings is not None:
if system.controllerSettings.Q.shape[1] == 1:
costQ = np.diag(system.controllerSettings.Q[:size_z,:].squeeze())
else:
costQ = np.diag(system.controllerSettings.Q)
if system.controllerSettings.R.shape[1] == 1:
costR = np.diag(system.controllerSettings.R[:size_z].squeeze())
else:
costR = np.diag(system.controllerSettings.R)
Kz = lqr(N.T@A@N, N.T@B, costQ, costR)[0]
else:
p = system.controllerSettings.poles[:N.shape[1]]
Kz = place(N.T@A@N, N.T@B, p)
if system.observerSettings is not None:
if system.observerSettings.Q.shape[1] == 1:
costQ = np.diag(system.observerSettings.Q[:size_z].squeeze())
else:
costQ = np.diag(system.observerSettings.Q)
if system.observerSettings.R.shape[1] == 1:
costR = np.diag(system.observerSettings.R[:size_z].squeeze())
else:
costR = np.diag(system.observerSettings.R)
L = lqr((N1.T@A@E).T, [email protected], costQ, costR)[0]
else:
p = system.observerSettings.poles[:E.shape[1]]
L = place((N1.T@A@E).T, [email protected], p)
L = L.T
Kzeta = np.linalg.pinv(N.T@B) @ N.T@A@R_used
K = np.hstack((Kz, Kzeta))
##################################################################
#observer structure
output.controller.matrix_chi = K
output.observer.matrix_chi = N1.T@A@E - L@C@E
output.observer.matrix_u = N1.T@B
output.observer.matrix_y = L
output.observer.vector = N1.T@g
output.observer.map = E
output.matrices = {'G': G, 'N': N, 'R': R, 'R_used': R_used, 'E': E,
'Kz': Kz, 'Kzeta': Kzeta, 'K': K, 'L': L,
'N1': N1}
if saveComputation==2:
return output
##################################################################
### desired
class Desired:
class Derivation:
def __init__(self):
self.NB = None
self.NB_projector= None
self.M = None
self.zdz_corrected = None
def __init__(self):
self.x = None
self.dx = None
self.z = None
self.dz = None
self.z_corrected= None
self.dz_corrected = None
self.u_corrected = None
self.derivation = Desired.Derivation()
desired = Desired()
desired.x = system.x_desired
desired.dx = system.dx_desired
desired.z = [email protected]
desired.dz = [email protected]
desired.derivation.NB = svd_suit(N.T@B,tol)
desired.derivation.NB_projector = desired.derivation.NB.left_null @ desired.derivation.NB.left_null.T
desired.derivation.M = svd_suit(np.hstack([[email protected]@A@N, desired.derivation.NB_projector]), tol)
desired.derivation.zdz_corrected = [email protected][email protected]@g + [email protected] @ np.vstack([desired.z, desired.dz])
desired.z_corrected = desired.derivation.zdz_corrected[:size_z]
desired.dz_corrected = desired.derivation.zdz_corrected[size_z:]
if np.linalg.norm( [email protected]_null.T@
(desired.dz_corrected - N.T@A@[email protected]_corrected - N.T@g) ) < tol:
desired.u_corrected = desired.derivation.NB.pinv @ (desired.dz_corrected - N.T@A@[email protected]_corrected - N.T@g)
output.controller.vector = desired.u_corrected
else:
raise Warning('cannot create a node')
if saveComputation == 1:
output.desired = desired
return output
##################################################################
### IC
class InitialConditions:
def __init__(self):
self.x
self.z
self.zeta
self.chiEstimateRandom
x_initial = system.x_initial
initialConditions = InitialConditions()
initialConditions.x = x_initial
initialConditions.z = N.T@x_initial
initialConditions.zeta = R_used.T@x_initial
InitialConditions.chi_estimate_random = 0.01*np.random.randn(size_chi, 1)
output.initialConditions = initialConditions
zeta = initialConditions.zeta
u_des = desired.u_corrected
z_des = desired.z_corrected
x_des = N@z_des + R_used@zeta
desired.zeta_corrected = zeta
desired.x_corrected = x_des
output.desired = desired
##################################################################
### Projected LTI in z-chi coordinates
class ClosedLoop:
class Coords:
def __init__(self):
self.matrix = None
self.vector = None
self.ode_func = None
self.Y0 = None
self.M = None
def __init__(self):
self.z_chi = ClosedLoop.Coords()
self.x_chi = ClosedLoop.Coords()
closed_loop = ClosedLoop()
closed_loop.z_chi.matrix = np.vstack(np.hstack([N.T@A@N, -N.T@B@K]),
np.hstack([L@C@N, (N1.T@A@E - N1.T@B@K - L@C@E)]))
closed_loop.z_chi.vector = np.vstack([N.T@A@R_used@zeta + N.T @B@Kz@z_des + N.T @B@u_des + N.T @g,
L@C@R_used@zeta + N1.T@B@Kz@z_des + N1.T@B@u_des + N1.T@g])
closed_loop.z_chi.ode_fnc = lambda y: closed_loop.z_chi.matrix @ y + closed_loop.z_xi.vector
closed_loop.z_chi.Y0 = np.hstack([initialConditions.z,initialConditions.chi_estimate_random])
##################################################################
### LTI in x-chi coordinates
closed_loop.x_chi.M = np.vstack([
np.hstack([np.eye(size_x), np.zeros((size_x, size_chi)), -R]),
np.hstack([np.zeros((size_chi, size_x)), np.eye((size_chi,size_chi)),np.zeros((size_chi, size_l))]),
np.hstack([G.M,np.zeros((size_l, size_chi)),np.zeros((size_l, size_l))])
])
iM = np.linalg.pinv(closed_loop.x_chi.M)
iM11 = iM[:(size_x+size_chi), :(size_x+size_chi)]
closed_loop.x_chi.matrix = [email protected]([np.hstack([A, -B@K]),
np.hstack([L@C, (N1.T@A@E - N1.T@B@K - L@C@E)])])
closed_loop.x_chi.vector = [email protected]([B@Kz@z_des + B@u_des+ g,N1.T@B@Kz@z_des + N1.T@B@u_des + N1.T@g])
closed_loop.x_chi.ode_fnc = lambda y: closed_loop.x_chi.matrix @ y + closed_loop.x_xi.vector
closed_loop.x_chi.Y0 = np.vstack([initialConditions.x, initialConditions.chi_estimate_random])
output.closed_loop = closed_loop
return output