def _partial_velocity(self, vlist, ulist, frame):
"""Returns the list of partial velocities, replacing qdot's in the
velocity list if necessary.
"""
if self._qdot_u_map is None:
raise ___KDEqError
v = [vel.subs(self._qdot_u_map) for vel in vlist]
return partial_velocity(v, ulist, frame)
def _partial_velocity(self, vlist, ulist, frame):
"""Returns the list of partial velocities, replacing qdot's in the
velocity list if necessary.
"""
if self._qdot_u_map is None:
raise ___KDEqError
v = [vel.subs(self._qdot_u_map) for vel in vlist]
return partial_velocity(v, ulist, frame)
def _form_fr(self, fl):
"""Form the generalized active force.
Computes the vector of the generalized active force vector.
Used to compute E.o.M. in the form Fr + Fr* = 0.
Parameters
==========
fl : list
Takes in a list of (Point, Vector) or (ReferenceFrame, Vector)
tuples which represent the force at a point or torque on a frame.
"""
if not hasattr(fl, '__iter__'):
raise TypeError('Force pairs must be supplied in an iterable.')
N = self._inertial
self._forcelist = fl[:]
u = self._u
o = len(u) # number of gen. speeds
b = len(fl) # number of forces
FR = zeros(o, 1)
# pull out relevant velocities for constructing partial velocities
vel_list = []
f_list = []
for i in fl:
if isinstance(i[0], ReferenceFrame):
vel_list += [i[0].ang_vel_in(N)]
elif isinstance(i[0], Point):
vel_list += [i[0].vel(N)]
else:
raise TypeError('First entry in pair must be point or frame.')
f_list += [i[1]]
partials = partial_velocity(vel_list, u, N)
# Fill Fr with dot product of partial velocities and forces
for i in range(o):
for j in range(b):
FR[i] -= partials[j][i] & f_list[j]
# In case there are dependent speeds
m = len(self._udep) # number of dependent speeds
if m != 0:
p = o - m
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.
Computes the vector of the generalized active force vector.
Used to compute E.o.M. in the form Fr + Fr* = 0.
Parameters
==========
fl : list
Takes in a list of (Point, Vector) or (ReferenceFrame, Vector)
tuples which represent the force at a point or torque on a frame.
"""
if not hasattr(fl, '__iter__'):
raise TypeError('Force pairs must be supplied in an iterable.')
N = self._inertial
self._forcelist = fl[:]
u = self._u
o = len(u) # number of gen. speeds
b = len(fl) # number of forces
FR = zeros(o, 1)
# pull out relevant velocities for constructing partial velocities
vel_list = []
f_list = []
for i in fl:
if isinstance(i[0], ReferenceFrame):
vel_list += [i[0].ang_vel_in(N)]
elif isinstance(i[0], Point):
vel_list += [i[0].vel(N)]
else:
raise TypeError('First entry in pair must be point or frame.')
f_list += [i[1]]
partials = partial_velocity(vel_list, u, N)
# Fill Fr with dot product of partial velocities and forces
for i in range(o):
for j in range(b):
FR[i] -= partials[j][i] & f_list[j]
# In case there are dependent speeds
m = len(self._udep) # number of dependent speeds
if m != 0:
p = o - m
FRtilde = FR[:p, 0]
FRold = FR[p:o, 0]
FRtilde += self._Ars.T * FRold
FR = FRtilde
self._fr = FR
return FR
def _form_frstar(self, bl):
"""Form the generalized inertia force.
Computes the vector of the generalized inertia force vector.
Used to compute E.o.M. in the form Fr + Fr* = 0.
Parameters
==========
bl : list
A list of all RigidBody's and Particle's in the system.
"""
if not hasattr(bl, '__iter__'):
raise TypeError('Bodies must be supplied in an iterable.')
t = dynamicsymbols._t
N = self._inertial
self._bodylist = bl
u = self._u # all speeds
udep = self._udep # dependent speeds
o = len(u)
m = len(udep)
p = o - m
udot = self._udot
udotzero = dict(zip(udot, [0] * o))
# auxiliary speeds
uaux = self._uaux
uauxdot = [diff(i, t) for i in uaux]
# dictionary of auxiliary speeds which are equal to zero
uaz = dict(zip(uaux, [0] * len(uaux)))
uadz = dict(zip(uauxdot, [0] * len(uauxdot)))
MM = zeros(o, o)
nonMM = zeros(o, 1)
partials = []
# Fill up the list of partials: format is a list with no. 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.
for v in bl:
if isinstance(v, RigidBody):
partials += [
partial_velocity(
[v.masscenter.vel(N),
v.frame.ang_vel_in(N)], u, N)
]
elif isinstance(v, Particle):
partials += [partial_velocity([v.point.vel(N)], u, N)]
else:
raise TypeError('The body list needs RigidBody or '
'Particle as list elements.')
# This section does 2 things - computes the parts of Fr* that are
# associated with the udots, and the parts that are not associated with
# the udots. This happens for RigidBody and Particle a little
# differently, but similar process overall.
for i, v in enumerate(bl):
if isinstance(v, RigidBody):
M = v.mass.subs(uaz).doit()
I, P = v.inertia
if P != v.masscenter:
# redefine I about the center of mass
# have I S/O, want I S/S*
# I S/O = I S/S* + I S*/O; I S/S* = I S/O - I S*/O
f = v.frame
d = v.masscenter.pos_from(P)
I -= inertia_of_point_mass(M, d, f)
I = I.subs(uaz).doit()
for j in range(o):
for k in range(o):
# translational
MM[j, k] += M * (partials[i][0][j].subs(uaz).doit()
& partials[i][0][k])
# rotational
temp = (I & partials[i][1][j].subs(uaz).doit())
MM[j, k] += (temp & partials[i][1][k])
# translational components
nonMM[j] += (
(M.diff(t) * v.masscenter.vel(N)).subs(uaz).doit()
& partials[i][0][j])
nonMM[j] += (
M *
v.masscenter.acc(N).subs(udotzero).subs(uaz).doit()
& partials[i][0][j])
# rotational components
omega = v.frame.ang_vel_in(N).subs(uaz).doit()
nonMM[j] += ((I.dt(v.frame) & omega).subs(uaz).doit()
& partials[i][1][j])
nonMM[j] += ((I & v.frame.ang_acc_in(N)
).subs(udotzero).subs(uaz).doit()
& partials[i][1][j])
nonMM[j] += ((omega ^ (I & omega)).subs(uaz).doit()
& partials[i][1][j])
if isinstance(v, Particle):
M = v.mass.subs(uaz).doit()
for j in range(o):
for k in range(o):
MM[j, k] += M * (partials[i][0][j].subs(uaz).doit()
& partials[i][0][k])
nonMM[j] += M.diff(t) * (v.point.vel(N).subs(uaz).doit()
& partials[i][0][j])
nonMM[j] += (
M * v.point.acc(N).subs(udotzero).subs(uaz).doit()
& partials[i][0][j])
FRSTAR = MM * Matrix(udot).subs(uadz) + nonMM
# For motion constraints, m is the number of constraints
# Really, one should just look at Kane's book for descriptions of this
# process
if m != 0:
FRSTARtilde = FRSTAR[:p, 0]
FRSTARold = FRSTAR[p:o, 0]
FRSTARtilde += self._Ars.T * FRSTARold
FRSTAR = FRSTARtilde
MMi = MM[:p, :]
MMd = MM[p:o, :]
MM = MMi + self._Ars.T * MMd
self._frstar = FRSTAR
zeroeq = self._fr + self._frstar
zeroeq = zeroeq.subs(udotzero)
self._k_d = MM
self._f_d = zeroeq
return FRSTAR
def _form_frstar(self, bl):
"""Form the generalized inertia force.
Computes the vector of the generalized inertia force vector.
Used to compute E.o.M. in the form Fr + Fr* = 0.
Parameters
==========
bl : list
A list of all RigidBody's and Particle's in the system.
"""
if not hasattr(bl, '__iter__'):
raise TypeError('Bodies must be supplied in an iterable.')
t = dynamicsymbols._t
N = self._inertial
self._bodylist = bl
u = self._u # all speeds
udep = self._udep # dependent speeds
o = len(u)
m = len(udep)
p = o - m
udot = self._udot
udotzero = dict(zip(udot, [0] * o))
# auxiliary speeds
uaux = self._uaux
uauxdot = [diff(i, t) for i in uaux]
# dictionary of auxiliary speeds which are equal to zero
uaz = dict(zip(uaux, [0] * len(uaux)))
uadz = dict(zip(uauxdot, [0] * len(uauxdot)))
MM = zeros(o, o)
nonMM = zeros(o, 1)
partials = []
# Fill up the list of partials: format is a list with no. 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.
for v in bl:
if isinstance(v, RigidBody):
partials += [partial_velocity([v.masscenter.vel(N),
v.frame.ang_vel_in(N)], u, N)]
elif isinstance(v, Particle):
partials += [partial_velocity([v.point.vel(N)], u, N)]
else:
raise TypeError('The body list needs RigidBody or '
'Particle as list elements.')
# This section does 2 things - computes the parts of Fr* that are
# associated with the udots, and the parts that are not associated with
# the udots. This happens for RigidBody and Particle a little
# differently, but similar process overall.
for i, v in enumerate(bl):
if isinstance(v, RigidBody):
M = v.mass.subs(uaz).doit()
I, P = v.inertia
if P != v.masscenter:
# redefine I about the center of mass
# have I S/O, want I S/S*
# I S/O = I S/S* + I S*/O; I S/S* = I S/O - I S*/O
f = v.frame
d = v.masscenter.pos_from(P)
I -= inertia_of_point_mass(M, d, f)
I = I.subs(uaz).doit()
for j in range(o):
for k in range(o):
# translational
MM[j, k] += M * (partials[i][0][j].subs(uaz).doit() &
partials[i][0][k])
# rotational
temp = (I & partials[i][1][j].subs(uaz).doit())
MM[j, k] += (temp &
partials[i][1][k])
# translational components
nonMM[j] += ( (M.diff(t) *
v.masscenter.vel(N)).subs(uaz).doit() &
partials[i][0][j])
nonMM[j] += (M *
v.masscenter.acc(N).subs(udotzero).subs(uaz).doit()
& partials[i][0][j])
# rotational components
omega = v.frame.ang_vel_in(N).subs(uaz).doit()
nonMM[j] += ((I.dt(v.frame) & omega).subs(uaz).doit() &
partials[i][1][j])
nonMM[j] += ((I &
v.frame.ang_acc_in(N)).subs(udotzero).subs(uaz).doit()
& partials[i][1][j])
nonMM[j] += ((omega ^ (I & omega)).subs(uaz).doit() &
partials[i][1][j])
if isinstance(v, Particle):
M = v.mass.subs(uaz).doit()
for j in range(o):
for k in range(o):
MM[j, k] += M * (partials[i][0][j].subs(uaz).doit() &
partials[i][0][k])
nonMM[j] += M.diff(t) * (v.point.vel(N).subs(uaz).doit() &
partials[i][0][j])
nonMM[j] += (M *
v.point.acc(N).subs(udotzero).subs(uaz).doit() &
partials[i][0][j])
FRSTAR = MM * Matrix(udot).subs(uadz) + nonMM
# For motion constraints, m is the number of constraints
# Really, one should just look at Kane's book for descriptions of this
# process
if m != 0:
FRSTARtilde = FRSTAR[:p, 0]
FRSTARold = FRSTAR[p:o, 0]
FRSTARtilde += self._Ars.T * FRSTARold
FRSTAR = FRSTARtilde
MMi = MM[:p, :]
MMd = MM[p:o, :]
MM = MMi + self._Ars.T * MMd
self._frstar = FRSTAR
zeroeq = self._fr + self._frstar
zeroeq = zeroeq.subs(udotzero)
self._k_d = MM
self._f_d = zeroeq
return FRSTAR
