def _form_fr(self, fl):
"""Form the generalized active force."""
if not iterable(fl):
raise TypeError('Force pairs must be supplied in an iterable.')
N = self._inertial
# pull out relevant velocities for constructing partial velocities
vel_list, f_list = _f_list_parser(fl, N)
vel_list = [i.subs(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._fr = FR
return FR
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 form_lagranges_equations(self):
"""Method to form Lagrange's equations of motion.
Returns a vector of equations of motion using Lagrange's equations of
the second kind.
"""
qds = self._qdots
qdd_zero = dict((i, 0) for i in self._qdoubledots)
n = len(self.q)
# Internally we represent the EOM as four terms:
# EOM = term1 - term2 - term3 - term4 = 0
# First term
self._term1 = self._L.jacobian(qds)
self._term1 = self._term1.diff(dynamicsymbols._t).T
# Second term
self._term2 = self._L.jacobian(self.q).T
# Third term
if self.coneqs:
coneqs = self.coneqs
m = len(coneqs)
# Creating the multipliers
self.lam_vec = Matrix(dynamicsymbols('lam1:' + str(m + 1)))
self.lam_coeffs = -coneqs.jacobian(qds)
self._term3 = self.lam_coeffs.T * self.lam_vec
# Extracting the coeffecients of the qdds from the diff coneqs
diffconeqs = coneqs.diff(dynamicsymbols._t)
self._m_cd = diffconeqs.jacobian(self._qdoubledots)
# The remaining terms i.e. the 'forcing' terms in diff coneqs
self._f_cd = -diffconeqs.subs(qdd_zero)
else:
self._term3 = zeros(n, 1)
# Fourth term
if self.forcelist:
N = self.inertial
self._term4 = zeros(n, 1)
flist = zip(*_f_list_parser(self.forcelist, N))
for i, qd in enumerate(qds):
for obj, force in self.forcelist:
self._term4[i] = sum(v.diff(qd, N) & f for (v, f) in flist)
else:
self._term4 = zeros(n, 1)
# Form the dynamic mass and forcing matrices
without_lam = self._term1 - self._term2 - self._term4
self._m_d = without_lam.jacobian(self._qdoubledots)
self._f_d = -without_lam.subs(qdd_zero)
# Form the EOM
self.eom = without_lam - self._term3
return self.eom
def form_lagranges_equations(self):
"""Method to form Lagrange's equations of motion.
Returns a vector of equations of motion using Lagrange's equations of
the second kind.
"""
qds = self._qdots
qdd_zero = dict((i, 0) for i in self._qdoubledots)
n = len(self.q)
# Internally we represent the EOM as four terms:
# EOM = term1 - term2 - term3 - term4 = 0
# First term
self._term1 = self._L.jacobian(qds)
self._term1 = self._term1.diff(dynamicsymbols._t).T
# Second term
self._term2 = self._L.jacobian(self.q).T
# Third term
if self.coneqs:
coneqs = self.coneqs
m = len(coneqs)
# Creating the multipliers
self.lam_vec = Matrix(dynamicsymbols('lam1:' + str(m + 1)))
self.lam_coeffs = -coneqs.jacobian(qds)
self._term3 = self.lam_coeffs.T * self.lam_vec
# Extracting the coeffecients of the qdds from the diff coneqs
diffconeqs = coneqs.diff(dynamicsymbols._t)
self._m_cd = diffconeqs.jacobian(self._qdoubledots)
# The remaining terms i.e. the 'forcing' terms in diff coneqs
self._f_cd = -diffconeqs.subs(qdd_zero)
else:
self._term3 = zeros(n, 1)
# Fourth term
if self.forcelist:
N = self.inertial
self._term4 = zeros(n, 1)
flist = zip(*_f_list_parser(self.forcelist, N))
for i, qd in enumerate(qds):
for obj, force in self.forcelist:
self._term4[i] = sum(v.diff(qd, N) & f for (v, f) in flist)
else:
self._term4 = zeros(n, 1)
# Form the dynamic mass and forcing matrices
without_lam = self._term1 - self._term2 - self._term4
self._m_d = without_lam.jacobian(self._qdoubledots)
self._f_d = -without_lam.subs(qdd_zero)
# Form the EOM
self.eom = without_lam - self._term3
return self.eom
