def test_jet_extend_basis1(self):
x1, x2, x3 = xx = st.symb_vector("x1, x2, x3")
xx_tmp, ddx = pc.setup_objects(xx)
self.assertTrue(xx is xx_tmp)
# get the individual forms
dx1, dx2, dx3 = ddx
dx1.jet_extend_basis()
xdot1, xdot2, xdot3 = xxd = pc.st.time_deriv(xx, xx)
xddot1, xddot2, xddot3 = xxdd = pc.st.time_deriv(xx, xx, order=2)
full_basis = st.row_stack(xx, xxd, xxdd)
foo, ddX = pc.setup_objects(full_basis)
dx1.jet_extend_basis()
self.assertEqual(ddX[0].basis, dx1.basis)
self.assertEqual(ddX[0].coeff, dx1.coeff)
half_basis = st.row_stack(xx, xxd)
foo, ddY = pc.setup_objects(half_basis)
dx2.jet_extend_basis()
self.assertEqual(ddY[1].basis, dx2.basis)
self.assertEqual(ddY[1].coeff, dx2.coeff)
def generate_constraints_funcs(self):
"""
Generate callable functions which represent the algebraic constraints a(tt) and its first two derivatives
adot(tt, ttdot) and addot(tt, ttdot, ttddot).
:return: None
"""
if self.constraints_func is not None:
return
actual_symbs = self.constraints.atoms(sp.Symbol)
expected_symbs = set(self.mod.tt)
if not actual_symbs == expected_symbs:
msg = "Constraints can only converted to numerical func if all parameters are passed for substitution. " \
"Unexpected symbols: {}".format(actual_symbs.difference(expected_symbs))
raise ValueError(msg)
self.constraints_func = st.expr_to_func(self.mod.tt, self.constraints)
# now we need also the differentiated constraints (e.g. to calculate consistent velocities and accelerations)
self.constraints_d = st.time_deriv(self.constraints, self.mod.tt)
self.constraints_dd = st.time_deriv(self.constraints_d, self.mod.tt)
xx = st.row_stack(self.mod.tt, self.mod.ttd)
# this function depends on coordinates ttheta and velocities ttheta_dot
self.constraints_d_func = st.expr_to_func(xx, self.constraints_d)
zz = st.row_stack(self.mod.tt, self.mod.ttd, self.mod.ttdd)
# this function depends on coordinates ttheta and velocities ttheta_dot and accel
self.constraints_dd_func = st.expr_to_func(zz, self.constraints_dd)
def calc_eqlbr(self, parameter_values=None, ttheta_guess=None, etype='one_ep', display=False, **kwargs):
"""In the simplest case(RT1 and 2) only one of the equilibrium points of
a system is used for linearization and other researches. Such a equilibrium
point is calculated by minimizing the target function for a certain
input variable.
In other case(NT) all of the equilibrium points of a system are needed, which can be calculated
by using Slover in Sympy to solve the differential equations for certain input values.
:param: uu: certain input value defined by user
:param: parameter_values: unknown system parameters in list.(Default: all parameters are known)
:param: ttheta_guess: initial values for minimizing the target function.(Default: 0)
:param: etype: 'one_ep': one equilibrium point, 'all_ep': all of the equilibrium points.
:param: display: display all information of the result of the 'fmin' fucntion
:return: equilibrium point(s) in list
"""
if parameter_values is None:
parameter_values = []
if kwargs.get('uu') is None:
# if the system doesn't have input
assert self.tau is None
uu = []
all_vars = st.row_stack(self.tt)
uu_para = []
else:
uu = kwargs.get('uu')
all_vars = st.row_stack(self.tt, self.tau)
uu_para = list(zip(self.tau, uu))
class Type_equilibrium(Enum):
one_ep = auto()
all_ep = auto()
if etype == Type_equilibrium.one_ep.name:
# set all of the derivatives of the system states to zero
state_eqns = self.eqns.subz0(self.ttd, self.ttdd)
# target function for minimizing
state_eqns_func = st.expr_to_func(all_vars, state_eqns.subs(parameter_values))
if ttheta_guess is None:
ttheta_guess = st.to_np(self.tt * 0)
def target(ttheta):
"""target function for the certain global input values uu
"""
all_values = np.r_[ttheta, uu] # combine arrays
rhs = state_eqns_func(*all_values)
return np.sum(rhs ** 2)
self.eqlbr = fmin(target, ttheta_guess, disp=display)
elif etype == Type_equilibrium.all_ep.name:
state_eqns = self.eqns.subz0(self.ttd, self.ttdd)
all_vars_value = uu_para + parameter_values
self.eqlbr = sp.solve(state_eqns.subs(all_vars_value), self.tt)
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 transform_2nd_to_1st_order_matrices(P0, P1, P2, xx):
"""Transforms an implicit second order (tangential) representation of a mechanical system to
an first order representation (needed to apply the "Franke-approach")
:param P0: eqns.jacobian(theta)
:param P1: eqns.jacobian(theta_d)
:param P2: eqns.jacobian(theta_dd)
:param xx: vector of state variables
:return: P0_bar, P1_bar
with P0_bar = implicit_state_equations.jacobian(x)
and P1_bar = implicit_state_equations.jacobian(x_d)
"""
assert P0.shape == P1.shape == P2.shape
assert xx.shape == (P0.shape[1]*2, 1)
xxd = st.time_deriv(xx, xx)
N = int(xx.shape[0]/2)
# definitional equations like xdot1 - x3 = 0 (for N=2)
eqns_def = xx[N:, :] - xxd[:N, :]
eqns_mech = P2*xxd[N:, :] + P1*xxd[:N, :] + P0*xx[:N, :]
F = st.row_stack(eqns_def, eqns_mech)
P0_bar = F.jacobian(xx)
P1_bar = F.jacobian(xxd)
return P0_bar, P1_bar
def regular_completion(matrix):
m,n = matrix.shape
r = matrix.rank()
#~ IPS()
assert m!=n, "There is no regular completion of a square matrix."
if m<n:
assert r==m, "Matrix does not have full row rank."
A, B, V_pi = reshape_matrix_columns(matrix)
zeros = sp.zeros(n-m,m)
ones = sp.eye(n-m)
S = st.col_stack(zeros,ones)
completion = S*V_pi.T
regular_matrix = st.row_stack(matrix,completion)
assert st.rnd_number_rank(regular_matrix)==n, "Regular completion seems to be wrong."
elif n<m:
assert r==n, "Matrix does not have full column rank."
A, B, V_pi = reshape_matrix_columns(matrix.T)
zeros = sp.zeros(m-n,n)
ones = sp.eye(m-n)
S = st.col_stack(zeros,ones)
completion = V_pi*S.T
regular_matrix = st.col_stack(completion,matrix)
assert st.rnd_number_rank(regular_matrix)==m, "Regular completion seems to be wrong."
return completion
def test_unimod_inv3(self):
y1, y2 = yy = st.symb_vector('y1, y2', commutative=False)
s = sp.Symbol('s', commutative=False)
ydot1, ydot2 = yyd1 = st.time_deriv(yy, yy, order=1, commutative=False)
yddot1, yddot2 = yyd2 = st.time_deriv(yy,
yy,
order=2,
commutative=False)
yyd3 = st.time_deriv(yy, yy, order=3, commutative=False)
yyd4 = st.time_deriv(yy, yy, order=4, commutative=False)
yya = st.row_stack(yy, yyd1, yyd2, yyd3, yyd4)
M3 = sp.Matrix([[ydot2, y1 * s],
[
y2 * yddot2 + y2 * ydot2 * s, y1 * yddot2 +
y2 * y1 * s**2 + y2 * ydot1 * s + ydot2 * ydot1
]])
M3inv = nct.unimod_inv(M3, s, time_dep_symbs=yya)
product3a = nct.right_shift_all(nct.nc_mul(M3, M3inv),
s,
func_symbols=yya)
product3b = nct.right_shift_all(nct.nc_mul(M3inv, M3),
s,
func_symbols=yya)
res3a = nct.make_all_symbols_commutative(product3a)[0]
res3b = nct.make_all_symbols_commutative(product3b)[0]
res3a.simplify()
res3b.simplify()
self.assertEqual(res3a, sp.eye(2))
self.assertEqual(res3b, sp.eye(2))
def test_left_mul_by_2(self):
x1, x2, x3 = xx = st.symb_vector('x1:4', commutative=False)
xdot1, xdot2, xdot3 = xxdot = st.time_deriv(xx, xx)
xddot1, xddot2, xddot3 = xxddot = st.time_deriv(xxdot, xxdot)
XX = st.row_stack(xx, xxdot, xxddot)
C = sp.Symbol('C', commutative=False)
Q = sp.Matrix([[x3 / sin(x1), 1, 0], [-tan(x1), 0, x3]])
Q_ = st.col_stack(Q, sp.zeros(2, 6))
# matrix independent of s
M2 = sp.Matrix([[1, 0], [-C, 1]])
# 1-forms
w1 = pc.DifferentialForm(1, XX, coeff=Q_[0, :])
w2 = pc.DifferentialForm(1, XX, coeff=Q_[1, :])
# vector 1-form
w = pc.VectorDifferentialForm(1, XX, coeff=Q_)
t = w.left_mul_by(M2, additional_symbols=[C])
# object to compare with:
t2 = -C * w1 + w2
self.assertEqual(t2.coeff, t.coeff.row(1).T)
def test_vector_form_append_2(self):
x1, x2, x3 = xx = st.symb_vector('x1:4', commutative=False)
xdot1, xdot2, xdot3 = xxdot = st.time_deriv(xx, xx)
xddot1, xddot2, xddot3 = xxddot = st.time_deriv(xxdot, xxdot)
XX = st.row_stack(xx, xxdot, xxddot)
s = sp.Symbol('s', commutative=False)
C = sp.Symbol('C', commutative=False)
# vector 1-form
Q1 = sp.Matrix([[x3 / sin(x1), 1, 0], [-tan(x1), 0, x3]])
Q1_ = st.col_stack(Q1, sp.zeros(2, 6))
w1 = pc.VectorDifferentialForm(1, XX, coeff=Q1_)
# 1-forms
Q2 = sp.Matrix([[x1, x2, x3], [x3, x1, x2]])
Q2_ = st.col_stack(Q2, sp.zeros(2, 6))
w2 = pc.VectorDifferentialForm(1, XX, coeff=Q2_)
w1.append(w2)
# vector form to compare with:
B = sp.Matrix([[x3 / sin(x1), 1, 0], [-tan(x1), 0, x3], [x1, x2, x3],
[x3, x1, x2]])
B_ = st.col_stack(B, sp.zeros(4, 6))
self.assertEqual(w1.coeff, B_)
def test_left_mul_by_1(self):
x1, x2, x3 = xx = st.symb_vector('x1:4', commutative=False)
xdot1, xdot2, xdot3 = xxdot = st.time_deriv(xx, xx)
xddot1, xddot2, xddot3 = xxddot = st.time_deriv(xxdot, xxdot)
XX = st.row_stack(xx, xxdot, xxddot)
s = sp.Symbol('s', commutative=False)
C = sp.Symbol('C', commutative=False)
Q = sp.Matrix([[x3 / sin(x1), 1, 0], [-tan(x1), 0, x3]])
Q_ = st.col_stack(Q, sp.zeros(2, 6))
# s-dependent matrix
M1 = sp.Matrix([[1, 0], [-C * s, 1]])
# 1-forms
w1 = pc.DifferentialForm(1, XX, coeff=Q_[0, :])
w2 = pc.DifferentialForm(1, XX, coeff=Q_[1, :])
# vector 1-form
w = pc.VectorDifferentialForm(1, XX, coeff=Q_)
t = w.left_mul_by(M1, s, [C])
t2 = -C * w1.dot() + w2
self.assertEqual(t2.coeff, t.coeff.row(1).T)
def transform_2nd_to_1st_order_matrices(P0, P1, P2, xx):
"""Transforms an implicit second order (tangential) representation of a mechanical system to
an first order representation (needed to apply the "Franke-approach")
:param P0: eqns.jacobian(theta)
:param P1: eqns.jacobian(theta_d)
:param P2: eqns.jacobian(theta_dd)
:param xx: vector of state variables
:return: P0_bar, P1_bar
with P0_bar = implicit_state_equations.jacobian(x)
and P1_bar = implicit_state_equations.jacobian(x_d)
"""
assert P0.shape == P1.shape == P2.shape
assert xx.shape == (P0.shape[1] * 2, 1)
xxd = st.time_deriv(xx, xx)
N = int(xx.shape[0] / 2)
# definitional equations like xdot1 - x3 = 0 (for N=2)
eqns_def = xx[N:, :] - xxd[:N, :]
eqns_mech = P2 * xxd[N:, :] + P1 * xxd[:N, :] + P0 * xx[:N, :]
F = st.row_stack(eqns_def, eqns_mech)
P0_bar = F.jacobian(xx)
P1_bar = F.jacobian(xxd)
return P0_bar, P1_bar
def test_unimod_inv(self):
y1, y2 = yy = st.symb_vector('y1, y2', commutative=False)
s = sp.Symbol('s', commutative=False)
ydot1, ydot2 = yyd1 = st.time_deriv(yy, yy, order=1, commutative=False)
yddot1, yddot2 = yyd2 = st.time_deriv(yy,
yy,
order=2,
commutative=False)
yyd3 = st.time_deriv(yy, yy, order=3, commutative=False)
yyd4 = st.time_deriv(yy, yy, order=4, commutative=False)
yya = st.row_stack(yy, yyd1, yyd2, yyd3, yyd4)
M1 = sp.Matrix([yy[0]])
M1inv = nct.unimod_inv(M1, s, time_dep_symbs=yy)
self.assertEqual(M1inv, M1.inv())
M2 = sp.Matrix([[y1, y1 * s], [0, y2]])
M2inv = nct.unimod_inv(M2, s, time_dep_symbs=yy)
product2a = nct.right_shift_all(nct.nc_mul(M2, M2inv),
s,
func_symbols=yya)
product2b = nct.right_shift_all(nct.nc_mul(M2inv, M2),
s,
func_symbols=yya)
res2a = nct.make_all_symbols_commutative(product2a)[0]
res2b = nct.make_all_symbols_commutative(product2b)[0]
self.assertEqual(res2a, sp.eye(2))
self.assertEqual(res2b, sp.eye(2))
def regular_completion(matrix):
m, n = matrix.shape
r = matrix.rank()
#~ IPS()
assert m != n, "There is no regular completion of a square matrix."
if m < n:
assert r == m, "Matrix does not have full row rank."
A, B, V_pi = reshape_matrix_columns(matrix)
zeros = sp.zeros(n - m, m)
ones = sp.eye(n - m)
S = st.col_stack(zeros, ones)
completion = S * V_pi.T
regular_matrix = st.row_stack(matrix, completion)
assert st.rnd_number_rank(
regular_matrix) == n, "Regular completion seems to be wrong."
elif n < m:
assert r == n, "Matrix does not have full column rank."
A, B, V_pi = reshape_matrix_columns(matrix.T)
zeros = sp.zeros(m - n, n)
ones = sp.eye(m - n)
S = st.col_stack(zeros, ones)
completion = V_pi * S.T
regular_matrix = st.col_stack(completion, matrix)
assert st.rnd_number_rank(
regular_matrix) == m, "Regular completion seems to be wrong."
return completion
def calc_state_eq(self, simplify=True):
"""
reformulate the second order model to a first order statespace model
xd = f(x)+g(x)*u
"""
self.xx = st.row_stack(self.tt, self.ttd)
self.x = self.xx # xx is preferred now
eq2nd_order = self.solve_for_acc(simplify=simplify)
self.state_eq = st.row_stack(self.ttd, eq2nd_order)
self.f = self.state_eq.subs(st.zip0(self.tau))
self.g = self.state_eq.jacobian(self.tau)
if simplify:
self.f.simplify()
self.g.simplify()
def calc_state_eq(self, simplify=True):
"""
reformulate the second order model to a first order statespace model
xd = f(x)+g(x)*u
"""
self.xx = st.row_stack(self.tt, self.ttd)
self.x = self.xx # xx is preferred now
eq2nd_order = self.solve_for_acc(simplify=simplify)
self.state_eq = st.row_stack(self.ttd, eq2nd_order)
self.f = self.state_eq.subs(st.zip0(self.tau))
self.g = self.state_eq.jacobian(self.tau)
if simplify:
self.f.simplify()
self.g.simplify()
def calc_coll_part_lin_state_eq(self, simplify=True):
"""
calc vector fields ff, and gg of collocated linearization.
self.ff and self.gg are set.
"""
self.xx = st.row_stack(self.tt, self.ttd)
self.x = self.xx # xx is preferred now
nq = len(self.tau)
np = len(self.tt) - nq
B = self.eqns.jacobian(self.tau)
cond1 = B[:np, :] == sp.zeros(np, nq)
cond2 = B[np:, :] == -sp.eye(nq)
if not cond1 and cond2:
msg = "The jacobian of the equations of motion does not have the expected structure: %s"
raise NotImplementedError(msg % str(B))
# set the actuated accelearations as new inputs
self.aa = self.ttdd[-nq:, :]
self.calc_mass_matrix()
if simplify:
self.M.simplify()
M11 = self.M[:np, :np]
M12 = self.M[:np, np:]
d = M11.berkowitz_det()
adj = M11.adjugate()
if simplify:
d = d.simplify()
adj.simplify()
M11inv = adj / d
# setting input and acceleration to 0
C1K1 = self.eqns[:np, :].subs(st.zip0(self.ttdd, self.tau))
# eq_passive = -M11inv*C1K1 - M11inv*M12*self.aa
self.ff = st.row_stack(self.ttd, -M11inv * C1K1, self.aa * 0)
self.gg = st.row_stack(sp.zeros(np + nq, nq), -M11inv * M12,
sp.eye(nq))
if simplify:
self.ff.simplify()
self.gg.simplify()
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 calc_coll_part_lin_state_eq(self, simplify=True):
"""
calc vector fields ff, and gg of collocated linearization.
self.ff and self.gg are set.
"""
self.xx = st.row_stack(self.tt, self.ttd)
self.x = self.xx # xx is preferred now
nq = len(self.tau)
np = len(self.tt) - nq
B = self.eqns.jacobian(self.tau)
cond1 = B[:np, :] == sp.zeros(np, nq)
cond2 = B[np:, :] == -sp.eye(nq)
if not cond1 and cond2:
msg = "The jacobian of the equations of motion does not have the expected structure: %s"
raise NotImplementedError(msg % str(B))
# set the actuated accelearations as new inputs
self.aa = self.ttdd[-nq:, :]
self.calc_mass_matrix()
if simplify:
self.M.simplify()
M11 = self.M[:np, :np]
M12 = self.M[:np, np:]
d = M11.berkowitz_det()
adj = M11.adjugate()
if simplify:
d = d.simplify()
adj.simplify()
M11inv = adj/d
# setting input and acceleration to 0
C1K1 = self.eqns[:np, :].subs(st.zip0(self.ttdd, self.tau))
#eq_passive = -M11inv*C1K1 - M11inv*M12*self.aa
self.ff = st.row_stack(self.ttd, -M11inv*C1K1, self.aa*0)
self.gg = st.row_stack(sp.zeros(np + nq, nq), -M11inv*M12, sp.eye(nq))
if simplify:
self.ff.simplify()
self.gg.simplify()
def append(self, k_form):
assert k_form.degree == self.degree, "Degrees of vector forms do not match."
if isinstance(k_form, DifferentialForm):
rows = k_form.coeff.T
else:
rows = k_form.coeff
self._w.append(k_form)
self.coeff = st.row_stack(self.coeff, rows)
self.m = self.m + 1
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_vector_form_dot(self):
x1, x2, x3 = xx = st.symb_vector('x1:4', commutative=False)
xdot1, xdot2, xdot3 = xxdot = st.time_deriv(xx, xx)
xddot1, xddot2, xddot3 = xxddot = st.time_deriv(xxdot, xxdot)
XX = st.row_stack(xx, xxdot, xxddot)
Q = sp.Matrix([[x3 / sin(x1), 1, 0], [1, 0, x3]])
Q_ = st.col_stack(Q, sp.zeros(2, 6))
# vector 1-forms
omega = pc.VectorDifferentialForm(1, XX, coeff=Q_)
omega_dot = omega.dot()
omega_1, omega_2 = omega.unpack()
omega_1dot = omega_1.dot()
omega_2dot = omega_2.dot()
Qdot_ = st.row_stack(omega_1dot.coeff.T, omega_2dot.coeff.T)
# vector form to compare with
omegadot_comp = pc.VectorDifferentialForm(1, XX, coeff=Qdot_)
self.assertEqual(omega_dot.coeff, omegadot_comp.coeff)
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 __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_unimod_inv2(self):
y1, y2 = yy = st.symb_vector('y1, y2', commutative=False)
s = sp.Symbol('s', commutative=False)
ydot1, ydot2 = yyd1 = st.time_deriv(yy, yy, order=1, commutative=False)
yddot1, yddot2 = yyd2 = st.time_deriv(yy, yy, order=2, commutative=False)
yyd3 = st.time_deriv(yy, yy, order=3, commutative=False)
yyd4 = st.time_deriv(yy, yy, order=4, commutative=False)
yya = st.row_stack(yy, yyd1, yyd2, yyd3, yyd4)
# this Matrix is not unimodular due to factor 13 (should be 1)
M3 = sp.Matrix([[ydot2, 13*y1*s],
[y2*yddot2 + y2*ydot2*s, y1*yddot2 + y2*y1*s**2 + y2*ydot1*s + ydot2*ydot1]])
with self.assertRaises(ValueError) as cm:
res = nct.unimod_inv(M3, s, time_dep_symbs=yya)
def test_vector_form_dot(self):
x1, x2, x3 = xx = st.symb_vector('x1:4', commutative=False)
xdot1, xdot2, xdot3 = xxdot = st.time_deriv(xx, xx)
xddot1, xddot2, xddot3 = xxddot = st.time_deriv(xxdot, xxdot)
XX = st.row_stack(xx, xxdot, xxddot)
Q = sp.Matrix([[x3 / sin(x1), 1, 0], [1, 0, x3]])
Q_ = st.col_stack(Q, sp.zeros(2, 6))
# vector 1-forms
omega = pc.VectorDifferentialForm(1, XX, coeff=Q_)
with self.assertRaises(ValueError) as cm:
# coordinates are not part of basis
omega.dot().dot().dot()
def calc_eqlbr_rt1(mod,
uu,
sys_paras,
ttheta_guess=None,
display=False,
debug=False):
"""
In the simplest case, only one of the equilibrium points of
a nonlinear system is used for linearization.Such a equilibrium
point is calculated by minimizing the target function for a certain
input variable.
:param mod: symbolic_model
:param uu: system inputs
:param sys_paras: system parameters
:param ttheta_guess: initial guess of the equilibrium points
:param display: Set to True to print convergence messages.
:param debug: output control for debugging in unittest(False:normal
output,True: output local variables and normal output)
:return: Parameter that minimizes function
"""
# set all of the derivatives of the system states to zero
stat_eqns = mod.eqns.subz0(mod.ttd, mod.ttdd)
all_vars = st.row_stack(mod.tt, mod.uu)
# target function for minimizing
mod.stat_eqns_func = st.expr_to_func(all_vars, stat_eqns.subs(sys_paras))
if ttheta_guess is None:
ttheta_guess = st.to_np(mod.tt * 0)
def target(ttheta):
"""target function for the certain global input values uu
"""
all_vars = np.r_[ttheta, uu] # combine arrays
rhs = mod.stat_eqns_func(*all_vars)
return np.sum(rhs**2)
res = fmin(target, ttheta_guess, disp=display)
if debug:
C = ipydex.Container(fetch_locals=True)
return C, res
return res
def test_vector_form_get_coeff_from_idcs(self):
x1, x2, x3 = xx = st.symb_vector('x1:4')
xdot1, xdot2, xdot3 = xxdot = st.time_deriv(xx, xx)
xddot1, xddot2, xddot3 = xxddot = st.time_deriv(xxdot, xxdot)
XX = st.row_stack(xx, xxdot, xxddot)
Q = sp.Matrix([[x3 / sin(x1), 1, 0], [-tan(x1), 0, x3]])
Q_ = st.col_stack(Q, sp.zeros(2, 6))
w = pc.VectorDifferentialForm(1, XX, coeff=Q_)
w_0 = w.get_coeff_from_idcs(0)
Q_0 = Q_.col(0)
self.assertEqual(w_0, Q_0)
w_1 = w.get_coeff_from_idcs(1)
Q_1 = Q_.col(1)
self.assertEqual(w_1, Q_1)
def stack_to_vector_form(*arg):
"""
This function stacks k-forms to a vector k-form.
"""
# TODO: asserts
if len(arg) == 1:
return arg[0]
else:
XX = arg[0].basis
k = arg[0].degree
coeff_matrix = sp.Matrix([])
for i in range(0, len(arg)):
coeff_matrix_i = arg[i].coeff.T
coeff_matrix = st.row_stack(coeff_matrix, coeff_matrix_i)
return VectorDifferentialForm(k, XX, coeff=coeff_matrix)
def test_vector_k_form(self):
x1, x2, x3 = xx = st.symb_vector('x1:4')
xdot1, xdot2, xdot3 = xxdot = st.time_deriv(xx, xx)
xddot1, xddot2, xddot3 = xxddot = st.time_deriv(xxdot, xxdot)
XX = st.row_stack(xx, xxdot, xxddot)
Q = sp.Matrix([[x3 / sin(x1), 1, 0], [-tan(x1), 0, x3]])
Q_ = st.col_stack(Q, sp.zeros(2, 6))
w1 = pc.DifferentialForm(1, XX, coeff=Q_[0, :])
w2 = pc.DifferentialForm(1, XX, coeff=Q_[1, :])
w = pc.VectorDifferentialForm(1, XX, coeff=Q_)
w1_tilde = w.get_differential_form(0)
self.assertEqual(w1.coeff, w1_tilde.coeff)
w2_tilde = w.get_differential_form(1)
self.assertEqual(w2.coeff, w2_tilde.coeff)
def test_vector_form_subs(self):
x1, x2, x3 = xx = st.symb_vector('x1:4', commutative=False)
xdot1, xdot2, xdot3 = xxdot = st.time_deriv(xx, xx)
xddot1, xddot2, xddot3 = xxddot = st.time_deriv(xxdot, xxdot)
XX = st.row_stack(xx, xxdot, xxddot)
C = sp.Symbol('C', commutative=False)
Q = sp.Matrix([[x3 / sin(x1), 1, 0], [C, 0, x3]])
Q_ = st.col_stack(Q, sp.zeros(2, 6))
B = sp.Matrix([[x3 / sin(x1), 1, 0], [-tan(x1), 0, x3]])
B_ = st.col_stack(B, sp.zeros(2, 6))
# vector 1-forms
omega = pc.VectorDifferentialForm(1, XX, coeff=Q_).subs(C, -tan(x1))
self.assertEqual(B_, omega.coeff)
def test_stack_to_vector_form(self):
x1, x2, x3 = xx = st.symb_vector('x1:4', commutative=False)
xdot1, xdot2, xdot3 = xxdot = st.time_deriv(xx, xx)
xddot1, xddot2, xddot3 = xxddot = st.time_deriv(xxdot, xxdot)
XX = st.row_stack(xx, xxdot, xxddot)
Q = sp.Matrix([[x3 / sin(x1), 1, 0], [-tan(x1), 0, x3]])
Q_ = st.col_stack(Q, sp.zeros(2, 6))
# 1-forms
w1 = pc.DifferentialForm(1, XX, coeff=Q_[0, :])
w2 = pc.DifferentialForm(1, XX, coeff=Q_[1, :])
w_stacked = pc.stack_to_vector_form(w1, w2)
# vector 1-form
w = pc.VectorDifferentialForm(1, XX, coeff=Q_)
self.assertEqual(w.coeff, w_stacked.coeff)
def test_left_mul_by_3(self):
x1, x2, x3 = xx = st.symb_vector('x1:4', commutative=False)
xdot1, xdot2, xdot3 = xxdot = st.time_deriv(xx, xx)
xddot1, xddot2, xddot3 = xxddot = st.time_deriv(xxdot, xxdot)
XX = st.row_stack(xx, xxdot, xxddot)
s = sp.Symbol('s', commutative=False)
C = sp.Symbol('C', commutative=False)
Q = sp.Matrix([[x3 / sin(x1), 1, 0], [-tan(x1), 0, x3]])
Q_ = st.col_stack(Q, sp.zeros(2, 6))
M3 = sp.Matrix([[1, 0], [-C * s**2, 1]])
# vector 1-forms
w = pc.VectorDifferentialForm(1, XX, coeff=Q_)
with self.assertRaises(Exception) as cm:
# raises NotImplemented but this might change
t = w.left_mul_by(M3, s, additional_symbols=[C])
def test_unimod_inv3(self):
y1, y2 = yy = st.symb_vector('y1, y2', commutative=False)
s = sp.Symbol('s', commutative=False)
ydot1, ydot2 = yyd1 = st.time_deriv(yy, yy, order=1, commutative=False)
yddot1, yddot2 = yyd2 = st.time_deriv(yy, yy, order=2, commutative=False)
yyd3 = st.time_deriv(yy, yy, order=3, commutative=False)
yyd4 = st.time_deriv(yy, yy, order=4, commutative=False)
yya = st.row_stack(yy, yyd1, yyd2, yyd3, yyd4)
M3 = sp.Matrix([[ydot2, y1*s],
[y2*yddot2 + y2*ydot2*s, y1*yddot2 + y2*y1*s**2 + y2*ydot1*s + ydot2*ydot1]])
M3inv = nct.unimod_inv(M3, s, time_dep_symbs=yya)
product3a = nct.right_shift_all( nct.nc_mul(M3, M3inv), s, func_symbols=yya)
product3b = nct.right_shift_all( nct.nc_mul(M3inv, M3), s, func_symbols=yya)
res3a = nct.make_all_symbols_commutative(product3a)[0]
res3b = nct.make_all_symbols_commutative(product3b)[0]
res3a.simplify()
res3b.simplify()
self.assertEqual(res3a, sp.eye(2))
self.assertEqual(res3b, sp.eye(2))
def test_unimod_inv(self):
y1, y2 = yy = st.symb_vector('y1, y2', commutative=False)
s = sp.Symbol('s', commutative=False)
ydot1, ydot2 = yyd1 = st.time_deriv(yy, yy, order=1, commutative=False)
yddot1, yddot2 = yyd2 = st.time_deriv(yy, yy, order=2, commutative=False)
yyd3 = st.time_deriv(yy, yy, order=3, commutative=False)
yyd4 = st.time_deriv(yy, yy, order=4, commutative=False)
yya = st.row_stack(yy, yyd1, yyd2, yyd3, yyd4)
M1 = sp.Matrix([yy[0]])
M1inv = nct.unimod_inv(M1, s, time_dep_symbs=yy)
self.assertEqual(M1inv, M1.inv())
M2 = sp.Matrix([[y1, y1*s], [0, y2]])
M2inv = nct.unimod_inv(M2, s, time_dep_symbs=yy)
product2a = nct.right_shift_all( nct.nc_mul(M2, M2inv), s, func_symbols=yya)
product2b = nct.right_shift_all( nct.nc_mul(M2inv, M2), s, func_symbols=yya)
res2a = nct.make_all_symbols_commutative( product2a)[0]
res2b = nct.make_all_symbols_commutative( product2b)[0]
self.assertEqual(res2a, sp.eye(2))
self.assertEqual(res2b, sp.eye(2))
def test_difference_vector_forms(self):
x1, x2, x3 = xx = st.symb_vector('x1:4', commutative=False)
xdot1, xdot2, xdot3 = xxdot = st.time_deriv(xx, xx)
xddot1, xddot2, xddot3 = xxddot = st.time_deriv(xxdot, xxdot)
XX = st.row_stack(xx, xxdot, xxddot)
A = sp.Matrix([[x3 / sin(x1), 1, 0], [1, 0, x3]])
A_ = st.col_stack(A, sp.zeros(2, 6))
B = sp.Matrix([[x3 / cos(x1), 0, 1], [-tan(x1), 0, x3]])
B_ = st.col_stack(B, sp.zeros(2, 6))
# vector 1-forms
omega_a = pc.VectorDifferentialForm(1, XX, coeff=A_)
omega_b = pc.VectorDifferentialForm(1, XX, coeff=B_)
omega_c = omega_a - omega_b
# vector form to compare with
Q_ = A_ - B_
omega_comp = pc.VectorDifferentialForm(1, XX, coeff=Q_)
self.assertEqual(omega_c.coeff, omega_comp.coeff)
def test_minors_funcs(self):
s = sp.Symbol("s")
A = sp.Matrix([[-2, 0, 1], [3, 2, 0], [2, 0, -1]])
C = sp.Matrix([0, 1, 0]).T
AC = st.row_stack(s * sp.eye(3) - A, C)
M1 = st.col_minor(AC.T, 0, 1, 2)
M2 = st.col_minor(AC.T, 0, 1, 3)
M3 = st.col_minor(AC.T, 0, 2, 3)
M4 = st.col_minor(AC.T, 1, 2, 3)
all_row_minors = st.all_k_minors(AC, k=3)
self.assertEqual(all_row_minors, [M1, M2, M3, M4])
all_row_minors_idcs = st.all_k_minors(AC, k=3, return_indices=True)
eres = [(((0, 1, 2), (0, 1, 2)), s**3 + s**2 - 6 * s),
(((0, 1, 3), (0, 1, 2)), 3),
(((0, 2, 3), (0, 1, 2)), -s**2 - 3 * s),
(((1, 2, 3), (0, 1, 2)), 3 * s + 3)]
self.assertEqual(all_row_minors_idcs, eres)
def unimod_inv(M, s=None, t=None, time_dep_symbs=[], simplify_nsm=True, max_deg=None):
""" Assumes that M(s) is an unimodular polynomial matrix and calculates its inverse
which is again unimodular
:param M: Matrix to be inverted
:param s: Derivative Symbol
:param time_dep_symbs: sequence of time dependent symbols
:param max_deg: maximum polynomial degree w.r.t. s of the ansatz
:return: Minv
"""
assert isinstance(M, sp.MatrixBase)
assert M.is_square
n = M.shape[0]
degree_m = nc_degree(M, s)
if max_deg is None:
# upper bound according to
# Levine 2011, On necessary and sufficient conditions for differential flatness, p. 73
max_deg = (n - 1)*degree_m
assert int(max_deg) == max_deg
assert max_deg >= 0
C = M*0
free_params = []
for i in range(max_deg+1):
prefix = 'c{0}_'.format(i)
c_part = st.symbMatrix(n, n, prefix, commutative=False)
C += nc_mul(c_part, s**i)
free_params.extend(list(c_part))
P = nc_mul(C, M) - sp.eye(n)
P2 = right_shift_all(P, s, t, time_dep_symbs).reshape(n*n, 1)
deg_P = nc_degree(P2, s)
part_eqns = []
for i in range(deg_P + 1):
# omit the highest order (because it behaves like in the commutative case)
res = P2.diff(s, i).subs(s, 0)#/sp.factorial(i)
part_eqns.append(res)
eqns = st.row_stack(*part_eqns) # equations for all degrees of s
# now non-commutativity is inferring
eqns2, st_c_nc = make_all_symbols_commutative(eqns)
free_params_c, st_c_nc_free_params = make_all_symbols_commutative(free_params)
# find out which of the equations are (in)homogeneous
eqns2_0 = eqns2.subs(st.zip0(free_params_c))
assert eqns2_0.atoms() in ({0, -1}, {-1}, set())
inhom_idcs = st.np.where(st.to_np(eqns2_0) != 0)[0]
hom_idcs = st.np.where(st.to_np(eqns2_0) == 0)[0]
eqns_hom = sp.Matrix(st.np.array(eqns2)[hom_idcs])
eqns_inh = sp.Matrix(st.np.array(eqns2)[inhom_idcs])
assert len(eqns_inh) == n
# find a solution for the homogeneous equations
# if this is not possible, M was not unimodular
Jh = eqns_hom.jacobian(free_params_c).expand()
nsm = st.nullspaceMatrix(Jh, simplify=simplify_nsm, sort_rows=True)
na = nsm.shape[1]
if na < n:
msg = 'Could not determine sufficiently large nullspace. '\
'Either M is not unimodular or the expressions are to complicated.'
# TODO: decide which of the two cases occurs, via substitution of
# random numbers and singular value decomposition
# (or application of st.generic_rank)
raise ValueError(msg)
# parameterize the inhomogenous equations with the solution of the homogeneous equations
# new free parameters:
aa = st.symb_vector('_a1:{0}'.format(na+1))
nsm_a = nsm*aa
eqns_inh2 = eqns_inh.subs(lzip(free_params_c, nsm_a))
# now solve the remaining equations
# solve the linear system
Jinh = eqns_inh2.jacobian(aa)
rhs_inh = -eqns_inh2.subs(st.zip0(aa))
assert rhs_inh == sp.ones(n, 1)
sol_vect = Jinh.solve(rhs_inh)
sol = lzip(aa, sol_vect)
# get the values for all free_params (now they are not free anymore)
free_params_sol_c = nsm_a.subs(sol)
# replace the commutative symbols with the original non_commutative symbols (of M)
free_params_sol = free_params_sol_c.subs(st_c_nc)
Minv = C.subs(lzip(free_params, free_params_sol))
return Minv
def unimod_inv(M,
s=None,
t=None,
time_dep_symbs=[],
simplify_nsm=True,
max_deg=None):
""" Assumes that M(s) is an unimodular polynomial matrix and calculates its inverse
which is again unimodular
:param M: Matrix to be inverted
:param s: Derivative Symbol
:param time_dep_symbs: sequence of time dependent symbols
:param max_deg: maximum polynomial degree w.r.t. s of the ansatz
:return: Minv
"""
assert isinstance(M, sp.MatrixBase)
assert M.is_square
n = M.shape[0]
degree_m = nc_degree(M, s)
if max_deg is None:
# upper bound according to
# Levine 2011, On necessary and sufficient conditions for differential flatness, p. 73
max_deg = (n - 1) * degree_m
assert int(max_deg) == max_deg
assert max_deg >= 0
C = M * 0
free_params = []
for i in range(max_deg + 1):
prefix = 'c{0}_'.format(i)
c_part = st.symbMatrix(n, n, prefix, commutative=False)
C += nc_mul(c_part, s**i)
free_params.extend(list(c_part))
P = nc_mul(C, M) - sp.eye(n)
P2 = right_shift_all(P, s, t, time_dep_symbs).reshape(n * n, 1)
deg_P = nc_degree(P2, s)
part_eqns = []
for i in range(deg_P + 1):
# omit the highest order (because it behaves like in the commutative case)
res = P2.diff(s, i).subs(s, 0) #/sp.factorial(i)
part_eqns.append(res)
eqns = st.row_stack(*part_eqns) # equations for all degrees of s
# now non-commutativity is inferring
eqns2, st_c_nc = make_all_symbols_commutative(eqns)
free_params_c, st_c_nc_free_params = make_all_symbols_commutative(
free_params)
# find out which of the equations are (in)homogeneous
eqns2_0 = eqns2.subs(st.zip0(free_params_c))
assert eqns2_0.atoms() in ({0, -1}, {-1}, set())
inhom_idcs = st.np.where(st.to_np(eqns2_0) != 0)[0]
hom_idcs = st.np.where(st.to_np(eqns2_0) == 0)[0]
eqns_hom = sp.Matrix(st.np.array(eqns2)[hom_idcs])
eqns_inh = sp.Matrix(st.np.array(eqns2)[inhom_idcs])
assert len(eqns_inh) == n
# find a solution for the homogeneous equations
# if this is not possible, M was not unimodular
Jh = eqns_hom.jacobian(free_params_c).expand()
nsm = st.nullspaceMatrix(Jh, simplify=simplify_nsm, sort_rows=True)
na = nsm.shape[1]
if na < n:
msg = 'Could not determine sufficiently large nullspace. '\
'Either M is not unimodular or the expressions are to complicated.'
# TODO: decide which of the two cases occurs, via substitution of
# random numbers and singular value decomposition
# (or application of st.generic_rank)
raise ValueError(msg)
# parameterize the inhomogenous equations with the solution of the homogeneous equations
# new free parameters:
aa = st.symb_vector('_a1:{0}'.format(na + 1))
nsm_a = nsm * aa
eqns_inh2 = eqns_inh.subs(lzip(free_params_c, nsm_a))
# now solve the remaining equations
# solve the linear system
Jinh = eqns_inh2.jacobian(aa)
rhs_inh = -eqns_inh2.subs(st.zip0(aa))
assert rhs_inh == sp.ones(n, 1)
sol_vect = Jinh.solve(rhs_inh)
sol = lzip(aa, sol_vect)
# get the values for all free_params (now they are not free anymore)
free_params_sol_c = nsm_a.subs(sol)
# replace the commutative symbols with the original non_commutative symbols (of M)
free_params_sol = free_params_sol_c.subs(st_c_nc)
Minv = C.subs(lzip(free_params, free_params_sol))
return Minv
# definitorische Gleichungen
eq_defin = thetadot - mu
#eq_mech = AA*theta + BB*mu + CC*mudot
eq_mech = AA*theta + BB*thetadot + CC*mudot
# Container um zusätzliche Information über das Beispiel zu speichern
data = st.Container()
data.P0 = AA
data.P1 = BB
data.P2 = CC
data.eq_mech = eq_mech
data.time_dep_symbols = diff_symbols
F_eq_orig = F_eq = st.row_stack(eq_defin, eq_mech)
sys_name = "mechanik_ph_RTtt_seriell"
#~ from ipHelp import IPS
#~ IPS()
#F_eq = sp.Matrix([
#[ xdot1 - x4 ],
#[ xdot2 - x5 ],
#[ xdot3 - x6 ],
#[ a2*xdot4 + b2*xdot5 + c2*xdot6 + b1*x5 + c1*x6 + b0*x2 + c0*x3 ]])
def calc_lbi_nf_state_eq(self, simplify=False):
"""
calc vectorfields fz, and gz of the Lagrange-Byrnes-Isidori-Normalform
instead of the state xx
"""
n = len(self.tt)
nq = len(self.tau)
np = n - nq
nx = 2*n
# make sure that the system has the desired structure
B = self.eqns.jacobian(self.tau)
cond1 = B[:np, :] == sp.zeros(np, nq)
cond2 = B[np:, :] == -sp.eye(nq)
if not cond1 and cond2:
msg = "The jacobian of the equations of motion do not have the expected structure: %s"
raise NotImplementedError(msg % str(B))
pp = self.tt[:np,:]
qq = self.tt[np:,:]
uu = self.ttd[:np,:]
vv = self.ttd[np:,:]
ww = st.symb_vector('w1:{0}'.format(np+1))
assert len(vv) == nq
# state w.r.t normal form
self.zz = st.row_stack(qq, vv, pp, ww)
self.ww = ww
# set the actuated accelearations as new inputs
self.aa = self.ttdd[-nq:, :]
# input vectorfield
self.gz = sp.zeros(nx, nq)
self.gz[nq:2*nq, :] = sp.eye(nq) # identity matrix for the active coordinates
# drift vectorfield (will be completed below)
self.fz = sp.zeros(nx, 1)
self.fz[:nq, :] = vv
self.calc_mass_matrix()
if simplify:
self.M.simplify()
M11 = self.M[:np, :np]
M12 = self.M[:np, np:]
d = M11.berkowitz_det()
adj = M11.adjugate()
if simplify:
d = d.simplify()
adj.simplify()
M11inv = adj/d
# defining equation for ww: ww := uu + M11inv*M12*vv
uu_expr = ww - M11inv*M12*vv
# setting input tau and acceleration to 0 in the equations of motion
C1K1 = self.eqns[:np, :].subs(st.zip0(self.ttdd, self.tau))
N = st.time_deriv(M11inv*M12, self.tt)
ww_dot = -M11inv*C1K1.subs(lzip(uu, uu_expr)) + N.subs(lzip(uu, uu_expr))*vv
self.fz[2*nq:2*nq+np, :] = uu_expr
self.fz[2*nq+np:, :] = ww_dot
# how the new coordinates are defined:
self.ww_def = uu + M11inv*M12*vv
if simplify:
self.fz.simplify()
self.gz.simplify()
self.ww_def.simplify()
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)
