def make_gains():
name = 'cpp_test'
# System characteristics
A_c = np.asmatrix([
[0.0, 1.0],
[-1.0, -10.0],
])
B_c = np.asmatrix([
[0.0],
[10.0]
])
C = np.asmatrix([
[1.0, 0.0]
])
Q_controller = np.asmatrix([
[1.0, 0.0],
[0.0, 5.0]
])
R_controller = np.asmatrix([
[1.]
])
# Noise characteristics
Q_noise = np.asmatrix([
[1e-2, 0.],
[0., 1e-1]
])
R_noise = np.asmatrix([
[0.1]
])
A_d, B_d, Q_d, R_d = c2d(A_c, B_c, dt, Q_noise, R_noise)
K = clqr(A_c, B_c, Q_controller, R_controller)
Kff = feedforwards(A_d, B_d)
L = dkalman(A_d, C, Q_d, R_d)
gains = StateSpaceGains(name, dt, A_d, B_d, C, None, Q_d, R_noise, K, Kff, L)
gains.add_writable_constant('A_c', A_c)
gains.add_writable_constant('B_c', B_c)
return gains
def make_augmented_gains(second_stage, has_cube, subname):
unaugmented_gains = make_gains(second_stage, has_cube, subname)
dt = unaugmented_gains.dt
A_c = np.asmatrix(np.zeros((3, 3)))
A_c[:2, :2] = unaugmented_gains.A_c
A_c[:2, 2:3] = unaugmented_gains.B_c
B_c = np.asmatrix(np.zeros((3, 1)))
B_c[:2, :] = unaugmented_gains.B_c
C = np.asmatrix(np.zeros((1, 3)))
C[:, :2] = unaugmented_gains.C
D = np.asmatrix(np.zeros((1, 1)))
K = np.zeros((1, 3))
K[:, :2] = unaugmented_gains.K
K[0, 2] = 1.
Q_noise = np.zeros((3, 3))
Q_noise[:2, :2] = unaugmented_gains.Q_c
Q_noise[2, 2] = 10.
R_noise = np.asmatrix([[1e-5]])
# Kalman noise matrix
Q_kalman = np.asmatrix([[1e-2, 0.0, 0.0], [0.0, 2e-1, 0.0],
[0.0, 0.0, 3e3]])
Q_ff = np.asmatrix([[0.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.0]])
A_d, B_d, Q_d, R_d = c2d(A_c, B_c, dt, Q_noise, R_noise)
_, _, Q_dkalman, R_dkalman = c2d(A_c, B_c, dt, Q_kalman, R_noise)
L = dkalman(A_d, C, Q_dkalman, R_dkalman)
Kff = feedforwards(A_d, B_d, Q_ff)
name = unaugmented_gains.name + '_integral'
gains = StateSpaceGains(name, dt, A_d, B_d, C, None, Q_d, R_noise, K, Kff,
L)
return gains
def make_gains():
# x = | Angle |
# | Angular velocity |
# u = voltage
# y = encoder
name = 'gains'
# Parameters
moment_inertia = 4.0 * (.03**2) / 2.0 + 0.93 * (3.5 * 0.0254)**2.0 / 2.0
gear_ratio = 1.0 / 4.0
efficiency = .91
# motor characteristics
free_speed = 18700. / 60.0 * 6.28
free_current = .67
stall_torque = .71
stall_current = 134.
resistance = 12. / stall_current
torque_constant = stall_torque / stall_current
velocity_constant = (12. - free_current * resistance) / free_speed
num_motors = 2.0
sensor_ratio = 1.0
# back emf torque
emf = -(torque_constant * velocity_constant) / (
resistance * gear_ratio**2. / num_motors)
# motor torque
mtq = efficiency * torque_constant / (gear_ratio * resistance / num_motors)
# rotational acceleration
t2a = 1. / moment_inertia
A_c = np.asmatrix([[0., 1.], [0., t2a * emf]])
B_c = np.asmatrix([[0.], [t2a * mtq]])
C = np.asmatrix([[sensor_ratio, 0.]])
# Controller weighting
Q_controller = np.asmatrix([[0., 0.], [0., 5e-1]])
R_controller = np.asmatrix([[1.]])
# Noise
Q_noise = np.asmatrix([[1e-2, 0.], [0., 1e1]])
R_noise = np.asmatrix([[0.1]])
Q_ff = np.asmatrix([[0., 0.], [0., 1.]])
A_d, B_d, Q_d, R_d = c2d(A_c, B_c, dt, Q_noise, R_noise)
K = clqr(A_c, B_c, Q_controller, R_controller)
Kff = feedforwards(A_d, B_d, Q_ff)
L = dkalman(A_d, C, Q_d, R_d)
gains = StateSpaceGains(name, dt, A_d, B_d, C, None, Q_d, R_noise, K, Kff,
L)
gains.A_c = A_c
gains.B_c = B_c
gains.Q_c = Q_noise
return gains
def make_gains(second_stage, has_cube, subname='gains'):
# x = | Linear Height |
# | Linear Velocity |
# u = voltage
# y = encoder
name = subname
mass_carriage = 6.0
if second_stage:
mass_carriage += 2.5
if has_cube:
mass_carriage += 1.59
# Parameters
r = (1.0 + 1.0 / 16.0) * 0.0254
J = 0.68 * r**2 + mass_carriage * r**2
G = (12.0 / 100.0) * (14.0 / 30.0)
eff = .8
w_free = 18730. / 60. * 6.28
I_free = .7
T_stall = 0.71
I_stall = 134.
R = 12. / I_stall
Kt = T_stall / I_stall
Kv = (12. - I_free * R) / w_free
num_motors = 2.
sensor_ratio = 1.
A_c = np.asmatrix([
[0., 1.0],
[0., -Kt * Kv / (J * G * G * R)],
])
B_c = np.asmatrix([[0.], [Kt * r / (J * G * R)]])
C = np.asmatrix([[sensor_ratio, 0.]])
# Noise
Q_noise = np.asmatrix([[0., 0.], [0., 0.]])
R_noise = np.asmatrix([[1e-5]])
Q_ff = np.asmatrix([[0., 0.], [0., 1.]])
A_d, B_d, Q_d, R_d = c2d(A_c, B_c, dt, Q_noise, R_noise)
K = place(A_c, B_c, [-10.0, -7.0])
Kff = feedforwards(A_d, B_d, Q_ff)
L = dkalman(A_d, C, Q_d, R_d)
gains = StateSpaceGains(name, dt, A_d, B_d, C, None, Q_d, R_noise, K, Kff,
L)
gains.A_c = A_c
gains.B_c = B_c
gains.Q_c = Q_noise
return gains