def rhs(self, inv_method=None, **kwargs):
""" Returns equations that can be solved numerically
Parameters
==========
inv_method : str
The specific sympy inverse matrix calculation method to use. For a
list of valid methods, see :py:method:
`~sympy.matrices.matrices.MatrixBase.inv`
"""
if 'method' in kwargs:
#The method kwarg is deprecated in favor of inv_method.
SymPyDeprecationWarning(feature="method kwarg",
useinstead="inv_method kwarg",
deprecated_since_version="0.7.6").warn()
#For now accept both
inv_method = kwargs['method']
if inv_method is None:
self._rhs = _mat_inv_mul(self.mass_matrix_full,
self.forcing_full)
else:
self._rhs = (self.mass_matrix_full.inv(inv_method,
try_block_diag=True) * self.forcing_full)
return self._rhs
def rhs(self, inv_method=None):
""" Returns the system's equations of motion in first order form.
The output of this will be the right hand side of:
[qdot, udot].T = f(q, u, t)
Or, the equations of motion in first order form. The right hand side
is what is needed by most numerical ODE integrators.
Parameters
==========
inv_method : str
The specific sympy inverse matrix calculation method to use. For a
list of valid methods, see :py:method:
`~sympy.matrices.matrices.MatrixBase.inv`
"""
if inv_method is None:
self._rhs = _mat_inv_mul(self.mass_matrix_full,
self.forcing_full)
else:
self._rhs = (self.mass_matrix_full.inv(inv_method,
try_block_diag=True) * self.forcing_full)
return self._rhs
def rhs(self, inv_method=None):
""" Returns the system's equations of motion in first order form.
The output of this will be the right hand side of:
[qdot, udot].T = f(q, u, t)
Or, the equations of motion in first order form. The right hand side
is what is needed by most numerical ODE integrators.
Parameters
==========
inv_method : str
The specific sympy inverse matrix calculation method to use. For a
list of valid methods, see :py:method:
`~sympy.matrices.matrices.MatrixBase.inv`
"""
if inv_method is None:
self._rhs = _mat_inv_mul(self.mass_matrix_full, self.forcing_full)
else:
self._rhs = (
self.mass_matrix_full.inv(inv_method, try_block_diag=True) *
self.forcing_full)
return self._rhs
def test_mat_inv_mul():
# Uses SymPy generated primes as matrix entries, so each entry in
# each matrix should be symbolic and unique, allowing proper comparison.
# Checks _mat_inv_mul against Matrix.inv / Matrix.__mul__.
from sympy import Matrix, prime
# going to form 3 matrices
# 1 n x n
# different n x n
# 1 n x 2n
n = 3
m1 = Matrix(n, n, lambda i, j: prime(i * n + j + 2))
m2 = Matrix(n, n, lambda i, j: prime(i * n + j + 5))
m3 = Matrix(n, n, lambda i, j: prime(i + j * n + 2))
assert _mat_inv_mul(m1, m2) == m1.inv() * m2
assert _mat_inv_mul(m1, m3) == m1.inv() * m3
def test_mat_inv_mul():
# Uses SymPy generated primes as matrix entries, so each entry in
# each matrix should be symbolic and unique, allowing proper comparison.
# Checks _mat_inv_mul against Matrix.inv / Matrix.__mul__.
from sympy import Matrix, prime
# going to form 3 matrices
# 1 n x n
# different n x n
# 1 n x 2n
n = 3
m1 = Matrix(n, n, lambda i, j: prime(i * n + j + 2))
m2 = Matrix(n, n, lambda i, j: prime(i * n + j + 5))
m3 = Matrix(n, n, lambda i, j: prime(i + j * n + 2))
assert _mat_inv_mul(m1, m2) == m1.inv() * m2
assert _mat_inv_mul(m1, m3) == m1.inv() * m3
def _kindiffeq(self, kdeqs):
"""Supply all the kinematic differential equations in a list.
They should be in the form [Expr1, Expr2, ...] where Expri is equal to
zero
Parameters
==========
kdeqs : list (of Expr)
The listof kinematic differential equations
"""
if len(self._q) != len(kdeqs):
raise ValueError('There must be an equal number of kinematic '
'differential equations and coordinates.')
uaux = self._uaux
# dictionary of auxiliary speeds which are equal to zero
uaz = dict(list(zip(uaux, [0] * len(uaux))))
#kdeqs = Matrix(kdeqs).subs(uaz)
kdeqs = Matrix(kdeqs)
qdot = self._qdot
qdotzero = dict(list(zip(qdot, [0] * len(qdot))))
u = self._u
uzero = dict(list(zip(u, [0] * len(u))))
f_k = kdeqs.subs(uzero).subs(qdotzero)
k_kqdot = (kdeqs.subs(uzero) - f_k).jacobian(Matrix(qdot))
k_ku = (kdeqs.subs(qdotzero) - f_k).jacobian(Matrix(u))
self._k_ku = _mat_inv_mul(k_kqdot, k_ku)
self._f_k = _mat_inv_mul(k_kqdot, f_k)
self._k_kqdot = eye(len(qdot))
self._qdot_u_map = solve_linear_system_LU(
Matrix([
self._k_kqdot.T, -(self._k_ku * Matrix(self._u) + self._f_k).T
]).T, self._qdot)
self._k_ku = _mat_inv_mul(k_kqdot, k_ku).subs(uaz)
self._f_k = _mat_inv_mul(k_kqdot, f_k).subs(uaz)
def _kindiffeq(self, kdeqs):
"""Supply all the kinematic differential equations in a list.
They should be in the form [Expr1, Expr2, ...] where Expri is equal to
zero
Parameters
==========
kdeqs : list (of Expr)
The listof kinematic differential equations
"""
if len(self._q) != len(kdeqs):
raise ValueError('There must be an equal number of kinematic '
'differential equations and coordinates.')
uaux = self._uaux
# dictionary of auxiliary speeds which are equal to zero
uaz = dict(list(zip(uaux, [0] * len(uaux))))
#kdeqs = Matrix(kdeqs).subs(uaz)
kdeqs = Matrix(kdeqs)
qdot = self._qdot
qdotzero = dict(list(zip(qdot, [0] * len(qdot))))
u = self._u
uzero = dict(list(zip(u, [0] * len(u))))
f_k = kdeqs.subs(uzero).subs(qdotzero)
k_kqdot = (kdeqs.subs(uzero) - f_k).jacobian(Matrix(qdot))
k_ku = (kdeqs.subs(qdotzero) - f_k).jacobian(Matrix(u))
self._k_ku = _mat_inv_mul(k_kqdot, k_ku)
self._f_k = _mat_inv_mul(k_kqdot, f_k)
self._k_kqdot = eye(len(qdot))
self._qdot_u_map = solve_linear_system_LU(Matrix([self._k_kqdot.T,
-(self._k_ku * Matrix(self._u) + self._f_k).T]).T, self._qdot)
self._k_ku = _mat_inv_mul(k_kqdot, k_ku).subs(uaz)
self._f_k = _mat_inv_mul(k_kqdot, f_k).subs(uaz)
def linearize(self):
""" Method used to generate linearized equations.
Note that for linearization, it is assumed that time is not perturbed,
but only coordinates and positions. The "forcing" vector's jacobian is
computed with respect to the state vector in the form [Qi, Qd, Ui, Ud].
This is the "f_lin_A" matrix.
It also finds any non-state dynamicsymbols and computes the jacobian of
the "forcing" vector with respect to them. This is the "f_lin_B"
matrix; if this is empty, an empty matrix is created.
Consider the following:
If our equations are: [M]qudot = f, where [M] is the full mass matrix,
qudot is a vector of the deriatives of the coordinates and speeds, and
f in the full forcing vector, the linearization process is as follows:
[M]qudot = [f_lin_A]qu + [f_lin_B]y, where qu is the state vector,
f_lin_A is the jacobian of the full forcing vector with respect to the
state vector, f_lin_B is the jacobian of the full forcing vector with
respect to any non-speed/coordinate dynamicsymbols which show up in the
full forcing vector, and y is a vector of those dynamic symbols (each
column in f_lin_B corresponds to a row of the y vector, each of which
is a non-speed/coordinate dynamicsymbol).
To get the traditional state-space A and B matrix, you need to multiply
the f_lin_A and f_lin_B matrices by the inverse of the mass matrix.
Caution needs to be taken when inverting large symbolic matrices;
substituting in numerical values before inverting will work better.
A tuple of (f_lin_A, f_lin_B, other_dynamicsymbols) is returned.
"""
if (self._fr is None) or (self._frstar is None):
raise ValueError('Need to compute Fr, Fr* first.')
# Note that this is now unneccessary, and it should never be
# encountered; I still think it should be in here in case the user
# manually sets these matrices incorrectly.
for i in self._q:
if self._k_kqdot.diff(i) != 0 * self._k_kqdot:
raise ValueError('Matrix K_kqdot must not depend on any q.')
t = dynamicsymbols._t
uaux = self._uaux
uauxdot = [diff(i, t) for i in uaux]
# dictionary of auxiliary speeds & derivatives which are equal to zero
subdict = dict(list(zip(uaux + uauxdot, [0] * (len(uaux) + len(uauxdot)))))
# Checking for dynamic symbols outside the dynamic differential
# equations; throws error if there is.
insyms = set(
self._q + self._qdot + self._u + self._udot + uaux + uauxdot)
if any(self._find_dynamicsymbols(i, insyms) for i in [self._k_kqdot,
self._k_ku,
self._f_k,
self._k_dnh,
self._f_dnh,
self._k_d]):
raise ValueError('Cannot have dynamicsymbols outside dynamic '
'forcing vector.')
other_dyns = list(self._find_dynamicsymbols(self._f_d.subs(subdict),
insyms))
# make it canonically ordered so the jacobian is canonical
other_dyns.sort(key=default_sort_key)
for i in other_dyns:
if diff(i, dynamicsymbols._t) in other_dyns:
raise ValueError('Cannot have derivatives of specified '
'quantities when linearizing forcing terms.')
o = len(self._u) # number of speeds
n = len(self._q) # number of coordinates
l = len(self._qdep) # number of configuration constraints
m = len(self._udep) # number of motion constraints
qi = Matrix(self._q[: n - l]) # independent coords
qd = Matrix(self._q[n - l: n]) # dependent coords; could be empty
ui = Matrix(self._u[: o - m]) # independent speeds
ud = Matrix(self._u[o - m: o]) # dependent speeds; could be empty
qdot = Matrix(self._qdot) # time derivatives of coordinates
# with equations in the form MM udot = forcing, expand that to:
# MM_full [q,u].T = forcing_full. This combines coordinates and
# speeds together for the linearization, which is necessary for the
# linearization process, due to dependent coordinates. f1 is the rows
# from the kinematic differential equations, f2 is the rows from the
# dynamic differential equations (and differentiated non-holonomic
# constraints).
f1 = self._k_ku * Matrix(self._u) + self._f_k
f2 = self._f_d
# Only want to do this if these matrices have been filled in, which
# occurs when there are dependent speeds
if m != 0:
f2 = self._f_d.col_join(self._f_dnh)
fnh = self._f_nh + self._k_nh * Matrix(self._u)
f1 = f1.subs(subdict)
f2 = f2.subs(subdict)
fh = self._f_h.subs(subdict)
fku = (self._k_ku * Matrix(self._u)).subs(subdict)
fkf = self._f_k.subs(subdict)
# In the code below, we are applying the chain rule by hand on these
# things. All the matrices have been changed into vectors (by
# multiplying the dynamic symbols which it is paired with), so we can
# take the jacobian of them. The basic operation is take the jacobian
# of the f1, f2 vectors wrt all of the q's and u's. f1 is a function of
# q, u, and t; f2 is a function of q, qdot, u, and t. In the code
# below, we are not considering perturbations in t. So if f1 is a
# function of the q's, u's but some of the q's or u's could be
# dependent on other q's or u's (qd's might be dependent on qi's, ud's
# might be dependent on ui's or qi's), so what we do is take the
# jacobian of the f1 term wrt qi's and qd's, the jacobian wrt the qd's
# gets multiplied by the jacobian of qd wrt qi, this is extended for
# the ud's as well. dqd_dqi is computed by taking a taylor expansion of
# the holonomic constraint equations about q*, treating q* - q as dq,
# seperating into dqd (depedent q's) and dqi (independent q's) and the
# rearranging for dqd/dqi. This is again extended for the speeds.
# First case: configuration and motion constraints
if (l != 0) and (m != 0):
fh_jac_qi = fh.jacobian(qi)
fh_jac_qd = fh.jacobian(qd)
fnh_jac_qi = fnh.jacobian(qi)
fnh_jac_qd = fnh.jacobian(qd)
fnh_jac_ui = fnh.jacobian(ui)
fnh_jac_ud = fnh.jacobian(ud)
fku_jac_qi = fku.jacobian(qi)
fku_jac_qd = fku.jacobian(qd)
fku_jac_ui = fku.jacobian(ui)
fku_jac_ud = fku.jacobian(ud)
fkf_jac_qi = fkf.jacobian(qi)
fkf_jac_qd = fkf.jacobian(qd)
f1_jac_qi = f1.jacobian(qi)
f1_jac_qd = f1.jacobian(qd)
f1_jac_ui = f1.jacobian(ui)
f1_jac_ud = f1.jacobian(ud)
f2_jac_qi = f2.jacobian(qi)
f2_jac_qd = f2.jacobian(qd)
f2_jac_ui = f2.jacobian(ui)
f2_jac_ud = f2.jacobian(ud)
f2_jac_qdot = f2.jacobian(qdot)
dqd_dqi = - _mat_inv_mul(fh_jac_qd, fh_jac_qi)
dud_dqi = _mat_inv_mul(fnh_jac_ud, (fnh_jac_qd *
dqd_dqi - fnh_jac_qi))
dud_dui = - _mat_inv_mul(fnh_jac_ud, fnh_jac_ui)
dqdot_dui = - self._k_kqdot.inv() * (fku_jac_ui +
fku_jac_ud * dud_dui)
dqdot_dqi = - self._k_kqdot.inv() * (fku_jac_qi + fkf_jac_qi +
(fku_jac_qd + fkf_jac_qd) * dqd_dqi + fku_jac_ud * dud_dqi)
f1_q = f1_jac_qi + f1_jac_qd * dqd_dqi + f1_jac_ud * dud_dqi
f1_u = f1_jac_ui + f1_jac_ud * dud_dui
f2_q = (f2_jac_qi + f2_jac_qd * dqd_dqi + f2_jac_qdot * dqdot_dqi +
f2_jac_ud * dud_dqi)
f2_u = f2_jac_ui + f2_jac_ud * dud_dui + f2_jac_qdot * dqdot_dui
# Second case: configuration constraints only
elif l != 0:
dqd_dqi = - _mat_inv_mul(fh.jacobian(qd), fh.jacobian(qi))
dqdot_dui = - self._k_kqdot.inv() * fku.jacobian(ui)
dqdot_dqi = - self._k_kqdot.inv() * (fku.jacobian(qi) +
fkf.jacobian(qi) + (fku.jacobian(qd) + fkf.jacobian(qd)) *
dqd_dqi)
f1_q = (f1.jacobian(qi) + f1.jacobian(qd) * dqd_dqi)
f1_u = f1.jacobian(ui)
f2_jac_qdot = f2.jacobian(qdot)
f2_q = (f2.jacobian(qi) + f2.jacobian(qd) * dqd_dqi +
f2.jac_qdot * dqdot_dqi)
f2_u = f2.jacobian(ui) + f2_jac_qdot * dqdot_dui
# Third case: motion constraints only
elif m != 0:
dud_dqi = _mat_inv_mul(fnh.jacobian(ud), - fnh.jacobian(qi))
dud_dui = - _mat_inv_mul(fnh.jacobian(ud), fnh.jacobian(ui))
dqdot_dui = - self._k_kqdot.inv() * (fku.jacobian(ui) +
fku.jacobian(ud) * dud_dui)
dqdot_dqi = - self._k_kqdot.inv() * (fku.jacobian(qi) +
fkf.jacobian(qi) + fku.jacobian(ud) * dud_dqi)
f1_jac_ud = f1.jacobian(ud)
f2_jac_qdot = f2.jacobian(qdot)
f2_jac_ud = f2.jacobian(ud)
f1_q = f1.jacobian(qi) + f1_jac_ud * dud_dqi
f1_u = f1.jacobian(ui) + f1_jac_ud * dud_dui
f2_q = (f2.jacobian(qi) + f2_jac_qdot * dqdot_dqi + f2_jac_ud
* dud_dqi)
f2_u = (f2.jacobian(ui) + f2_jac_ud * dud_dui + f2_jac_qdot *
dqdot_dui)
# Fourth case: No constraints
else:
dqdot_dui = - self._k_kqdot.inv() * fku.jacobian(ui)
dqdot_dqi = - self._k_kqdot.inv() * (fku.jacobian(qi) +
fkf.jacobian(qi))
f1_q = f1.jacobian(qi)
f1_u = f1.jacobian(ui)
f2_jac_qdot = f2.jacobian(qdot)
f2_q = f2.jacobian(qi) + f2_jac_qdot * dqdot_dqi
f2_u = f2.jacobian(ui) + f2_jac_qdot * dqdot_dui
f_lin_A = -(f1_q.row_join(f1_u)).col_join(f2_q.row_join(f2_u))
if other_dyns:
f1_oths = f1.jacobian(other_dyns)
f2_oths = f2.jacobian(other_dyns)
f_lin_B = -f1_oths.col_join(f2_oths)
else:
f_lin_B = Matrix([])
return (f_lin_A, f_lin_B, Matrix(other_dyns))
def _speeds(self, uind, udep=[], coneqs=[], diffconeqs=None, u_auxiliary=[]):
"""Supply all the generalized speeds in a list.
If there are motion constraints or auxiliary speeds, they are provided
here as well (as well as motion constraints).
Parameters
==========
uind : list
A list of independent generalized speeds
udep : list
Optional list of dependent speeds
coneqs : list
Optional List of constraint expressions; these are expressions
which are equal to zero which define a speed (motion) constraint.
diffconeqs : list
Optional, calculated automatically otherwise; list of constraint
equations; again equal to zero, but define an acceleration
constraint.
u_auxiliary : list
An optional list of auxiliary speeds used for brining
non-contributing forces into evidence
"""
if not hasattr(uind, '__iter__'):
raise TypeError('Supply generalized speeds in an iterable.')
self._u = uind + udep
self._udot = [diff(i, dynamicsymbols._t) for i in self._u]
self._uaux = u_auxiliary
if not hasattr(udep, '__iter__'):
raise TypeError('Supply dependent speeds in an iterable.')
if len(udep) != len(coneqs):
raise ValueError('There must be an equal number of dependent '
'speeds and constraints.')
if diffconeqs is not None:
if len(udep) != len(diffconeqs):
raise ValueError('There must be an equal number of dependent '
'speeds and constraints.')
if len(udep) != 0:
u = self._u
uzero = dict(list(zip(u, [0] * len(u))))
coneqs = Matrix(coneqs)
udot = self._udot
udotzero = dict(list(zip(udot, [0] * len(udot))))
self._udep = udep
self._f_nh = coneqs.subs(uzero)
self._k_nh = (coneqs - self._f_nh).jacobian(u)
# if no differentiated non holonomic constraints were given, calculate
if diffconeqs is None:
self._k_dnh = self._k_nh
self._f_dnh = (self._k_nh.diff(dynamicsymbols._t) * Matrix(u) +
self._f_nh.diff(dynamicsymbols._t))
else:
self._f_dnh = diffconeqs.subs(udotzero)
self._k_dnh = (diffconeqs - self._f_dnh).jacobian(udot)
o = len(u) # number of generalized speeds
m = len(udep) # number of motion constraints
p = o - m # number of independent speeds
# For a reminder, form of non-holonomic constraints is:
# B u + C = 0
B = self._k_nh[:, :]
C = self._f_nh[:, 0]
# We partition B into indenpendent and dependent columns
# Ars is then -Bdep.inv() * Bind, and it relates depedent speeds to
# independent speeds as: udep = Ars uind, neglecting the C term here.
self._depB = B
self._depC = C
mr1 = B[:, :p]
ml1 = B[:, p:o]
self._Ars = - _mat_inv_mul(ml1, mr1)
def _old_linearize(self):
"""Old method to linearize the equations of motion. Returns a tuple of
(f_lin_A, f_lin_B, y) for forming [M]qudot = [f_lin_A]qu + [f_lin_B]y.
Deprecated in favor of new method using Linearizer class. Please change
your code to use the new `linearize` method."""
if (self._fr is None) or (self._frstar is None):
raise ValueError('Need to compute Fr, Fr* first.')
# Note that this is now unneccessary, and it should never be
# encountered; I still think it should be in here in case the user
# manually sets these matrices incorrectly.
for i in self._q:
if self._k_kqdot.diff(i) != 0 * self._k_kqdot:
raise ValueError('Matrix K_kqdot must not depend on any q.')
t = dynamicsymbols._t
uaux = self._uaux
uauxdot = [diff(i, t) for i in uaux]
# dictionary of auxiliary speeds & derivatives which are equal to zero
subdict = dict(
list(zip(uaux + uauxdot, [0] * (len(uaux) + len(uauxdot)))))
# Checking for dynamic symbols outside the dynamic differential
# equations; throws error if there is.
insyms = set(self._q + self._qdot + self._u + self._udot + uaux +
uauxdot)
if any(
self._find_dynamicsymbols(i, insyms) for i in [
self._k_kqdot, self._k_ku, self._f_k, self._k_dnh,
self._f_dnh, self._k_d
]):
raise ValueError('Cannot have dynamicsymbols outside dynamic '
'forcing vector.')
other_dyns = list(
self._find_dynamicsymbols(self._f_d.subs(subdict), insyms))
# make it canonically ordered so the jacobian is canonical
other_dyns.sort(key=default_sort_key)
for i in other_dyns:
if diff(i, dynamicsymbols._t) in other_dyns:
raise ValueError('Cannot have derivatives of specified '
'quantities when linearizing forcing terms.')
o = len(self._u) # number of speeds
n = len(self._q) # number of coordinates
l = len(self._qdep) # number of configuration constraints
m = len(self._udep) # number of motion constraints
qi = Matrix(self._q[:n - l]) # independent coords
qd = Matrix(self._q[n - l:n]) # dependent coords; could be empty
ui = Matrix(self._u[:o - m]) # independent speeds
ud = Matrix(self._u[o - m:o]) # dependent speeds; could be empty
qdot = Matrix(self._qdot) # time derivatives of coordinates
# with equations in the form MM udot = forcing, expand that to:
# MM_full [q,u].T = forcing_full. This combines coordinates and
# speeds together for the linearization, which is necessary for the
# linearization process, due to dependent coordinates. f1 is the rows
# from the kinematic differential equations, f2 is the rows from the
# dynamic differential equations (and differentiated non-holonomic
# constraints).
f1 = self._k_ku * Matrix(self._u) + self._f_k
f2 = self._f_d
# Only want to do this if these matrices have been filled in, which
# occurs when there are dependent speeds
if m != 0:
f2 = self._f_d.col_join(self._f_dnh)
fnh = self._f_nh + self._k_nh * Matrix(self._u)
f1 = f1.subs(subdict)
f2 = f2.subs(subdict)
fh = self._f_h.subs(subdict)
fku = (self._k_ku * Matrix(self._u)).subs(subdict)
fkf = self._f_k.subs(subdict)
# In the code below, we are applying the chain rule by hand on these
# things. All the matrices have been changed into vectors (by
# multiplying the dynamic symbols which it is paired with), so we can
# take the jacobian of them. The basic operation is take the jacobian
# of the f1, f2 vectors wrt all of the q's and u's. f1 is a function of
# q, u, and t; f2 is a function of q, qdot, u, and t. In the code
# below, we are not considering perturbations in t. So if f1 is a
# function of the q's, u's but some of the q's or u's could be
# dependent on other q's or u's (qd's might be dependent on qi's, ud's
# might be dependent on ui's or qi's), so what we do is take the
# jacobian of the f1 term wrt qi's and qd's, the jacobian wrt the qd's
# gets multiplied by the jacobian of qd wrt qi, this is extended for
# the ud's as well. dqd_dqi is computed by taking a taylor expansion of
# the holonomic constraint equations about q*, treating q* - q as dq,
# separating into dqd (depedent q's) and dqi (independent q's) and the
# rearranging for dqd/dqi. This is again extended for the speeds.
# First case: configuration and motion constraints
if (l != 0) and (m != 0):
fh_jac_qi = fh.jacobian(qi)
fh_jac_qd = fh.jacobian(qd)
fnh_jac_qi = fnh.jacobian(qi)
fnh_jac_qd = fnh.jacobian(qd)
fnh_jac_ui = fnh.jacobian(ui)
fnh_jac_ud = fnh.jacobian(ud)
fku_jac_qi = fku.jacobian(qi)
fku_jac_qd = fku.jacobian(qd)
fku_jac_ui = fku.jacobian(ui)
fku_jac_ud = fku.jacobian(ud)
fkf_jac_qi = fkf.jacobian(qi)
fkf_jac_qd = fkf.jacobian(qd)
f1_jac_qi = f1.jacobian(qi)
f1_jac_qd = f1.jacobian(qd)
f1_jac_ui = f1.jacobian(ui)
f1_jac_ud = f1.jacobian(ud)
f2_jac_qi = f2.jacobian(qi)
f2_jac_qd = f2.jacobian(qd)
f2_jac_ui = f2.jacobian(ui)
f2_jac_ud = f2.jacobian(ud)
f2_jac_qdot = f2.jacobian(qdot)
dqd_dqi = -_mat_inv_mul(fh_jac_qd, fh_jac_qi)
dud_dqi = _mat_inv_mul(fnh_jac_ud,
(fnh_jac_qd * dqd_dqi - fnh_jac_qi))
dud_dui = -_mat_inv_mul(fnh_jac_ud, fnh_jac_ui)
dqdot_dui = -self._k_kqdot.inv() * (fku_jac_ui +
fku_jac_ud * dud_dui)
dqdot_dqi = -self._k_kqdot.inv() * (
fku_jac_qi + fkf_jac_qi +
(fku_jac_qd + fkf_jac_qd) * dqd_dqi + fku_jac_ud * dud_dqi)
f1_q = f1_jac_qi + f1_jac_qd * dqd_dqi + f1_jac_ud * dud_dqi
f1_u = f1_jac_ui + f1_jac_ud * dud_dui
f2_q = (f2_jac_qi + f2_jac_qd * dqd_dqi + f2_jac_qdot * dqdot_dqi +
f2_jac_ud * dud_dqi)
f2_u = f2_jac_ui + f2_jac_ud * dud_dui + f2_jac_qdot * dqdot_dui
# Second case: configuration constraints only
elif l != 0:
dqd_dqi = -_mat_inv_mul(fh.jacobian(qd), fh.jacobian(qi))
dqdot_dui = -self._k_kqdot.inv() * fku.jacobian(ui)
dqdot_dqi = -self._k_kqdot.inv() * (
fku.jacobian(qi) + fkf.jacobian(qi) +
(fku.jacobian(qd) + fkf.jacobian(qd)) * dqd_dqi)
f1_q = (f1.jacobian(qi) + f1.jacobian(qd) * dqd_dqi)
f1_u = f1.jacobian(ui)
f2_jac_qdot = f2.jacobian(qdot)
f2_q = (f2.jacobian(qi) + f2.jacobian(qd) * dqd_dqi +
f2.jac_qdot * dqdot_dqi)
f2_u = f2.jacobian(ui) + f2_jac_qdot * dqdot_dui
# Third case: motion constraints only
elif m != 0:
dud_dqi = _mat_inv_mul(fnh.jacobian(ud), -fnh.jacobian(qi))
dud_dui = -_mat_inv_mul(fnh.jacobian(ud), fnh.jacobian(ui))
dqdot_dui = -self._k_kqdot.inv() * (fku.jacobian(ui) +
fku.jacobian(ud) * dud_dui)
dqdot_dqi = -self._k_kqdot.inv() * (fku.jacobian(qi) +
fkf.jacobian(qi) +
fku.jacobian(ud) * dud_dqi)
f1_jac_ud = f1.jacobian(ud)
f2_jac_qdot = f2.jacobian(qdot)
f2_jac_ud = f2.jacobian(ud)
f1_q = f1.jacobian(qi) + f1_jac_ud * dud_dqi
f1_u = f1.jacobian(ui) + f1_jac_ud * dud_dui
f2_q = (f2.jacobian(qi) + f2_jac_qdot * dqdot_dqi +
f2_jac_ud * dud_dqi)
f2_u = (f2.jacobian(ui) + f2_jac_ud * dud_dui +
f2_jac_qdot * dqdot_dui)
# Fourth case: No constraints
else:
dqdot_dui = -self._k_kqdot.inv() * fku.jacobian(ui)
dqdot_dqi = -self._k_kqdot.inv() * (fku.jacobian(qi) +
fkf.jacobian(qi))
f1_q = f1.jacobian(qi)
f1_u = f1.jacobian(ui)
f2_jac_qdot = f2.jacobian(qdot)
f2_q = f2.jacobian(qi) + f2_jac_qdot * dqdot_dqi
f2_u = f2.jacobian(ui) + f2_jac_qdot * dqdot_dui
f_lin_A = -(f1_q.row_join(f1_u)).col_join(f2_q.row_join(f2_u))
if other_dyns:
f1_oths = f1.jacobian(other_dyns)
f2_oths = f2.jacobian(other_dyns)
f_lin_B = -f1_oths.col_join(f2_oths)
else:
f_lin_B = Matrix([])
return (f_lin_A, f_lin_B, Matrix(other_dyns))
def _speeds(self,
uind,
udep=[],
coneqs=[],
diffconeqs=None,
u_auxiliary=[]):
"""Supply all the generalized speeds in a list.
If there are motion constraints or auxiliary speeds, they are provided
here as well (as well as motion constraints).
Parameters
==========
uind : list
A list of independent generalized speeds
udep : list
Optional list of dependent speeds
coneqs : list
Optional List of constraint expressions; these are expressions
which are equal to zero which define a speed (motion) constraint.
diffconeqs : list
Optional, calculated automatically otherwise; list of constraint
equations; again equal to zero, but define an acceleration
constraint.
u_auxiliary : list
An optional list of auxiliary speeds used for brining
non-contributing forces into evidence
"""
if not hasattr(uind, '__iter__'):
raise TypeError('Supply generalized speeds in an iterable.')
self._u = uind + udep
self._udot = [diff(i, dynamicsymbols._t) for i in self._u]
self._uaux = u_auxiliary
if not hasattr(udep, '__iter__'):
raise TypeError('Supply dependent speeds in an iterable.')
if len(udep) != len(coneqs):
raise ValueError('There must be an equal number of dependent '
'speeds and constraints.')
if diffconeqs is not None:
if len(udep) != len(diffconeqs):
raise ValueError('There must be an equal number of dependent '
'speeds and constraints.')
if len(udep) != 0:
u = self._u
uzero = dict(list(zip(u, [0] * len(u))))
coneqs = Matrix(coneqs)
udot = self._udot
udotzero = dict(list(zip(udot, [0] * len(udot))))
self._udep = udep
self._f_nh = coneqs.subs(uzero)
self._k_nh = (coneqs - self._f_nh).jacobian(u)
# if no differentiated non holonomic constraints were given, calculate
if diffconeqs is None:
self._k_dnh = self._k_nh
self._f_dnh = (self._k_nh.diff(dynamicsymbols._t) * Matrix(u) +
self._f_nh.diff(dynamicsymbols._t))
else:
self._f_dnh = diffconeqs.subs(udotzero)
self._k_dnh = (diffconeqs - self._f_dnh).jacobian(udot)
o = len(u) # number of generalized speeds
m = len(udep) # number of motion constraints
p = o - m # number of independent speeds
# For a reminder, form of non-holonomic constraints is:
# B u + C = 0
B = self._k_nh[:, :]
C = self._f_nh[:, 0]
# We partition B into indenpendent and dependent columns
# Ars is then -Bdep.inv() * Bind, and it relates depedent speeds to
# independent speeds as: udep = Ars uind, neglecting the C term here.
self._depB = B
self._depC = C
mr1 = B[:, :p]
ml1 = B[:, p:o]
self._Ars = -_mat_inv_mul(ml1, mr1)