def _form_fr(self, fl):
"""Form the generalized active force."""
if fl is not None and (len(fl) == 0 or not iterable(fl)):
raise ValueError('Force pairs must be supplied in an '
'non-empty iterable or None.')
N = self._inertial
# pull out relevant velocities for constructing partial velocities
vel_list, f_list = _f_list_parser(fl, N)
vel_list = [msubs(i, self._qdot_u_map) for i in vel_list]
f_list = [msubs(i, self._qdot_u_map) for i in f_list]
# Fill Fr with dot product of partial velocities and forces
o = len(self.u)
b = len(f_list)
FR = zeros(o, 1)
partials = partial_velocity(vel_list, self.u, N)
for i in range(o):
FR[i] = sum(partials[j][i] & f_list[j] for j in range(b))
# In case there are dependent speeds
if self._udep:
p = o - len(self._udep)
FRtilde = FR[:p, 0]
FRold = FR[p:o, 0]
FRtilde += self._Ars.T * FRold
FR = FRtilde
self._forcelist = fl
self._fr = FR
return FR
def _initialize_kindiffeq_matrices(self, kdeqs):
"""Initialize the kinematic differential equation matrices."""
if kdeqs:
if len(self.q) != len(kdeqs):
raise ValueError('There must be an equal number of kinematic '
'differential equations and coordinates.')
kdeqs = Matrix(kdeqs)
u = self.u
qdot = self._qdot
# Dictionaries setting things to zero
u_zero = dict((i, 0) for i in u)
uaux_zero = dict((i, 0) for i in self._uaux)
qdot_zero = dict((i, 0) for i in qdot)
f_k = msubs(kdeqs, u_zero, qdot_zero)
k_ku = (msubs(kdeqs, qdot_zero) - f_k).jacobian(u)
k_kqdot = (msubs(kdeqs, u_zero) - f_k).jacobian(qdot)
f_k = k_kqdot.LUsolve(f_k)
k_ku = k_kqdot.LUsolve(k_ku)
k_kqdot = eye(len(qdot))
self._qdot_u_map = solve_linear_system_LU(
Matrix([k_kqdot.T, -(k_ku * u + f_k).T]).T, qdot)
self._f_k = msubs(f_k, uaux_zero)
self._k_ku = msubs(k_ku, uaux_zero)
self._k_kqdot = k_kqdot
else:
self._qdot_u_map = None
self._f_k = Matrix()
self._k_ku = Matrix()
self._k_kqdot = Matrix()
def _initialize_kindiffeq_matrices(self, kdeqs):
"""Initialize the kinematic differential equation matrices."""
if kdeqs:
if len(self.q) != len(kdeqs):
raise ValueError('There must be an equal number of kinematic '
'differential equations and coordinates.')
kdeqs = Matrix(kdeqs)
u = self.u
qdot = self._qdot
# Dictionaries setting things to zero
u_zero = dict((i, 0) for i in u)
uaux_zero = dict((i, 0) for i in self._uaux)
qdot_zero = dict((i, 0) for i in qdot)
f_k = msubs(kdeqs, u_zero, qdot_zero)
k_ku = (msubs(kdeqs, qdot_zero) - f_k).jacobian(u)
k_kqdot = (msubs(kdeqs, u_zero) - f_k).jacobian(qdot)
f_k = k_kqdot.LUsolve(f_k)
k_ku = k_kqdot.LUsolve(k_ku)
k_kqdot = eye(len(qdot))
self._qdot_u_map = solve_linear_system_LU(
Matrix([k_kqdot.T, -(k_ku * u + f_k).T]).T, qdot)
self._f_k = msubs(f_k, uaux_zero)
self._k_ku = msubs(k_ku, uaux_zero)
self._k_kqdot = k_kqdot
else:
self._qdot_u_map = None
self._f_k = Matrix()
self._k_ku = Matrix()
self._k_kqdot = Matrix()
def solve_multipliers(self, op_point=None, sol_type="dict"):
"""Solves for the values of the lagrange multipliers symbolically at
the specified operating point
Parameters
==========
op_point : dict or iterable of dicts, optional
Point at which to solve at. The operating point is specified as
a dictionary or iterable of dictionaries of {symbol: value}. The
value may be numeric or symbolic itself.
sol_type : str, optional
Solution return type. Valid options are:
- 'dict': A dict of {symbol : value} (default)
- 'Matrix': An ordered column matrix of the solution
"""
# Determine number of multipliers
k = len(self.lam_vec)
if k == 0:
raise ValueError("System has no lagrange multipliers to solve for.")
# Compose dict of operating conditions
if isinstance(op_point, dict):
op_point_dict = op_point
elif iterable(op_point):
op_point_dict = {}
for op in op_point:
op_point_dict.update(op)
elif op_point is None:
op_point_dict = {}
else:
raise TypeError(
"op_point must be either a dictionary or an "
"iterable of dictionaries."
)
# Compose the system to be solved
mass_matrix = self.mass_matrix.col_join(
(-self.lam_coeffs.row_join(zeros(k, k)))
)
force_matrix = self.forcing.col_join(self._f_cd)
# Sub in the operating point
mass_matrix = msubs(mass_matrix, op_point_dict)
force_matrix = msubs(force_matrix, op_point_dict)
# Solve for the multipliers
sol_list = mass_matrix.LUsolve(-force_matrix)[-k:]
if sol_type == "dict":
return dict(zip(self.lam_vec, sol_list))
elif sol_type == "Matrix":
return Matrix(sol_list)
else:
raise ValueError("Unknown sol_type {:}.".format(sol_type))
def solve_multipliers(self, op_point=None, sol_type='dict'):
"""Solves for the values of the lagrange multipliers symbolically at
the specified operating point
Parameters
==========
op_point : dict or iterable of dicts, optional
Point at which to solve at. The operating point is specified as
a dictionary or iterable of dictionaries of {symbol: value}. The
value may be numeric or symbolic itself.
sol_type : str, optional
Solution return type. Valid options are:
- 'dict': A dict of {symbol : value} (default)
- 'Matrix': An ordered column matrix of the solution
"""
# Determine number of multipliers
k = len(self.lam_vec)
if k == 0:
raise ValueError("System has no lagrange multipliers to solve for.")
# Compose dict of operating conditions
if isinstance(op_point, dict):
op_point_dict = op_point
elif iterable(op_point):
op_point_dict = {}
for op in op_point:
op_point_dict.update(op)
elif op_point is None:
op_point_dict = {}
else:
raise TypeError("op_point must be either a dictionary or an "
"iterable of dictionaries.")
# Compose the system to be solved
mass_matrix = self.mass_matrix.col_join((-self.lam_coeffs.row_join(
zeros(k, k))))
force_matrix = self.forcing.col_join(self._f_cd)
# Sub in the operating point
mass_matrix = msubs(mass_matrix, op_point_dict)
force_matrix = msubs(force_matrix, op_point_dict)
# Solve for the multipliers
sol_list = mass_matrix.LUsolve(-force_matrix)[-k:]
if sol_type == 'dict':
return dict(zip(self.lam_vec, sol_list))
elif sol_type == 'Matrix':
return Matrix(sol_list)
else:
raise ValueError("Unknown sol_type {:}.".format(sol_type))
def _form_fr(self, fl):
"""Form the generalized active force."""
if fl != None and (len(fl) == 0 or not iterable(fl)):
raise ValueError('Force pairs must be supplied in an '
'non-empty iterable or None.')
N = self._inertial
# pull out relevant velocities for constructing partial velocities
vel_list, f_list = _f_list_parser(fl, N)
vel_list = [msubs(i, self._qdot_u_map) for i in vel_list]
# Fill Fr with dot product of partial velocities and forces
o = len(self.u)
b = len(f_list)
FR = zeros(o, 1)
partials = partial_velocity(vel_list, self.u, N)
for i in range(o):
FR[i] = sum(partials[j][i] & f_list[j] for j in range(b))
# In case there are dependent speeds
if self._udep:
p = o - len(self._udep)
FRtilde = FR[:p, 0]
FRold = FR[p:o, 0]
FRtilde += self._Ars.T * FRold
FR = FRtilde
self._forcelist = fl
self._fr = FR
return FR
def get_partial_velocity(body):
if isinstance(body, RigidBody):
vlist = [body.masscenter.vel(N), body.frame.ang_vel_in(N)]
elif isinstance(body, Particle):
vlist = [body.point.vel(N),]
else:
raise TypeError('The body list may only contain either '
'RigidBody or Particle as list elements.')
v = [msubs(vel, self._qdot_u_map) for vel in vlist]
return partial_velocity(v, self.u, N)
def get_partial_velocity(body):
if isinstance(body, RigidBody):
vlist = [body.masscenter.vel(N), body.frame.ang_vel_in(N)]
elif isinstance(body, Particle):
vlist = [body.point.vel(N),]
else:
raise TypeError('The body list may only contain either '
'RigidBody or Particle as list elements.')
v = [msubs(vel, self._qdot_u_map) for vel in vlist]
return partial_velocity(v, self.u, N)
def to_linearizer(self):
"""Returns an instance of the Linearizer class, initiated from the
data in the KanesMethod class. This may be more desirable than using
the linearize class method, as the Linearizer object will allow more
efficient recalculation (i.e. about varying operating points)."""
if (self._fr is None) or (self._frstar is None):
raise ValueError('Need to compute Fr, Fr* first.')
# Get required equation components. The Kane's method class breaks
# these into pieces. Need to reassemble
f_c = self._f_h
if self._f_nh and self._k_nh:
f_v = self._f_nh + self._k_nh * Matrix(self._u)
else:
f_v = Matrix()
if self._f_dnh and self._k_dnh:
f_a = self._f_dnh + self._k_dnh * Matrix(self._udot)
else:
f_a = Matrix()
# Dicts to sub to zero, for splitting up expressions
u_zero = dict((i, 0) for i in self._u)
ud_zero = dict((i, 0) for i in self._udot)
qd_zero = dict((i, 0) for i in self._qdot)
qd_u_zero = dict((i, 0) for i in Matrix([self._qdot, self._u]))
# Break the kinematic differential eqs apart into f_0 and f_1
f_0 = msubs(self._f_k, u_zero) + self._k_kqdot * Matrix(self._qdot)
f_1 = msubs(self._f_k, qd_zero) + self._k_ku * Matrix(self._u)
# Break the dynamic differential eqs into f_2 and f_3
f_2 = msubs(self._frstar, qd_u_zero)
f_3 = msubs(self._frstar, ud_zero) + self._fr
f_4 = zeros(len(f_2), 1)
# Get the required vector components
q = self._q
u = self._u
if self._qdep:
q_i = q[:-len(self._qdep)]
else:
q_i = q
q_d = self._qdep
if self._udep:
u_i = u[:-len(self._udep)]
else:
u_i = u
u_d = self._udep
# Form dictionary to set auxiliary speeds & their derivatives to 0.
uaux = self._uaux
uauxdot = uaux.diff(dynamicsymbols._t)
uaux_zero = dict((i, 0) for i in Matrix([uaux, uauxdot]))
# Checking for dynamic symbols outside the dynamic differential
# equations; throws error if there is.
sym_list = set(Matrix([q, self._qdot, u, self._udot, uaux, uauxdot]))
if any(
find_dynamicsymbols(i, sym_list) 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.')
# Find all other dynamic symbols, forming the forcing vector r.
# Sort r to make it canonical.
r = list(find_dynamicsymbols(self._f_d.subs(uaux_zero), sym_list))
r.sort(key=default_sort_key)
# Check for any derivatives of variables in r that are also found in r.
for i in r:
if diff(i, dynamicsymbols._t) in r:
raise ValueError('Cannot have derivatives of specified \
quantities when linearizing forcing terms.')
return Linearizer(f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i,
q_d, u_i, u_d, r)
def _form_frstar(self, bl):
"""Form the generalized inertia force."""
if not iterable(bl):
raise TypeError('Bodies must be supplied in an iterable.')
t = dynamicsymbols._t
N = self._inertial
# Dicts setting things to zero
udot_zero = dict((i, 0) for i in self._udot)
uaux_zero = dict((i, 0) for i in self._uaux)
uauxdot = [diff(i, t) for i in self._uaux]
uauxdot_zero = dict((i, 0) for i in uauxdot)
# Dictionary of q' and q'' to u and u'
q_ddot_u_map = dict(
(k.diff(t), v.diff(t)) for (k, v) in self._qdot_u_map.items())
q_ddot_u_map.update(self._qdot_u_map)
# Fill up the list of partials: format is a list with num elements
# equal to number of entries in body list. Each of these elements is a
# list - either of length 1 for the translational components of
# particles or of length 2 for the translational and rotational
# components of rigid bodies. The inner most list is the list of
# partial velocities.
def get_partial_velocity(body):
if isinstance(body, RigidBody):
vlist = [body.masscenter.vel(N), body.frame.ang_vel_in(N)]
elif isinstance(body, Particle):
vlist = [
body.point.vel(N),
]
else:
raise TypeError('The body list may only contain either '
'RigidBody or Particle as list elements.')
v = [vel.subs(self._qdot_u_map) for vel in vlist]
return partial_velocity(v, self._u, N)
partials = [get_partial_velocity(body) for body in bl]
# Compute fr_star in two components:
# fr_star = -(MM*u' + nonMM)
o = len(self._u)
MM = zeros(o, o)
nonMM = zeros(o, 1)
zero_uaux = lambda expr: expr.subs(uaux_zero)
zero_udot_uaux = lambda expr: expr.subs(udot_zero).subs(uaux_zero)
for i, body in enumerate(bl):
if isinstance(body, RigidBody):
M = zero_uaux(body.mass)
I = zero_uaux(body.central_inertia)
vel = zero_uaux(body.masscenter.vel(N))
omega = zero_uaux(body.frame.ang_vel_in(N))
acc = zero_udot_uaux(body.masscenter.acc(N))
inertial_force = (M.diff(t) * vel + M * acc)
inertial_torque = zero_uaux((I.dt(body.frame) & omega) + (
I & body.frame.ang_acc_in(N)).subs(udot_zero) +
(omega ^ (I & omega)))
for j in range(o):
tmp_vel = zero_uaux(partials[i][0][j])
tmp_ang = zero_uaux(I & partials[i][1][j])
for k in range(o):
# translational
MM[j, k] += M * (tmp_vel & partials[i][0][k])
# rotational
MM[j, k] += (tmp_ang & partials[i][1][k])
nonMM[j] += inertial_force & partials[i][0][j]
nonMM[j] += inertial_torque & partials[i][1][j]
else:
M = zero_uaux(body.mass)
vel = zero_uaux(body.point.vel(N))
acc = zero_udot_uaux(body.point.acc(N))
inertial_force = (M.diff(t) * vel + M * acc)
for j in range(o):
temp = zero_uaux(partials[i][0][j])
for k in range(o):
MM[j, k] += M * (temp & partials[i][0][k])
nonMM[j] += inertial_force & partials[i][0][j]
# Compose fr_star out of MM and nonMM
MM = zero_uaux(msubs(MM, q_ddot_u_map))
nonMM = msubs(msubs(nonMM, q_ddot_u_map), udot_zero, uauxdot_zero,
uaux_zero)
fr_star = -(MM * Matrix(self._udot).subs(uauxdot_zero) + nonMM)
# If there are dependent speeds, we need to find fr_star_tilde
if self._udep:
p = o - len(self._udep)
fr_star_ind = fr_star[:p, 0]
fr_star_dep = fr_star[p:o, 0]
fr_star = fr_star_ind + (self._Ars.T * fr_star_dep)
# Apply the same to MM
MMi = MM[:p, :]
MMd = MM[p:o, :]
MM = MMi + (self._Ars.T * MMd)
self._frstar = fr_star
self._k_d = MM
self._f_d = -msubs(self._fr + self._frstar, udot_zero)
return fr_star
def linearize(self, op_point=None, A_and_B=False, simplify=False):
"""Linearize the system about the operating point. Note that
q_op, u_op, qd_op, ud_op must satisfy the equations of motion.
These may be either symbolic or numeric.
Parameters
----------
op_point : dict or iterable of dicts, optional
Dictionary or iterable of dictionaries containing the operating
point conditions. These will be substituted in to the linearized
system before the linearization is complete. Leave blank if you
want a completely symbolic form. Note that any reduction in
symbols (whether substituted for numbers or expressions with a
common parameter) will result in faster runtime.
A_and_B : bool, optional
If A_and_B=False (default), (M, A, B) is returned for forming
[M]*[q, u]^T = [A]*[q_ind, u_ind]^T + [B]r. If A_and_B=True,
(A, B) is returned for forming dx = [A]x + [B]r, where
x = [q_ind, u_ind]^T.
simplify : bool, optional
Determines if returned values are simplified before return.
For large expressions this may be time consuming. Default is False.
Potential Issues
----------------
Note that the process of solving with A_and_B=True is
computationally intensive if there are many symbolic parameters.
For this reason, it may be more desirable to use the default
A_and_B=False, returning M, A, and B. More values may then be
substituted in to these matrices later on. The state space form can
then be found as A = P.T*M.LUsolve(A), B = P.T*M.LUsolve(B), where
P = Linearizer.perm_mat.
"""
# Run the setup if needed:
if not self._setup_done:
self._setup()
# Compose dict of operating conditions
if isinstance(op_point, dict):
op_point_dict = op_point
elif isinstance(op_point, Iterable):
op_point_dict = {}
for op in op_point:
op_point_dict.update(op)
else:
op_point_dict = {}
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Rename terms to shorten expressions
M_qq = self._M_qq
M_uqc = self._M_uqc
M_uqd = self._M_uqd
M_uuc = self._M_uuc
M_uud = self._M_uud
M_uld = self._M_uld
A_qq = self._A_qq
A_uqc = self._A_uqc
A_uqd = self._A_uqd
A_qu = self._A_qu
A_uuc = self._A_uuc
A_uud = self._A_uud
B_u = self._B_u
C_0 = self._C_0
C_1 = self._C_1
C_2 = self._C_2
# Build up Mass Matrix
# |M_qq 0_nxo 0_nxk|
# M = |M_uqc M_uuc 0_mxk|
# |M_uqd M_uud M_uld|
if o != 0:
col2 = Matrix([zeros(n, o), M_uuc, M_uud])
if k != 0:
col3 = Matrix([zeros(n + m, k), M_uld])
if n != 0:
col1 = Matrix([M_qq, M_uqc, M_uqd])
if o != 0 and k != 0:
M = col1.row_join(col2).row_join(col3)
elif o != 0:
M = col1.row_join(col2)
else:
M = col1
elif k != 0:
M = col2.row_join(col3)
else:
M = col2
M_eq = msubs(M, op_point_dict)
# Build up state coefficient matrix A
# |(A_qq + A_qu*C_1)*C_0 A_qu*C_2|
# A = |(A_uqc + A_uuc*C_1)*C_0 A_uuc*C_2|
# |(A_uqd + A_uud*C_1)*C_0 A_uud*C_2|
# Col 1 is only defined if n != 0
if n != 0:
r1c1 = A_qq
if o != 0:
r1c1 += (A_qu * C_1)
r1c1 = r1c1 * C_0
if m != 0:
r2c1 = A_uqc
if o != 0:
r2c1 += (A_uuc * C_1)
r2c1 = r2c1 * C_0
else:
r2c1 = Matrix()
if o - m + k != 0:
r3c1 = A_uqd
if o != 0:
r3c1 += (A_uud * C_1)
r3c1 = r3c1 * C_0
else:
r3c1 = Matrix()
col1 = Matrix([r1c1, r2c1, r3c1])
else:
col1 = Matrix()
# Col 2 is only defined if o != 0
if o != 0:
if n != 0:
r1c2 = A_qu * C_2
else:
r1c2 = Matrix()
if m != 0:
r2c2 = A_uuc * C_2
else:
r2c2 = Matrix()
if o - m + k != 0:
r3c2 = A_uud * C_2
else:
r3c2 = Matrix()
col2 = Matrix([r1c2, r2c2, r3c2])
else:
col2 = Matrix()
if col1:
if col2:
Amat = col1.row_join(col2)
else:
Amat = col1
else:
Amat = col2
Amat_eq = msubs(Amat, op_point_dict)
# Build up the B matrix if there are forcing variables
# |0_(n + m)xs|
# B = |B_u |
if s != 0 and o - m + k != 0:
Bmat = zeros(n + m, s).col_join(B_u)
Bmat_eq = msubs(Bmat, op_point_dict)
else:
Bmat_eq = Matrix()
# kwarg A_and_B indicates to return A, B for forming the equation
# dx = [A]x + [B]r, where x = [q_indnd, u_indnd]^T,
import sympy
if A_and_B:
A_cont = self.perm_mat.T * M_eq.LUsolve(Amat_eq)
if Bmat_eq:
B_cont = self.perm_mat.T * M_eq.LUsolve(Bmat_eq)
else:
# Bmat = Matrix([]), so no need to sub
B_cont = Bmat_eq
if simplify:
sympy.simplify(A_cont)
sympy.simplify(B_cont)
return A_cont, B_cont
# Otherwise return M, A, B for forming the equation
# [M]dx = [A]x + [B]r, where x = [q, u]^T
else:
if simplify:
sympy.simplify(M_eq)
sympy.simplify(Amat_eq)
sympy.simplify(Bmat_eq)
return M_eq, Amat_eq, Bmat_eq
def to_linearizer(self):
"""Returns an instance of the Linearizer class, initiated from the
data in the KanesMethod class. This may be more desirable than using
the linearize class method, as the Linearizer object will allow more
efficient recalculation (i.e. about varying operating points)."""
if (self._fr is None) or (self._frstar is None):
raise ValueError('Need to compute Fr, Fr* first.')
# Get required equation components. The Kane's method class breaks
# these into pieces. Need to reassemble
f_c = self._f_h
if self._f_nh and self._k_nh:
f_v = self._f_nh + self._k_nh*Matrix(self.u)
else:
f_v = Matrix()
if self._f_dnh and self._k_dnh:
f_a = self._f_dnh + self._k_dnh*Matrix(self._udot)
else:
f_a = Matrix()
# Dicts to sub to zero, for splitting up expressions
u_zero = dict((i, 0) for i in self.u)
ud_zero = dict((i, 0) for i in self._udot)
qd_zero = dict((i, 0) for i in self._qdot)
qd_u_zero = dict((i, 0) for i in Matrix([self._qdot, self.u]))
# Break the kinematic differential eqs apart into f_0 and f_1
f_0 = msubs(self._f_k, u_zero) + self._k_kqdot*Matrix(self._qdot)
f_1 = msubs(self._f_k, qd_zero) + self._k_ku*Matrix(self.u)
# Break the dynamic differential eqs into f_2 and f_3
f_2 = msubs(self._frstar, qd_u_zero)
f_3 = msubs(self._frstar, ud_zero) + self._fr
f_4 = zeros(len(f_2), 1)
# Get the required vector components
q = self.q
u = self.u
if self._qdep:
q_i = q[:-len(self._qdep)]
else:
q_i = q
q_d = self._qdep
if self._udep:
u_i = u[:-len(self._udep)]
else:
u_i = u
u_d = self._udep
# Form dictionary to set auxiliary speeds & their derivatives to 0.
uaux = self._uaux
uauxdot = uaux.diff(dynamicsymbols._t)
uaux_zero = dict((i, 0) for i in Matrix([uaux, uauxdot]))
# Checking for dynamic symbols outside the dynamic differential
# equations; throws error if there is.
sym_list = set(Matrix([q, self._qdot, u, self._udot, uaux, uauxdot]))
if any(find_dynamicsymbols(i, sym_list) 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.')
# Find all other dynamic symbols, forming the forcing vector r.
# Sort r to make it canonical.
r = list(find_dynamicsymbols(msubs(self._f_d, uaux_zero), sym_list))
r.sort(key=default_sort_key)
# Check for any derivatives of variables in r that are also found in r.
for i in r:
if diff(i, dynamicsymbols._t) in r:
raise ValueError('Cannot have derivatives of specified \
quantities when linearizing forcing terms.')
return Linearizer(f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i,
q_d, u_i, u_d, r)
def _form_frstar(self, bl):
"""Form the generalized inertia force."""
if not iterable(bl):
raise TypeError('Bodies must be supplied in an iterable.')
t = dynamicsymbols._t
N = self._inertial
# Dicts setting things to zero
udot_zero = dict((i, 0) for i in self._udot)
uaux_zero = dict((i, 0) for i in self._uaux)
uauxdot = [diff(i, t) for i in self._uaux]
uauxdot_zero = dict((i, 0) for i in uauxdot)
# Dictionary of q' and q'' to u and u'
q_ddot_u_map = dict((k.diff(t), v.diff(t)) for (k, v) in
self._qdot_u_map.items())
q_ddot_u_map.update(self._qdot_u_map)
# Fill up the list of partials: format is a list with num elements
# equal to number of entries in body list. Each of these elements is a
# list - either of length 1 for the translational components of
# particles or of length 2 for the translational and rotational
# components of rigid bodies. The inner most list is the list of
# partial velocities.
def get_partial_velocity(body):
if isinstance(body, RigidBody):
vlist = [body.masscenter.vel(N), body.frame.ang_vel_in(N)]
elif isinstance(body, Particle):
vlist = [body.point.vel(N),]
else:
raise TypeError('The body list may only contain either '
'RigidBody or Particle as list elements.')
v = [msubs(vel, self._qdot_u_map) for vel in vlist]
return partial_velocity(v, self.u, N)
partials = [get_partial_velocity(body) for body in bl]
# Compute fr_star in two components:
# fr_star = -(MM*u' + nonMM)
o = len(self.u)
MM = zeros(o, o)
nonMM = zeros(o, 1)
zero_uaux = lambda expr: msubs(expr, uaux_zero)
zero_udot_uaux = lambda expr: msubs(msubs(expr, udot_zero), uaux_zero)
for i, body in enumerate(bl):
if isinstance(body, RigidBody):
M = zero_uaux(body.mass)
I = zero_uaux(body.central_inertia)
vel = zero_uaux(body.masscenter.vel(N))
omega = zero_uaux(body.frame.ang_vel_in(N))
acc = zero_udot_uaux(body.masscenter.acc(N))
inertial_force = (M.diff(t) * vel + M * acc)
inertial_torque = zero_uaux((I.dt(body.frame) & omega) +
msubs(I & body.frame.ang_acc_in(N), udot_zero) +
(omega ^ (I & omega)))
for j in range(o):
tmp_vel = zero_uaux(partials[i][0][j])
tmp_ang = zero_uaux(I & partials[i][1][j])
for k in range(o):
# translational
MM[j, k] += M * (tmp_vel & partials[i][0][k])
# rotational
MM[j, k] += (tmp_ang & partials[i][1][k])
nonMM[j] += inertial_force & partials[i][0][j]
nonMM[j] += inertial_torque & partials[i][1][j]
else:
M = zero_uaux(body.mass)
vel = zero_uaux(body.point.vel(N))
acc = zero_udot_uaux(body.point.acc(N))
inertial_force = (M.diff(t) * vel + M * acc)
for j in range(o):
temp = zero_uaux(partials[i][0][j])
for k in range(o):
MM[j, k] += M * (temp & partials[i][0][k])
nonMM[j] += inertial_force & partials[i][0][j]
# Compose fr_star out of MM and nonMM
MM = zero_uaux(msubs(MM, q_ddot_u_map))
nonMM = msubs(msubs(nonMM, q_ddot_u_map),
udot_zero, uauxdot_zero, uaux_zero)
fr_star = -(MM * msubs(Matrix(self._udot), uauxdot_zero) + nonMM)
# If there are dependent speeds, we need to find fr_star_tilde
if self._udep:
p = o - len(self._udep)
fr_star_ind = fr_star[:p, 0]
fr_star_dep = fr_star[p:o, 0]
fr_star = fr_star_ind + (self._Ars.T * fr_star_dep)
# Apply the same to MM
MMi = MM[:p, :]
MMd = MM[p:o, :]
MM = MMi + (self._Ars.T * MMd)
self._bodylist = bl
self._frstar = fr_star
self._k_d = MM
self._f_d = -msubs(self._fr + self._frstar, udot_zero)
return fr_star
def _initialize_constraint_matrices(self, config, vel, acc):
"""Initializes constraint matrices."""
# Define vector dimensions
o = len(self.u)
m = len(self._udep)
p = o - m
none_handler = lambda x: Matrix(x) if x else Matrix()
# Initialize configuration constraints
config = none_handler(config)
if len(self._qdep) != len(config):
raise ValueError('There must be an equal number of dependent '
'coordinates and configuration constraints.')
self._f_h = none_handler(config)
# Initialize velocity and acceleration constraints
vel = none_handler(vel)
acc = none_handler(acc)
if len(vel) != m:
raise ValueError('There must be an equal number of dependent '
'speeds and velocity constraints.')
if acc and (len(acc) != m):
raise ValueError('There must be an equal number of dependent '
'speeds and acceleration constraints.')
if vel:
u_zero = dict((i, 0) for i in self.u)
udot_zero = dict((i, 0) for i in self._udot)
# When calling kanes_equations, another class instance will be
# created if auxiliary u's are present. In this case, the
# computation of kinetic differential equation matrices will be
# skipped as this was computed during the original KanesMethod
# object, and the qd_u_map will not be available.
if self._qdot_u_map is not None:
vel = msubs(vel, self._qdot_u_map)
self._f_nh = msubs(vel, u_zero)
self._k_nh = (vel - self._f_nh).jacobian(self.u)
# If no acceleration constraints given, calculate them.
if not acc:
self._f_dnh = (self._k_nh.diff(dynamicsymbols._t) * self.u +
self._f_nh.diff(dynamicsymbols._t))
self._k_dnh = self._k_nh
else:
if self._qdot_u_map is not None:
acc = msubs(acc, self._qdot_u_map)
self._f_dnh = msubs(acc, udot_zero)
self._k_dnh = (acc - self._f_dnh).jacobian(self._udot)
# Form of non-holonomic constraints is B*u + C = 0.
# We partition B into independent and dependent columns:
# Ars is then -B_dep.inv() * B_ind, and it relates dependent speeds
# to independent speeds as: udep = Ars*uind, neglecting the C term.
B_ind = self._k_nh[:, :p]
B_dep = self._k_nh[:, p:o]
self._Ars = -B_dep.LUsolve(B_ind)
else:
self._f_nh = Matrix()
self._k_nh = Matrix()
self._f_dnh = Matrix()
self._k_dnh = Matrix()
self._Ars = Matrix()
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(
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(
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(
find_dynamicsymbols(msubs(self._f_d, 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 = msubs(f1, subdict)
f2 = msubs(f2, subdict)
fh = msubs(self._f_h, subdict)
fku = msubs(self._k_ku * Matrix(self.u), subdict)
fkf = msubs(self._f_k, 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 = -fh_jac_qd.LUsolve(fh_jac_qi)
dud_dqi = fnh_jac_ud.LUsolve(fnh_jac_qd * dqd_dqi - fnh_jac_qi)
dud_dui = -fnh_jac_ud.LUsolve(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 = -fh.jacobian(qd).LUsolve(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 = fnh.jacobian(ud).LUsolve(-fnh.jacobian(qi))
dud_dui = -fnh.jacobian(ud).LUsolve(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 linearize(self, op_point=None, A_and_B=False, simplify=False):
"""Linearize the system about the operating point. Note that
q_op, u_op, qd_op, ud_op must satisfy the equations of motion.
These may be either symbolic or numeric.
Parameters
----------
op_point : dict or iterable of dicts, optional
Dictionary or iterable of dictionaries containing the operating
point conditions. These will be substituted in to the linearized
system before the linearization is complete. Leave blank if you
want a completely symbolic form. Note that any reduction in
symbols (whether substituted for numbers or expressions with a
common parameter) will result in faster runtime.
A_and_B : bool, optional
If A_and_B=False (default), (M, A, B) is returned for forming
[M]*[q, u]^T = [A]*[q_ind, u_ind]^T + [B]r. If A_and_B=True,
(A, B) is returned for forming dx = [A]x + [B]r, where
x = [q_ind, u_ind]^T.
simplify : bool, optional
Determines if returned values are simplified before return.
For large expressions this may be time consuming. Default is False.
Potential Issues
----------------
Note that the process of solving with A_and_B=True is
computationally intensive if there are many symbolic parameters.
For this reason, it may be more desirable to use the default
A_and_B=False, returning M, A, and B. More values may then be
substituted in to these matrices later on. The state space form can
then be found as A = P.T*M.LUsolve(A), B = P.T*M.LUsolve(B), where
P = Linearizer.perm_mat.
"""
# Run the setup if needed:
if not self._setup_done:
self._setup()
# Compose dict of operating conditions
if isinstance(op_point, dict):
op_point_dict = op_point
elif isinstance(op_point, collections.Iterable):
op_point_dict = {}
for op in op_point:
op_point_dict.update(op)
else:
op_point_dict = {}
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Rename terms to shorten expressions
M_qq = self._M_qq
M_uqc = self._M_uqc
M_uqd = self._M_uqd
M_uuc = self._M_uuc
M_uud = self._M_uud
M_uld = self._M_uld
A_qq = self._A_qq
A_uqc = self._A_uqc
A_uqd = self._A_uqd
A_qu = self._A_qu
A_uuc = self._A_uuc
A_uud = self._A_uud
B_u = self._B_u
C_0 = self._C_0
C_1 = self._C_1
C_2 = self._C_2
# Build up Mass Matrix
# |M_qq 0_nxo 0_nxk|
# M = |M_uqc M_uuc 0_mxk|
# |M_uqd M_uud M_uld|
if o != 0:
col2 = Matrix([zeros(n, o), M_uuc, M_uud])
if k != 0:
col3 = Matrix([zeros(n + m, k), M_uld])
if n != 0:
col1 = Matrix([M_qq, M_uqc, M_uqd])
if o != 0 and k != 0:
M = col1.row_join(col2).row_join(col3)
elif o != 0:
M = col1.row_join(col2)
else:
M = col1
elif k != 0:
M = col2.row_join(col3)
else:
M = col2
M_eq = msubs(M, op_point_dict)
# Build up state coefficient matrix A
# |(A_qq + A_qu*C_1)*C_0 A_qu*C_2|
# A = |(A_uqc + A_uuc*C_1)*C_0 A_uuc*C_2|
# |(A_uqd + A_uud*C_1)*C_0 A_uud*C_2|
# Col 1 is only defined if n != 0
if n != 0:
r1c1 = A_qq
if o != 0:
r1c1 += (A_qu * C_1)
r1c1 = r1c1 * C_0
if m != 0:
r2c1 = A_uqc
if o != 0:
r2c1 += (A_uuc * C_1)
r2c1 = r2c1 * C_0
else:
r2c1 = Matrix()
if o - m + k != 0:
r3c1 = A_uqd
if o != 0:
r3c1 += (A_uud * C_1)
r3c1 = r3c1 * C_0
else:
r3c1 = Matrix()
col1 = Matrix([r1c1, r2c1, r3c1])
else:
col1 = Matrix()
# Col 2 is only defined if o != 0
if o != 0:
if n != 0:
r1c2 = A_qu * C_2
else:
r1c2 = Matrix()
if m != 0:
r2c2 = A_uuc * C_2
else:
r2c2 = Matrix()
if o - m + k != 0:
r3c2 = A_uud * C_2
else:
r3c2 = Matrix()
col2 = Matrix([r1c2, r2c2, r3c2])
else:
col2 = Matrix()
if col1:
if col2:
Amat = col1.row_join(col2)
else:
Amat = col1
else:
Amat = col2
Amat_eq = msubs(Amat, op_point_dict)
# Build up the B matrix if there are forcing variables
# |0_(n + m)xs|
# B = |B_u |
if s != 0 and o - m + k != 0:
Bmat = zeros(n + m, s).col_join(B_u)
Bmat_eq = msubs(Bmat, op_point_dict)
else:
Bmat_eq = Matrix()
# kwarg A_and_B indicates to return A, B for forming the equation
# dx = [A]x + [B]r, where x = [q_indnd, u_indnd]^T,
if A_and_B:
A_cont = self.perm_mat.T * M_eq.LUsolve(Amat_eq)
if Bmat_eq:
B_cont = self.perm_mat.T * M_eq.LUsolve(Bmat_eq)
else:
# Bmat = Matrix([]), so no need to sub
B_cont = Bmat_eq
if simplify:
A_cont.simplify()
B_cont.simplify()
return A_cont, B_cont
# Otherwise return M, A, B for forming the equation
# [M]dx = [A]x + [B]r, where x = [q, u]^T
else:
if simplify:
M_eq.simplify()
Amat_eq.simplify()
Bmat_eq.simplify()
return M_eq, Amat_eq, Bmat_eq
def _initialize_constraint_matrices(self, config, vel, acc):
"""Initializes constraint matrices."""
# Define vector dimensions
o = len(self.u)
m = len(self._udep)
p = o - m
none_handler = lambda x: Matrix(x) if x else Matrix()
# Initialize configuration constraints
config = none_handler(config)
if len(self._qdep) != len(config):
raise ValueError('There must be an equal number of dependent '
'coordinates and configuration constraints.')
self._f_h = none_handler(config)
# Initialize velocity and acceleration constraints
vel = none_handler(vel)
acc = none_handler(acc)
if len(vel) != m:
raise ValueError('There must be an equal number of dependent '
'speeds and velocity constraints.')
if acc and (len(acc) != m):
raise ValueError('There must be an equal number of dependent '
'speeds and acceleration constraints.')
if vel:
u_zero = dict((i, 0) for i in self.u)
udot_zero = dict((i, 0) for i in self._udot)
# When calling kanes_equations, another class instance will be
# created if auxiliary u's are present. In this case, the
# computation of kinetic differential equation matrices will be
# skipped as this was computed during the original KanesMethod
# object, and the qd_u_map will not be available.
if self._qdot_u_map is not None:
vel = msubs(vel, self._qdot_u_map)
self._f_nh = msubs(vel, u_zero)
self._k_nh = (vel - self._f_nh).jacobian(self.u)
# If no acceleration constraints given, calculate them.
if not acc:
self._f_dnh = (self._k_nh.diff(dynamicsymbols._t) * self.u +
self._f_nh.diff(dynamicsymbols._t))
self._k_dnh = self._k_nh
else:
if self._qdot_u_map is not None:
acc = msubs(acc, self._qdot_u_map)
self._f_dnh = msubs(acc, udot_zero)
self._k_dnh = (acc - self._f_dnh).jacobian(self._udot)
# Form of non-holonomic constraints is B*u + C = 0.
# We partition B into independent and dependent columns:
# Ars is then -B_dep.inv() * B_ind, and it relates dependent speeds
# to independent speeds as: udep = Ars*uind, neglecting the C term.
B_ind = self._k_nh[:, :p]
B_dep = self._k_nh[:, p:o]
self._Ars = -B_dep.LUsolve(B_ind)
else:
self._f_nh = Matrix()
self._k_nh = Matrix()
self._f_dnh = Matrix()
self._k_dnh = Matrix()
self._Ars = Matrix()
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(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(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(find_dynamicsymbols(msubs(self._f_d, 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 = msubs(f1, subdict)
f2 = msubs(f2, subdict)
fh = msubs(self._f_h, subdict)
fku = msubs(self._k_ku * Matrix(self.u), subdict)
fkf = msubs(self._f_k, 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 = - fh_jac_qd.LUsolve(fh_jac_qi)
dud_dqi = fnh_jac_ud.LUsolve(fnh_jac_qd * dqd_dqi - fnh_jac_qi)
dud_dui = - fnh_jac_ud.LUsolve(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 = - fh.jacobian(qd).LUsolve(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 = fnh.jacobian(ud).LUsolve(- fnh.jacobian(qi))
dud_dui = - fnh.jacobian(ud).LUsolve(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))
