-
Notifications
You must be signed in to change notification settings - Fork 4
/
derive_acrocart
executable file
·88 lines (72 loc) · 2.31 KB
/
derive_acrocart
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
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
#!/usr/bin/python
from __future__ import division
from sympy import *
from sympy.utilities.codegen import codegen
init_printing() # allows pretty printing
# Helper function for creating groups of numbered scalars
def scalar_group(name, total, start=0, dep=None):
syms = []
for i in xrange(start, start+total):
syms.append(symbols(name + str(i)))
if dep: syms[-1] = syms[-1](dep)
return syms
print "Initializing symbols..."
# Parameters
ndof = 3
g = symbols("g")
m0, m1, m2 = m = scalar_group("m", ndof)
b0, b1, b2 = b = scalar_group("b", ndof)
l1, l2 = l = scalar_group("l", 2, start=1)
params = [g] + m + b + l
# Time, generalized coordinates, rates, accels, and input force
t = symbols("t")
q0, q1, q2 = coords = scalar_group("q", ndof, dep=t)
qd0, qd1, qd2 = rates = map(lambda f: diff(f, t), coords)
qdd0, qdd1, qdd2 = accels = map(lambda f: diff(f, t), rates)
u = symbols("u")
print "Evaluating system geometry..."
# Geometry
x0 = q0
x1 = x0 + (l1/2)*sin(q1)
x2 = x1 + (l1/2)*sin(q1) + (l2/2)*sin(q2)
y0 = 0
y1 = y0 - (l1/2)*cos(q1)
y2 = y1 - (l1/2)*cos(q1) - (l2/2)*cos(q2)
# Speeds
v0 = sqrt(diff(x0, t)**2 + diff(y0, t)**2)
v1 = sqrt(diff(x1, t)**2 + diff(y1, t)**2)
v2 = sqrt(diff(x2, t)**2 + diff(y2, t)**2)
w1 = diff(q1, t)
w2 = diff(q2, t)
# Inertial properties
I1 = (m1/12)*l1**2
I2 = (m2/12)*l2**2
print "Constructing Euler-Lagrange equations..."
# Lagrangian
kinetic = (m0/2)*v0**2 + (m1/2)*v1**2 + (m2/2)*v2**2 + (I1/2)*w1**2 + (I2/2)*w2**2
potential = m0*g*y0 + m1*g*y1 + m2*g*y2
L = simplify(kinetic - potential)
# Euler-Lagrange equations
EL0 = collect(simplify(diff(diff(L, qd0), t) - diff(L, q0) - u + b0*qd0), accels)
EL1 = collect(simplify(diff(diff(L, qd1), t) - diff(L, q1) + b1*qd1), accels)
EL2 = collect(simplify(diff(diff(L, qd2), t) - diff(L, q2) + b2*qd2), accels)
print "Solving Euler-Lagrange equations for state-space form..."
# Solve for state-space form
sol = solve((EL0, EL1, EL2), accels)
print "Computing jacobians..."
# Compute linearization of equations of motion
EOM = Matrix([qd0,
qd1,
qd2,
sol[qdd0],
sol[qdd1],
sol[qdd2]])
A = EOM.jacobian(Matrix(coords + rates))
B = EOM.jacobian(Matrix([u]))
print "----\n---- EOM"
print EOM
print "----\n---- A"
print A
print "----\n---- B"
print B
print "----\n----"