def __init__(self, inverted=True, calc_coll_part_lin=True):
self.inverted = inverted
self.calc_coll_part_lin = calc_coll_part_lin
# -----------------------------------------
# Pendel-Wagen System mit hängendem Pendel
# -----------------------------------------
pp = st.symb_vector(("varphi", ))
qq = st.symb_vector(("q", ))
ttheta = st.row_stack(pp, qq)
st.make_global(ttheta)
params = sp.symbols('m1, m2, l, g, q_r, t, T')
st.make_global(params)
ex = sp.Matrix([1, 0])
ey = sp.Matrix([0, 1])
# Koordinaten der Schwerpunkte und Gelenke
S1 = ex * q # Schwerpunkt Wagen
G2 = S1 # Pendel-Gelenk
# Schwerpunkt des Pendels (Pendel zeigt für kleine Winkel nach oben)
if inverted:
S2 = G2 + mt.Rz(varphi) * ey * l
else:
S2 = G2 + mt.Rz(varphi) * -ey * l
# Zeitableitungen der Schwerpunktskoordinaten
S1d, S2d = st.col_split(st.time_deriv(st.col_stack(S1, S2), ttheta))
# Energie
E_rot = 0 # (Punktmassenmodell)
E_trans = (m1 * S1d.T * S1d + m2 * S2d.T * S2d) / 2
E = E_rot + E_trans[0]
V = m2 * g * S2[1]
# Partiell linearisiertes Model
mod = mt.generate_symbolic_model(E, V, ttheta, [0, sp.Symbol("u")])
mod.calc_state_eq()
mod.calc_coll_part_lin_state_eq()
self.mod = mod
def test_unicycle(self):
# test the generation of Lagrange-Byrnes-Isidori-Normal form
theta = st.symb_vector("p1, q1, q2")
p1, q1, q2 = theta
params = sp.symbols(
'l1, l2, s1, s2, delta0, delta1, delta2, J0, J1, J2, m0, m1, m2, r, g'
)
l1, l2, s1, s2, delta0, delta1, delta2, J0, J1, J2, m0, m1, m2, r, g = params
QQ = sp.symbols("Q0, Q1, Q2")
Q0, Q1, Q2 = QQ
mu = st.time_deriv(theta, theta)
p1d, q1d, q2d = mu
# Geometry
# noinspection PyUnusedLocal
ex = Matrix([1, 0])
ey = Matrix([0, 1])
M0 = Matrix([-r * p1, r])
S1 = M0 + mt.Rz(p1 + q1) * ey * s1
S2 = M0 + mt.Rz(p1 + q1) * ey * l1 + mt.Rz(p1 + q1 + q2) * ey * s2
M0d = st.time_deriv(M0, theta)
S1d = st.time_deriv(S1, theta)
S2d = st.time_deriv(S2, theta)
# Energy
T_rot = (J0 * p1d**2 + J1 * (p1d + q1d)**2 + J2 *
(p1d + q1d + q2d)**2) / 2
T_trans = (m0 * M0d.T * M0d + m1 * S1d.T * S1d + m2 * S2d.T * S2d) / 2
T = T_rot + T_trans[0]
V = m1 * g * S1[1] + m2 * g * S2[1]
mod = mt.generate_symbolic_model(T, V, theta, [0, Q1, Q2])
self.assertEqual(mod.fz, None)
self.assertEqual(mod.gz, None)
mod.calc_lbi_nf_state_eq()
self.assertEqual(mod.fz.shape, (6, 1))
self.assertEqual(mod.gz, sp.eye(6)[:, 2:4])
def test_two_link_manipulator(self):
p1, q1 = ttheta = sp.Matrix(sp.symbols('p1, q1'))
pdot1, qdot1 = st.time_deriv(ttheta, ttheta)
tau1, = ttau = sp.Matrix(sp.symbols('F1,'))
params = sp.symbols('m1, m2, l1, l2, s1, s2, J1, J2')
m1, m2, l1, l2, s1, s2, J1, J2 = params
ex = Matrix([1, 0])
ey = Matrix([0, 1])
S1 = mt.Rz(q1) * ex * s1 # center of mass (first link)
G1 = mt.Rz(q1) * ex * l1 # first joint
S2 = G1 + mt.Rz(q1 + p1) * ex * s2
#velocity
S1d = st.time_deriv(S1, ttheta)
S2d = st.time_deriv(S2, ttheta)
T_rot = (J1 * qdot1**2 + J2 * (qdot1 + pdot1)**2) / 2
T_trans = (m1 * S1d.T * S1d + m2 * S2d.T * S2d) / 2
T = T_rot + T_trans[0]
V = 0
mod = mt.generate_symbolic_model(T, V, ttheta, [tau1, 0])
#mod.eqns.simplify()
#mod.calc_coll_part_lin_state_eq(simplify=True)
mod.calc_lbi_nf_state_eq(simplify=True)
w1 = mod.ww[0]
kappa = l1 * m2 * s2 / (J2 + m2 * s2**2)
fz4 = -kappa * qdot1 * sin(p1) * (w1 - kappa * qdot1 * cos(p1))
fzref = sp.Matrix([[qdot1], [0],
[(-qdot1 * (1 + kappa * cos(p1))) + w1], [fz4]])
w_def_ref = pdot1 + qdot1 * (1 + kappa * cos(p1))
self.assertEqual(mod.gz, sp.Matrix([0, 1, 0, 0]))
fz_diff = mod.fz - fzref
fz_diff.simplify()
self.assertEqual(fz_diff, sp.Matrix([0, 0, 0, 0]))
diff = mod.ww_def[0, 0] - w_def_ref
self.assertEqual(diff.simplify(), 0)
def test_cart_pole_with_dissi(self):
p1, q1 = ttheta = sp.Matrix(sp.symbols('p1, q1'))
F1, tau_dissi = sp.Matrix(sp.symbols('F1, tau_dissi'))
params = sp.symbols('m0, m1, l1, g, d1')
m0, m1, l1, g, d1 = params
pdot1 = st.time_deriv(p1, ttheta)
# q1dd = st.time_deriv(q1, ttheta, order=2)
ex = Matrix([1, 0])
ey = Matrix([0, 1])
S0 = ex * q1 # joint of the pendulum
S1 = S0 - mt.Rz(p1) * ey * l1 # center of mass
# velocity
S0d = st.time_deriv(S0, ttheta)
S1d = st.time_deriv(S1, ttheta)
T_rot = 0 # no moment of inertia (mathematical pendulum)
T_trans = (m0 * S0d.T * S0d + m1 * S1d.T * S1d) / 2
T = T_rot + T_trans[0]
V = m1 * g * S1[1]
QQ = sp.Matrix([tau_dissi, F1])
mod = mt.generate_symbolic_model(T, V, ttheta, QQ)
mod.substitute_ext_forces(QQ, [d1 * pdot1, F1], [F1])
self.assertEqual(len(mod.tau), 1)
self.assertEqual(len(mod.extforce_list), 1)
self.assertEqual(mod.tau[0], F1)
mod.calc_coll_part_lin_state_eq()
def test_simple_pendulum_with_actuated_mountpoint(self):
np = 1
nq = 2
n = np + nq
pp = st.symb_vector("p1:{0}".format(np + 1))
qq = st.symb_vector("q1:{0}".format(nq + 1))
p1, q1, q2 = ttheta = st.row_stack(pp, qq)
pdot1, qdot1, qdot2 = tthetad = st.time_deriv(ttheta, ttheta)
mud = st.time_deriv(ttheta, ttheta, order=2)
params = sp.symbols('l3, l4, s3, s4, J3, J4, m1, m2, m3, m4, g')
l3, l4, s3, s4, J3, J4, m1, m2, m3, m4, g = params
tau1, tau2 = ttau = st.symb_vector("tau1, tau2")
## Geometry
ex = sp.Matrix([1, 0])
ey = sp.Matrix([0, 1])
# Koordinaten der Schwerpunkte und Gelenke
S1 = ex * q1
S2 = ex * q1 + ey * q2
G3 = S2 # Gelenk
# Schwerpunkt des Pendels #zeigt nach oben
S3 = G3 + mt.Rz(p1) * ey * s3
# Zeitableitungen der Schwerpunktskoordinaten
Sd1, Sd2, Sd3 = st.col_split(
st.time_deriv(st.col_stack(S1, S2, S3), ttheta)) ##
# Energy
T_rot = (J3 * pdot1**2) / 2
T_trans = (m1 * Sd1.T * Sd1 + m2 * Sd2.T * Sd2 + m3 * Sd3.T * Sd3) / 2
T = T_rot + T_trans[0]
V = m1 * g * S1[1] + m2 * g * S2[1] + m3 * g * S3[1]
external_forces = [0, tau1, tau2]
assert not any(external_forces[:np])
mod = mt.generate_symbolic_model(T, V, ttheta, external_forces)
mod.calc_coll_part_lin_state_eq(simplify=True)
#pdot1, qdot1, qdot2 = mod.ttd
ff_ref = sp.Matrix([[pdot1], [qdot1], [qdot2],
[g * m3 * s3 * sin(p1) / (J3 + m3 * s3**2)], [0],
[0]])
gg_ref_part = sp.Matrix([
m3 * s3 * cos(p1) / (J3 + m3 * s3**2),
m3 * s3 * sin(p1) / (J3 + m3 * s3**2)
]).T
self.assertEqual(mod.ff, ff_ref)
self.assertEqual(mod.gg[-3, :], gg_ref_part)
def test_simple_pendulum_with_actuated_mountpoint(self):
np = 1
nq = 2
n = np + nq
pp = st.symb_vector("p1:{0}".format(np + 1))
qq = st.symb_vector("q1:{0}".format(nq + 1))
p1, q1, q2 = ttheta = st.row_stack(pp, qq)
pdot1, qdot1, qdot2 = st.time_deriv(ttheta, ttheta)
params = sp.symbols('l3, l4, s3, s4, J3, J4, m1, m2, m3, m4, g')
l3, l4, s3, s4, J3, J4, m1, m2, m3, m4, g = params
tau1, tau2 = st.symb_vector("tau1, tau2")
# Geometry
ex = sp.Matrix([1, 0])
ey = sp.Matrix([0, 1])
# Coordinates of centers of masses (com) and joints
S1 = ex * q1
S2 = ex * q1 + ey * q2
G3 = S2 # Joints
# com of pendulum (points upwards)
S3 = G3 + mt.Rz(p1) * ey * s3
# timederivatives
Sd1, Sd2, Sd3 = st.col_split(
st.time_deriv(st.col_stack(S1, S2, S3), ttheta)) ##
# Energy
T_rot = (J3 * pdot1**2) / 2
T_trans = (m1 * Sd1.T * Sd1 + m2 * Sd2.T * Sd2 + m3 * Sd3.T * Sd3) / 2
T = T_rot + T_trans[0]
V = m1 * g * S1[1] + m2 * g * S2[1] + m3 * g * S3[1]
external_forces = [0, tau1, tau2]
assert not any(external_forces[:np])
mod = mt.generate_symbolic_model(T, V, ttheta, external_forces)
mod.calc_coll_part_lin_state_eq(simplify=True)
# Note: pdot1, qdot1, qdot2 = mod.ttd
ff_ref = sp.Matrix([[pdot1], [qdot1], [qdot2],
[g * m3 * s3 * sin(p1) / (J3 + m3 * s3**2)], [0],
[0]])
gg_ref_part = sp.Matrix([
m3 * s3 * cos(p1) / (J3 + m3 * s3**2),
m3 * s3 * sin(p1) / (J3 + m3 * s3**2)
]).T
self.assertEqual(mod.ff, ff_ref)
self.assertEqual(mod.gg[-3, :], gg_ref_part)
def test_cart_pole(self):
p1, q1 = ttheta = sp.Matrix(sp.symbols('p1, q1'))
F1, = FF = sp.Matrix(sp.symbols('F1,'))
params = sp.symbols('m0, m1, l1, g')
m0, m1, l1, g = params
pdot1 = st.time_deriv(p1, ttheta)
# q1dd = st.time_deriv(q1, ttheta, order=2)
ex = Matrix([1, 0])
ey = Matrix([0, 1])
S0 = ex * q1 # joint of the pendulum
S1 = S0 - mt.Rz(p1) * ey * l1 # center of mass
#velocity
S0d = st.time_deriv(S0, ttheta)
S1d = st.time_deriv(S1, ttheta)
T_rot = 0 # no moment of inertia (mathematical pendulum)
T_trans = (m0 * S0d.T * S0d + m1 * S1d.T * S1d) / 2
T = T_rot + T_trans[0]
V = m1 * g * S1[1]
with self.assertRaises(ValueError) as cm:
# wrong length of external forces
mt.generate_symbolic_model(T, V, ttheta, [F1])
with self.assertRaises(ValueError) as cm:
# wrong length of external forces
mt.generate_symbolic_model(T, V, ttheta, [F1, 0, 0])
QQ = sp.Matrix([0, F1])
mod = mt.generate_symbolic_model(T, V, ttheta, [0, F1])
mod_1 = mt.generate_symbolic_model(T, V, ttheta, QQ)
mod_2 = mt.generate_symbolic_model(T, V, ttheta, QQ.T)
self.assertEqual(mod.eqns, mod_1.eqns)
self.assertEqual(mod.eqns, mod_2.eqns)
mod.eqns.simplify()
M = mod.MM
M.simplify()
M_ref = Matrix([[l1**2 * m1, l1 * m1 * cos(p1)],
[l1 * m1 * cos(p1), m1 + m0]])
self.assertEqual(M, M_ref)
rest = mod.eqns.subs(st.zip0((mod.ttdd)))
rest_ref = Matrix([[g * l1 * m1 * sin(p1)],
[-F1 - l1 * m1 * pdot1**2 * sin(p1)]])
self.assertEqual(M, M_ref)
mod.calc_state_eq(simplify=True)
mod.calc_coll_part_lin_state_eq(simplify=True)
pdot1, qdot1 = mod.ttd
ff_ref = sp.Matrix([[pdot1], [qdot1], [-g * sin(p1) / l1], [0]])
gg_ref = sp.Matrix([[0], [0], [-cos(p1) / l1], [1]])
self.assertEqual(mod.ff, ff_ref)
self.assertEqual(mod.gg, gg_ref)
def test_triple_pendulum(self):
np = 1
nq = 2
pp = sp.Matrix(sp.symbols("p1:{0}".format(np + 1)))
qq = sp.Matrix(sp.symbols("q1:{0}".format(nq + 1)))
ttheta = st.row_stack(pp, qq)
Q1, Q2 = sp.symbols('Q1, Q2')
p1_d, q1_d, q2_d = mu = st.time_deriv(ttheta, ttheta)
p1_dd, q1_dd, q2_dd = mu_d = st.time_deriv(ttheta, ttheta, order=2)
p1, q1, q2 = ttheta
# reordering according to chain
kk = sp.Matrix([q1, q2, p1])
kd1, kd2, kd3 = q1_d, q2_d, p1_d
params = sp.symbols(
'l1, l2, l3, l4, s1, s2, s3, s4, J1, J2, J3, J4, m1, m2, m3, m4, g'
)
l1, l2, l3, l4, s1, s2, s3, s4, J1, J2, J3, J4, m1, m2, m3, m4, g = params
# geometry
mey = -Matrix([0, 1])
# coordinates for centers of inertia and joints
S1 = mt.Rz(kk[0]) * mey * s1
G1 = mt.Rz(kk[0]) * mey * l1
S2 = G1 + mt.Rz(sum(kk[:2])) * mey * s2
G2 = G1 + mt.Rz(sum(kk[:2])) * mey * l2
S3 = G2 + mt.Rz(sum(kk[:3])) * mey * s3
G3 = G2 + mt.Rz(sum(kk[:3])) * mey * l3
# velocities of joints and center of inertia
Sd1 = st.time_deriv(S1, ttheta)
Sd2 = st.time_deriv(S2, ttheta)
Sd3 = st.time_deriv(S3, ttheta)
# energy
T_rot = (J1 * kd1**2 + J2 * (kd1 + kd2)**2 + J3 *
(kd1 + kd2 + kd3)**2) / 2
T_trans = (m1 * Sd1.T * Sd1 + m2 * Sd2.T * Sd2 + m3 * Sd3.T * Sd3) / 2
T = T_rot + T_trans[0]
V = m1 * g * S1[1] + m2 * g * S2[1] + m3 * g * S3[1]
external_forces = [0, Q1, Q2]
mod = mt.generate_symbolic_model(T,
V,
ttheta,
external_forces,
simplify=False)
eqns_ref = Matrix([[J3*(2*p1_dd + 2*q1_dd + 2*q2_dd)/2 + g*m3*s3*sin(p1 + q1 + q2) + m3*s3*(-l1*q1_d**2*sin(q1) + l1*q1_dd*cos(q1) - l2*(q1_d + q2_d)**2*sin(q1 + q2) + l2*(q1_dd + q2_dd)*cos(q1 + q2) - s3*(p1_d + q1_d + q2_d)**2*sin(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*cos(p1 + q1 + q2))*cos(p1 + q1 + q2) + m3*s3*(l1*q1_d**2*cos(q1) + l1*q1_dd*sin(q1) + l2*(q1_d + q2_d)**2*cos(q1 + q2) + l2*(q1_dd + q2_dd)*sin(q1 + q2) + s3*(p1_d + q1_d + q2_d)**2*cos(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*sin(p1 + q1 + q2))*sin(p1 + q1 + q2)], [J1*q1_dd + J2*(2*q1_dd + 2*q2_dd)/2 + J3*(2*p1_dd + 2*q1_dd + 2*q2_dd)/2 - Q1 + g*m1*s1*sin(q1) + \
g*m2*(l1*sin(q1) + s2*sin(q1 + q2)) + g*m3*(l1*sin(q1) + l2*sin(q1 + q2) + s3*sin(p1 + q1 + q2)) + m1*q1_dd*s1**2*sin(q1)**2 + m1*q1_dd*s1**2*cos(q1)**2 + m2*(2*l1*sin(q1) + 2*s2*sin(q1 + q2))*(l1*q1_d**2*cos(q1) + l1*q1_dd*sin(q1) + s2*(q1_d + q2_d)**2*cos(q1 + q2) + s2*(q1_dd + q2_dd)*sin(q1 + q2))/2 + m2*(2*l1*cos(q1) + 2*s2*cos(q1 + q2))*(-l1*q1_d**2*sin(q1) + l1*q1_dd*cos(q1) - s2*(q1_d + q2_d)**2*sin(q1 + q2) + s2*(q1_dd + q2_dd)*cos(q1 + q2))/2 + m3*(2*l1*sin(q1) + 2*l2*sin(q1 + q2) + 2*s3*sin(p1 + q1 + q2))*(l1*q1_d**2*cos(q1) + l1*q1_dd*sin(q1) + l2*(q1_d + q2_d)**2*cos(q1 + q2) + l2*(q1_dd + q2_dd)*sin(q1 + q2) + s3*(p1_d + q1_d + q2_d)**2*cos(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*sin(p1 + q1 + q2))/2 + m3*(2*l1*cos(q1) + 2*l2*cos(q1 + q2) + 2*s3*cos(p1 + q1 + q2))*(-l1*q1_d**2*sin(q1) + l1*q1_dd*cos(q1) - l2*(q1_d + q2_d)**2*sin(q1 + q2) + l2*(q1_dd + q2_dd)*cos(q1 + q2) - \
s3*(p1_d + q1_d + q2_d)**2*sin(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*cos(p1 + q1 + q2))/2], [J2*(2*q1_dd + 2*q2_dd)/2 + J3*(2*p1_dd + 2*q1_dd + 2*q2_dd)/2 - Q2 + g*m2*s2*sin(q1 + q2) + g*m3*(l2*sin(q1 + q2) + s3*sin(p1 + q1 + q2)) + m2*s2*(-l1*q1_d**2*sin(q1) + l1*q1_dd*cos(q1) - s2*(q1_d + q2_d)**2*sin(q1 + q2) + s2*(q1_dd + q2_dd)*cos(q1 + q2))*cos(q1 + q2) + \
m2*s2*(l1*q1_d**2*cos(q1) + l1*q1_dd*sin(q1) + s2*(q1_d + q2_d)**2*cos(q1 + q2) + s2*(q1_dd + q2_dd)*sin(q1 + q2))*sin(q1 + q2) + m3*(2*l2*sin(q1 + q2) + 2*s3*sin(p1 + q1 + q2))*(l1*q1_d**2*cos(q1) + l1*q1_dd*sin(q1) + l2*(q1_d + q2_d)**2*cos(q1 + q2) + l2*(q1_dd + q2_dd)*sin(q1 + q2) + s3*(p1_d + q1_d + q2_d)**2*cos(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*sin(p1 + q1 + q2))/2 + m3*(2*l2*cos(q1 + q2) + 2*s3*cos(p1 + q1 + q2))*(-l1*q1_d**2*sin(q1) + l1*q1_dd*cos(q1) - l2*(q1_d + q2_d)**2*sin(q1 + q2) + l2*(q1_dd + q2_dd)*cos(q1 + q2) - s3*(p1_d + q1_d + q2_d)**2*sin(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*cos(p1 + q1 + q2))/2]])
self.assertEqual(eqns_ref, mod.eqns)
def testRz(self):
Rz1 = mt.Rz(sp.pi * 0.4)
Rz2 = mt.Rz(npy.pi * 0.4, to_numpy=True)
self.assertTrue(npy.allclose(st.to_np(Rz1), Rz2))
def test_cart_pole(self):
p1, q1 = ttheta = sp.Matrix(sp.symbols('p1, q1'))
F1, = FF = sp.Matrix(sp.symbols('F1,'))
params = sp.symbols('m0, m1, l1, g')
parameter_values = list(dict(m0=2.0, m1=1.0, l1=2.0, g=9.81).items())
m0, m1, l1, g = params
pdot1 = st.time_deriv(p1, ttheta)
# q1dd = st.time_deriv(q1, ttheta, order=2)
ex = Matrix([1, 0])
ey = Matrix([0, 1])
S0 = ex * q1 # joint of the pendulum
S1 = S0 - mt.Rz(p1) * ey * l1 # center of mass
# velocity
S0d = st.time_deriv(S0, ttheta)
S1d = st.time_deriv(S1, ttheta)
T_rot = 0 # no moment of inertia (mathematical pendulum)
T_trans = (m0 * S0d.T * S0d + m1 * S1d.T * S1d) / 2
T = T_rot + T_trans[0]
V = m1 * g * S1[1]
# noinspection PyUnusedLocal
with self.assertRaises(ValueError) as cm:
# wrong length of external forces
mt.generate_symbolic_model(T, V, ttheta, [F1])
# noinspection PyUnusedLocal
with self.assertRaises(ValueError) as cm:
# wrong length of external forces
mt.generate_symbolic_model(T, V, ttheta, [F1, 0, 0])
QQ = sp.Matrix([0, F1])
mod = mt.generate_symbolic_model(T, V, ttheta, [0, F1])
mod_1 = mt.generate_symbolic_model(T, V, ttheta, QQ)
mod_2 = mt.generate_symbolic_model(T, V, ttheta, QQ.T)
self.assertEqual(mod.eqns, mod_1.eqns)
self.assertEqual(mod.eqns, mod_2.eqns)
mod.eqns.simplify()
M = mod.MM
M.simplify()
M_ref = Matrix([[l1**2 * m1, l1 * m1 * cos(p1)],
[l1 * m1 * cos(p1), m1 + m0]])
self.assertEqual(M, M_ref)
# noinspection PyUnusedLocal
rest = mod.eqns.subs(st.zip0(mod.ttdd))
# noinspection PyUnusedLocal
rest_ref = Matrix([[g * l1 * m1 * sin(p1)],
[-F1 - l1 * m1 * pdot1**2 * sin(p1)]])
self.assertEqual(M, M_ref)
mod.calc_state_eq(simplify=True)
mod.calc_coll_part_lin_state_eq(simplify=True)
# ensure that recalculation is carried out only when we want it
tmp_f, tmp_g = mod.f, mod.g
mod.f, mod.g = "foo", "bar"
mod.calc_state_eq(simplify=True)
self.assertEqual(mod.f, "foo")
mod.calc_state_eq(simplify=True, force_recalculation=True)
self.assertEqual((mod.f, mod.g), (tmp_f, tmp_g))
pdot1, qdot1 = mod.ttd
ff_ref = sp.Matrix([[pdot1], [qdot1], [-g * sin(p1) / l1], [0]])
gg_ref = sp.Matrix([[0], [0], [-cos(p1) / l1], [1]])
self.assertEqual(mod.ff, ff_ref)
self.assertEqual(mod.gg, gg_ref)
A, b = mod.calc_jac_lin(base="coll_part_lin",
parameter_values=parameter_values)
Ae = sp.Matrix([[0, 0, 1, 0], [0, 0, 0, 1], [-4.905, 0, 0, 0],
[0, 0, 0, 0]])
be = sp.Matrix([0, 0, -0.5, 1])
self.assertEqual(A, Ae)
self.assertEqual(b, be)
A, b = mod.calc_jac_lin(base="force_input",
parameter_values=parameter_values)
Ae = sp.Matrix([[0, 0, 1, 0], [0, 0, 0, 1], [-7.3575, 0, 0, 0],
[4.905, 0, 0, 0]])
be = sp.Matrix([0, 0, -.25, 0.5])
self.assertEqual(A, Ae)
self.assertEqual(b, be)
st.make_global(ttheta, tthetad, tthetadd)
params = sp.symbols('l1, l2, l3, s1, s2, s3, m1, m2, m3, J1, J2, J3, m0, g')
st.make_global(params)
tau1 = sp.Symbol("tau1")
#Einheitsvektoren
ex = sp.Matrix([1,0])
ey = sp.Matrix([0,1])
# Koordinaten der Schwerpunkte und Gelenke
S0 = ex*q1 # Schwerpunkt Wagen
G0 = S0 # Gelenk zwischen Wagen und Pendel 1
G1 = G0 + mt.Rz(p1)*ey*l1 # Gelenk zwischen Pendel 1 und 2
G2 = G1 + mt.Rz(p1+p2)*ey*l2 # Gelenk zwischen Pendel 2 und 3
# Schwerpunkte der Pendel (Pendel zeigen für kleine Winkel nach oben; Pendelwinkel relativ)
S1 = G0 + mt.Rz(p1)*ey*s1
S2 = G1 + mt.Rz(p1+p2)*ey*s2
S3 = G2 + mt.Rz(p1+p2+p3)*ey*s3
# Zeitableitungen der Schwerpunktskoordinaten
Sd0, Sd1, Sd2, Sd3 = st.col_split(st.time_deriv(st.col_stack(S0, S1, S2, S3), ttheta))
# Energie
T_rot = (J1*pdot1**2)/2 + (J2*(pdot1 + pdot2)**2)/2 + (J3*(pdot1 + pdot2 + pdot3)**2)/2
T_trans = (m0*Sd0.T*Sd0 + m1*Sd1.T*Sd1 + m2*Sd2.T*Sd2 + m3*Sd3.T*Sd3)/2
T = T_rot + T_trans[0]
params = sp.symbols('l1, l2, s1, s2, m1, m2, m0, g')
st.make_global(params)
tau1 = sp.Symbol("tau1")
#Einheitsvektoren
ex = sp.Matrix([1,0])
ey = sp.Matrix([0,1])
# Koordinaten der Schwerpunkte und Gelenke
S0 = ex*q1 # Schwerpunkt Wagen
G0 = S0 # Gelenk zwischen Wagen und Pendel 1
G1 = S0 # Gelenk zwischen Wagen und Pendel 2
# Schwerpunkte des Pendels (Pendel zeigt für kleine Winkel nach oben)
S1 = G0 + mt.Rz(p1)*ey*s1
S2 = G1 + mt.Rz(p2)*ey*s2
# Zeitableitungen der Schwerpunktskoordinaten
Sd0, Sd1, Sd2 = st.col_split(st.time_deriv(st.col_stack(S0, S1, S2), ttheta))
# Energie
T_rot = 0
T_trans = (m0*Sd0.T*Sd0 + m1*Sd1.T*Sd1 + m2*Sd2.T*Sd2)/2
T = T_rot + T_trans[0]
V = m1*g*S1[1] + m2*g*S2[1]
mod = mt.generate_symbolic_model(T, V, ttheta, [0, 0, tau1])
st.make_global(ttheta, tthetad, tthetadd)
params = sp.symbols('l1, l2, s1, s2, m1, m2, J1, J2, m0, g')
st.make_global(params)
tau1 = sp.Symbol("tau1")
#Einheitsvektoren
ex = sp.Matrix([1, 0])
ey = sp.Matrix([0, 1])
# Koordinaten der Schwerpunkte und Gelenke
S0 = ex * q1 # Schwerpunkt Wagen
G0 = S0 # Gelenk zwischen Wagen und Pendel 1
G1 = G0 + mt.Rz(p1) * ey * l1 # Gelenk zwischen Pendel 1 und 2
# Schwerpunkte der Pendel (Pendel zeigen für kleine Winkel nach oben; Pendelwinkel relativ)
S1 = G0 + mt.Rz(p1) * ey * s1
S2 = G1 + mt.Rz(p1 + p2) * ey * s2
# Zeitableitungen der Schwerpunktskoordinaten
Sd0, Sd1, Sd2 = st.col_split(st.time_deriv(st.col_stack(S0, S1, S2), ttheta))
# Energie
T_rot = (J1 * pdot1**2) / 2 + (J2 * (pdot1 + pdot2)**2) / 2
T_trans = (m0 * Sd0.T * Sd0 + m1 * Sd1.T * Sd1 + m2 * Sd2.T * Sd2) / 2
T = T_rot + T_trans[0]
V = m1 * g * S1[1] + m2 * g * S2[1]
params = sp.symbols('l1, s1, m1, m0, g')
st.make_global(params)
tau1 = sp.Symbol("tau1")
#Einheitsvektoren
ex = sp.Matrix([1, 0])
ey = sp.Matrix([0, 1])
# Koordinaten der Schwerpunkte und Gelenke
S0 = ex * q1 # Schwerpunkt Wagen
G0 = S0 # Gelenk zwischen Wagen und Pendel 1
# Schwerpunkte des Pendels (Pendel zeigt für kleine Winkel nach oben)
S1 = G0 + mt.Rz(p1) * ey * s1
# Zeitableitungen der Schwerpunktskoordinaten
Sd0, Sd1 = st.col_split(st.time_deriv(st.col_stack(S0, S1), ttheta))
# Energie
T_rot = 0
T_trans = (m0 * Sd0.T * Sd0 + m1 * Sd1.T * Sd1) / 2
T = T_rot + T_trans[0]
V = m1 * g * S1[1]
mod = mt.generate_symbolic_model(T, V, ttheta, [0, tau1])
# Zustandsraummodell
