def linearise(model):
""" linearise model at equilibrium point zero """
if model.zero_equilibrium:
J = model.f.jacobian(model.x)
# substitute state varibles with equilibrium point zero
model.A = J.subs(st.zip0(model.x))
model.b = model.g.subs(st.zip0(model.x))
def linearise(model):
""" linearise model at equilibrium point zero """
if model.zero_equilibrium:
J = model.f.jacobian(model.x)
# substitute state varibles with equilibrium point zero
model.A = J.subs(st.zip0(model.x))
model.b = model.g.subs(st.zip0(model.x))
def is_zero_equilibrium(model):
""" checks model for equilibrium point zero """
# substitution of state variables and their derivatives to zero
# substitution of input to zero --> stand-alone system
subslist = st.zip0(model.qs) + st.zip0(model.qds) + st.zip0(
model.qdds) + st.zip0(model.extforce_list)
eq_1 = model.eq_list.subs(subslist)
model.zero_equilibrium = all(eq_1)
def is_zero_equilibrium(model):
""" checks model for equilibrium point zero """
# substitution of state variables and their derivatives to zero
#substitution of input to zero --> stand-alone system
subslist = st.zip0(model.qs) + st.zip0(model.qds) + st.zip0(
model.qdds) + st.zip0(model.extforce_list)
eq_1 = model.eq_list.subs(subslist)
model.zero_equilibrium = all(eq_1)
def solve(problem_spec):
""" solution of full state feedback
:param problem_spec: ProblemSpecification object
:return: solution_data: states and output values of the stabilized system
"""
sys_f_body = msp.System_Property() # instance of the class System_Property
sys_f_body.sys_state = problem_spec.xx # state of the system
sys_f_body.tau = problem_spec.u # inputs of the system
# original nonlinear system functions
sys_f_body.n_state_func = problem_spec.rhs(problem_spec.xx, problem_spec.u)
# original output functions
sys_f_body.n_out_func = problem_spec.output_func(problem_spec.xx,
problem_spec.u)
sys_f_body.eqlbr = problem_spec.eqrt # equilibrium point
# linearize nonlinear system around the chosen equilibrium point
sys_f_body.sys_linerazition()
tuple_system = (sys_f_body.aa, sys_f_body.bb, sys_f_body.cc, sys_f_body.dd
) # system tuple
# call the state_feedback method to stabilize an unstable system
observer_res = ofr.full_observer(tuple_system,
problem_spec.poles_o,
problem_spec.poles_cl,
debug=False)
# simulation original nonlinear system with controller
f = sys_f_body.n_state_func.subs(st.zip0(
sys_f_body.tau)) # x_dot = f(x) + g(x) * u
g = sys_f_body.n_state_func.jacobian(sys_f_body.tau)
# observer simulation function
observer_sim = osf.Observer_SimModel(f, g, problem_spec.xx, tuple_system,
problem_spec.eqrt,
observer_res.observer_gain,
observer_res.state_feedback,
observer_res.pre_filter,
problem_spec.yr)
rhs = observer_sim.calc_observer_rhs_func()
res = odeint(rhs, problem_spec.xx0, problem_spec.tt)
# add equilibrium points to the estimated states from observer
res[:, len(problem_spec.xx):] += np.array(
[problem_spec.eqrt[i][1] for i in range(len(problem_spec.xx))])
# output function
output_function = sp.lambdify(problem_spec.xx,
sys_f_body.n_out_func,
modules='numpy')
yy = output_function(*res[:, 0:4].T)
solution_data = SolutionData()
solution_data.yy = yy # output of the stabilized system
solution_data.res = res # states of the stabilized system
solution_data.state_feedback = observer_res.state_feedback # controller gains
solution_data.observer_gain = observer_res.observer_gain # observer gains
solution_data.pre_filter = observer_res.pre_filter # pre-filter
return solution_data
def rhs(xx, uu):
""" Right hand side of the equation of motion in nonlinear state space form
:param xx: system state
:param uu: system input
:return: nonlinear state function
"""
R0 = 6 # reference resistance at the reference temperature in [Ohm]
Tr = 303.15 # reference temperature in [K]
alpha = 3.93e-3 # temperature coefficient in [1/K]
Ta = 293.15 # environment temperature in [K]
sigma = 5.67e-8 # Stefan–Boltzmann constant in [W/m**2/k**4]
A = 0.0025 # surface area of resistance m ** 2
c = 87 # heat capacity in [J/k]
'''
heat capacity is equal to specific heat capacity * mass of resistance
specific heat capacity for Cu: 394 [J/kg/K], density of Cu_resistance : 8.86[g/cm**3]
volume of Cu_resistance: 0.0025 [m**2] * 0.01 [m]
'''
x1 = xx[0] # state: temperature
u = uu[0]
p1_dot = u / (c * R0 * (1 + alpha * (x1 - Tr))) - (sigma * A *
(x1**4 - Ta**4)) / c
ff = sp.Matrix([p1_dot])
mod = mt.SymbolicModel()
mod.xx = sp.Matrix([x1])
mod.eqns = ff
mod.tau = sp.Matrix([u])
mod.f = ff.subs(st.zip0(mod.tau))
mod.g = ff.jacobian(mod.tau)
return mod
def test_subz0(self):
x1, x2, x3 = xx = st.symb_vector("x1, x2, x3")
y1, y2, y3 = yy = st.symb_vector("y1, y2, y3")
XX = (x1, x2)
a = x1 + 7 * x2 * x3
M1 = sp.Matrix([x2, x1 * x2, x3**2])
M2 = sp.ImmutableDenseMatrix(M1)
self.assertEqual(x1.subs(st.zip0(XX)), x1.subz0(XX))
self.assertEqual(a.subs(st.zip0(XX)), a.subz0(XX))
self.assertEqual(M1.subs(st.zip0(XX)), M1.subz0(XX))
self.assertEqual(M2.subs(st.zip0(XX)), M2.subz0(XX))
konst = sp.Matrix([1, 2, 3])
zz = konst + xx + 5 * yy
self.assertEqual(zz.subz0(xx, yy), konst)
def solve(problem_spec):
""" the design of a linear full observer is based on a linear system.
therefore the non-linear system should first be linearized at the beginning
:param problem_spec: ProblemSpecification object
:return: solution_data: states and output values of the stabilized system
"""
sys_f_body = msp.System_Property() # instance of the class System_Property
sys_f_body.sys_state = problem_spec.xx # state of the system
sys_f_body.tau = problem_spec.u # inputs of the system
# original nonlinear system functions
sys_f_body.n_state_func = problem_spec.rhs(problem_spec.xx, problem_spec.u)
# original output functions
sys_f_body.n_out_func = problem_spec.output_func(problem_spec.xx,
problem_spec.u)
sys_f_body.eqlbr = problem_spec.eqrt # equilibrium point
# linearize nonlinear system around the chosen equilibrium point
sys_f_body.sys_linerazition(parameter_values=None)
tuple_system = (sys_f_body.aa, sys_f_body.bb, sys_f_body.cc, sys_f_body.dd
) # system tuple
# calculate controller function
LQR_res = mlqr.lqr_method(tuple_system,
problem_spec.q,
problem_spec.r,
problem_spec.xx,
problem_spec.eqrt,
problem_spec.yr,
debug=False)
# simulation original nonlinear system with controller
f = sys_f_body.n_state_func.subs(st.zip0(
sys_f_body.tau)) # x_dot = f(x) + g(x) * u
g = sys_f_body.n_state_func.jacobian(sys_f_body.tau)
rhs = rhs_for_simulation(f, g, problem_spec.xx, LQR_res.input_func)
res = odeint(rhs, problem_spec.xx0, problem_spec.tt)
output_function = sp.lambdify(problem_spec.xx,
sys_f_body.n_out_func,
modules='numpy')
yy = output_function(*res.T)
solution_data = SolutionData()
solution_data.res = res # states of system
solution_data.pre_filter = LQR_res.pre_filter # pre-filter
solution_data.state_feedback = LQR_res.state_feedback # controller gain
solution_data.poles = LQR_res.poles_lqr
solution_data.yy = yy[0][0]
return solution_data
def solve(problem_spec):
""" solution of full state feedback
:param problem_spec: ProblemSpecification object
:return: solution_data: states and output values of the stabilized system, and controller gain
"""
sys_f_body = msp.System_Property() # instance of the class System_Property
sys_f_body.sys_state = problem_spec.xx # state of the system
sys_f_body.tau = problem_spec.u # inputs of the system
# original nonlinear system functions
sys_f_body.n_state_func = problem_spec.rhs(problem_spec.xx, problem_spec.u)
# original output functions
sys_f_body.n_out_func = problem_spec.output_func(problem_spec.xx,
problem_spec.u)
sys_f_body.eqlbr = problem_spec.eqrt # equilibrium point
# linearize nonlinear system around the chosen equilibrium point
sys_f_body.sys_linerazition()
tuple_system = (sys_f_body.aa, sys_f_body.bb, sys_f_body.cc, sys_f_body.dd
) # system tuple
# calculate controller function
# sfb for dataclass StateFeedbackResult
sfb = ftf.state_feedback(tuple_system,
problem_spec.poles_cl,
problem_spec.xx,
problem_spec.eqrt,
problem_spec.yr,
debug=False)
# simulation original nonlinear system with controller
f = sys_f_body.n_state_func.subs(st.zip0(
sys_f_body.tau)) # x_dot = f(x) + g(x) * u
g = sys_f_body.n_state_func.jacobian(sys_f_body.tau)
rhs = rhs_for_simulation(f, g, problem_spec.xx, sfb.input_func)
res = odeint(rhs, problem_spec.xx0, problem_spec.tt)
output_function = sp.lambdify(problem_spec.xx,
sys_f_body.n_out_func,
modules='numpy')
yy = output_function(*res.T)
solution_data = SolutionData()
solution_data.res = res # states of system
solution_data.pre_filter = sfb.pre_filter # pre-filter
solution_data.ff = sfb.state_feedback # controller gain
solution_data.input_fun = sfb.input_func # controller function
solution_data.yy = yy[0][0] # system output
return solution_data
def solve_for_acc(self, simplify=False):
self.calc_mass_matrix()
if simplify:
self.M.simplify()
M = self.M
rhs = self.eqns.subs(st.zip0(self.ttdd)) * -1
d = M.berkowitz_det()
adj = M.adjugate()
if simplify:
d = d.simplify()
adj.simplify()
Minv = adj/d
res = Minv*rhs
return res
def solve_for_acc(self, simplify=False):
self.calc_mass_matrix()
if simplify:
self.M.simplify()
M = self.M
rhs = self.eqns.subs(st.zip0(self.ttdd)) * -1
d = M.berkowitz_det()
adj = M.adjugate()
if simplify:
d = d.simplify()
adj.simplify()
Minv = adj / d
res = Minv * rhs
return res
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 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_zip0(self):
aa = sp.symbols('a1:5')
bb = sp.symbols('b1:4')
xx = sp.symbols('x1:3')
s1 = sum(aa)
self.assertEqual(s1.subs(st.zip0(aa)), 0)
self.assertEqual(s1.subs(st.zip0(aa, arg=3.5)), 3.5 * len(aa))
self.assertEqual(s1.subs(st.zip0(aa, arg=xx[0])), xx[0] * len(aa))
s2 = s1 + sum(bb) + sum(xx)
self.assertEqual(s1.subs(st.zip0(aa, bb, xx)), 0)
t2 = s2.subs(st.zip0(aa, bb, xx, arg=-2.1))
self.assertEqual(t2, -2.1 * len(aa + bb + xx))
ff = sp.Function('f1')(*xx), sp.Function('f2')(*xx)
s3 = ff[0] + ff[1] + 10
# noinspection PyUnresolvedReferences
self.assertEqual(s3.subs(st.zip0(ff)), 10)
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_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)
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 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
def solve_bezout_eq(p1, p2, var):
"""
solving the bezout equation
c1*p1 + c2*p2 = 1
for monovariate polynomials p1, p2
by ansatz-polynomials and equating
coefficients
"""
g1 = st.poly_degree(p1, var)
g2 = st.poly_degree(p2, var)
if (not sp.gcd(p1, p2) == 1) and (not p1*p2==0):
# pass
errmsg = "p1, p2 need to be coprime "\
"(condition for Bezout identity to be solveble).\n"\
"p1 = {p1}\n"\
"p2 = {p2}"
raise ValueError(errmsg.format(p1=p1, p2=p2))
if p1 == p2 == 0:
raise ValueError("invalid: p1==p2==0")
if p1 == 0 and g2 > 0:
raise ValueError("invalid: p1==0 and not p2==const ")
if p2 == 0 and g1 > 0:
raise ValueError("invalid: p2==0 and not p1==const ")
# Note: degree(0) = -sp.oo = -inf
k1 = g2 - 1
k2 = g1 - 1
if k1<0 and k2 <0:
if p1 == 0:
k2 = 0
else:
k1 = 0
if k1 == -sp.oo:
k1 = -1
if k2 == -sp.oo:
k2 = -1
cc1 = sp.symbols("c1_0:%i" % (k1 + 1))
cc2 = sp.symbols("c2_0:%i" % (k2 + 1))
c1 = coeff_list_to_poly(cc1, var)
c2 = coeff_list_to_poly(cc2, var)
# Bezout equation:
eq = c1*p1 + c2*p2 - 1
# solve it w.r.t. the unknown coeffs
sol = sp.solve(eq, cc1+cc2, dict=True)
if len(sol) == 0:
errmsg = "No solution found.\n"\
"p1 = {p1}\n"\
"p2 = {p2}"
raise ValueError(errmsg.format(p1=p1, p2=p2))
sol = sol[0]
sol_symbols = st.atoms(sp.Matrix(sol.values()), sp.Symbol)
# there might be some superflous coeffs
free_c_symbols = set(cc1+cc2).intersection(sol_symbols)
if free_c_symbols:
# set them to zero
fcs0 = st.zip0(free_c_symbols)
keys, values = zip(*sol.items())
new_values = [v.subs(fcs0) for v in values]
sol = dict(zip(keys, new_values)+fcs0)
return c1.subs(sol), c2.subs(sol)
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_create_simfunction(self):
x1, x2, x3, x4 = xx = sp.Matrix(sp.symbols("x1, x2, x3, x4"))
u1, u2 = uu = sp.Matrix(sp.symbols("u1, u2")) # inputs
p1, p2, p3, p4 = pp = sp.Matrix(
sp.symbols("p1, p2, p3, p4")) # parameter
t = sp.Symbol('t')
A = A0 = sp.randMatrix(len(xx), len(xx), -10, 10, seed=704)
B = B0 = sp.randMatrix(len(xx), len(uu), -10, 10, seed=705)
v1 = A[0, 0]
A[0, 0] = p1
v2 = A[2, -1]
A[2, -1] = p2
v3 = B[3, 0]
B[3, 0] = p3
v4 = B[2, 1]
B[2, 1] = p4
par_vals = lzip(pp, [v1, v2, v3, v4])
f = A * xx
G = B
fxu = (f + G * uu).subs(par_vals)
# some random initial values
x0 = st.to_np(sp.randMatrix(len(xx), 1, -10, 10, seed=706)).squeeze()
# Test handling of unsubstituted parameters
mod = st.SimulationModel(f, G, xx, model_parameters=par_vals[1:])
with self.assertRaises(ValueError) as cm:
rhs0 = mod.create_simfunction()
self.assertTrue("unexpected symbols" in cm.exception.args[0])
# create the model and the rhs-function
mod = st.SimulationModel(f, G, xx, par_vals)
rhs0 = mod.create_simfunction()
self.assertFalse(mod.compiler_called)
self.assertFalse(mod.use_sp2c)
res0_1 = rhs0(x0, 0)
dres0_1 = st.to_np(fxu.subs(lzip(xx, x0) + st.zip0(uu))).squeeze()
bin_res01 = np.isclose(res0_1, dres0_1) # binary array
self.assertTrue(np.all(bin_res01))
# difference should be [0, 0, ..., 0]
self.assertFalse(np.any(rhs0(x0, 0) - rhs0(x0, 3.7)))
# simulate
tt = np.linspace(0, 0.5,
100) # simulation should be short due to instability
res1 = sc.integrate.odeint(rhs0, x0, tt)
# create and try sympy_to_c bridge (currently only works on linux
# and if sympy_to_c is installed (e.g. with `pip install sympy_to_c`))
# until it is not available for windows we do not want it as a requirement
# see also https://stackoverflow.com/a/10572833/333403
try:
import sympy_to_c
except ImportError:
# noinspection PyUnusedLocal
sympy_to_c = None
sp2c_available = False
else:
sp2c_available = True
if sp2c_available:
rhs0_c = mod.create_simfunction(use_sp2c=True)
self.assertTrue(mod.compiler_called)
res1_c = sc.integrate.odeint(rhs0_c, x0, tt)
self.assertTrue(np.all(np.isclose(res1_c, res1)))
mod.compiler_called = None
rhs0_c = mod.create_simfunction(use_sp2c=True)
self.assertTrue(mod.compiler_called is None)
# proof calculation
# x(t) = x0*exp(A*t)
Anum = st.to_np(A.subs(par_vals))
Bnum = st.to_np(G.subs(par_vals))
# noinspection PyUnresolvedReferences
xt = [np.dot(sc.linalg.expm(Anum * T), x0) for T in tt]
xt = np.array(xt)
# test whether numeric results are close within given tolerance
bin_res1 = np.isclose(res1, xt, rtol=2e-5) # binary array
self.assertTrue(np.all(bin_res1))
# test handling of parameter free models:
mod2 = st.SimulationModel(Anum * xx, Bnum, xx)
rhs2 = mod2.create_simfunction()
res2 = sc.integrate.odeint(rhs2, x0, tt)
self.assertTrue(np.allclose(res1, res2))
# test input functions
des_input = st.piece_wise((0, t <= 1), (t, t < 2), (0.5, t < 3),
(1, True))
des_input_func_scalar = st.expr_to_func(t, des_input)
des_input_func_vec = st.expr_to_func(t,
sp.Matrix([des_input, des_input]))
# noinspection PyUnusedLocal
with self.assertRaises(TypeError) as cm:
mod2.create_simfunction(input_function=des_input_func_scalar)
rhs3 = mod2.create_simfunction(input_function=des_input_func_vec)
# noinspection PyUnusedLocal
res3_0 = rhs3(x0, 0)
rhs4 = mod2.create_simfunction(input_function=des_input_func_vec,
time_direction=-1)
res4_0 = rhs4(x0, 0)
self.assertTrue(np.allclose(res3_0, np.array([119., -18., -36.,
-51.])))
self.assertTrue(np.allclose(res4_0, -res3_0))
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()
