-
Notifications
You must be signed in to change notification settings - Fork 70
/
control.py
47 lines (31 loc) · 1.45 KB
/
control.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
#!/usr/bin/env python
# Controller Design
from numpy import zeros, matrix, eye, dot, asarray
from numpy.linalg import inv
from scipy.linalg import solve_continuous_are
from .utils import controllable
from .visualization import *
equilibrium_point = zeros(len(coordinates + speeds))
equilibrium_dict = dict(zip(coordinates + speeds, equilibrium_point))
parameter_dict = dict(zip(constants, numerical_constants))
linear_state_matrix, linear_input_matrix, inputs = \
kane.linearize(new_method=True, A_and_B=True)
f_A_lin = linear_state_matrix.subs(parameter_dict).subs(equilibrium_dict)
f_B_lin = linear_input_matrix.subs(parameter_dict).subs(equilibrium_dict)
m_mat = mass_matrix.subs(parameter_dict).subs(equilibrium_dict)
A = matrix(m_mat.inv() * f_A_lin).astype(float)
B = matrix(m_mat.inv() * f_B_lin).astype(float)
assert controllable(A, B)
Q = matrix(eye(6))
R = matrix(eye(3))
S = solve_continuous_are(A, B, Q, R)
K = inv(R) * B.T * S
# This is an annoying little issue. We specified the order of things when
# creating the rhs function, but the linearize function returns the F_B
# matrix in the order corresponding to whatever order it finds the joint
# torques. This would also screw things up if we specified a different
# ordering of the coordinates and speeds as the standard kane._q + kane._u
K = K[[0, 2, 1], :]
def controller(x, t):
return -asarray(dot(K, x)).flatten()
y = odeint(right_hand_side, x0, t, args=(controller, numerical_constants))