def set_inertia(self, I):
if not isinstance(I[0], Dyadic):
raise TypeError("RigidBody inertia must be a Dyadic object.")
if not isinstance(I[1], Point):
raise TypeError("RigidBody inertia must be about a Point.")
self._inertia = I[0]
self._inertia_point = I[1]
# 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
# I_S/S* = I_S/O - I_S*/O
from sympy.physics.mechanics.functions import inertia_of_point_mass
I_Ss_O = inertia_of_point_mass(self.mass, self.masscenter.pos_from(I[1]), self.frame)
self._central_inertia = I[0] - I_Ss_O
def inertia(self, I):
if not isinstance(I[0], Dyadic):
raise TypeError("RigidBody inertia must be a Dyadic object.")
if not isinstance(I[1], Point):
raise TypeError("RigidBody inertia must be about a Point.")
self._inertia = I[0]
self._inertia_point = I[1]
# 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
# I_S/S* = I_S/O - I_S*/O
from sympy.physics.mechanics.functions import inertia_of_point_mass
I_Ss_O = inertia_of_point_mass(self.mass,
self.masscenter.pos_from(I[1]),
self.frame)
self._central_inertia = I[0] - I_Ss_O
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 isinstance(bl, (list, tuple)):
raise TypeError('Bodies must be supplied in a list.')
if self._fr == None:
raise ValueError('Calculate Fr first, please.')
t = dynamicsymbols._t
N = self._inertial
self._bodylist = bl
u = self._u # all speeds
udep = self._udep # dependent speeds
o = len(u)
p = o - len(udep)
udot = self._udot
udotzero = dict(zip(udot, [0] * len(udot)))
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)))
# dictionary of derivatives of auxiliary speeds which are equal to zero
uadz = dict(zip(uauxdot, [0] * len(uauxdot)))
# Form R*, T* for each body or particle in the list
# This is stored as a list of tuples [(r*, t*),...]
# Each tuple is for a body or particle
# Within each rs is a tuple and ts is a tuple
# These have the same structure: ([list], value)
# The list is the coefficients of rs/ts wrt udots, value is everything
# else in the expression
# Partial velocities are stored as a list of tuple; a tuple for each
# body
# Each tuple has two elements, lists which represent the partial
# velocity for each ur; The first list is translational partial
# velocities, the second list is rotational translational velocities
MM = zeros(o, o)
nonMM = zeros(o, 1)
rsts = []
partials = []
for i, v in enumerate(bl): # go through list of bodies, particles
if isinstance(v, RigidBody):
om = v.frame.ang_vel_in(N).subs(uadz).subs(uaz) # ang velocity
omp = v.frame.ang_vel_in(N) # ang velocity, for partials
alp = v.frame.ang_acc_in(N).subs(uadz).subs(uaz) # ang acc
ve = v.mc.vel(N).subs(uadz).subs(uaz) # velocity
vep = v.mc.vel(N) # velocity, for partials
acc = v.mc.acc(N).subs(uadz).subs(uaz) # acceleration
m = (v.mass).subs(uadz).subs(uaz)
I, P = v.inertia
I = I.subs(uadz).subs(uaz)
if P != v.mc:
# redefine I about mass center
# 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.mc.pos_from(P)
I -= inertia_of_point_mass(m, d, f)
templist = []
# One could think of r star as a collection of coefficients of
# the udots plus another term. What we do here is get all of
# these coefficients and store them in a list, then we get the
# "other" term and put the list and other term in a tuple, for
# each body/particle. The same is done for t star. The reason
# for this is to not let the expressions get too large; so we
# keep them seperate for as long a possible
for j, w in enumerate(udot):
templist.append(-m * acc.diff(w, N))
other = -m.diff(t) * ve - m * acc.subs(udotzero)
rs = (templist, other)
templist = []
# see above note
for j, w in enumerate(udot):
templist.append(-I & alp.diff(w, N))
other = -((I.dt(v.frame) & om) + (I & alp.subs(udotzero)) +
(om ^ (I & om)))
ts = (templist, other)
tl1 = []
tl2 = []
# calculates the partials only once and stores them for later
for j, w in enumerate(u):
tl1.append(vep.diff(w, N))
tl2.append(omp.diff(w, N))
partials.append((tl1, tl2))
elif isinstance(v, Particle):
ve = v.point.vel(N).subs(uadz).subs(uaz)
vep = v.point.vel(N)
acc = v.point.acc(N).subs(uadz).subs(uaz)
m = v.mass.subs(uadz).subs(uaz)
templist = []
# see above note
for j, w in enumerate(udot):
templist.append(-m * acc.diff(w, N))
other = -m.diff(t) * ve - m * acc.subs(udotzero)
rs = (templist, other)
# We make an empty t star here so that way the later code
# doesn't care whether its operating on a body or particle
ts = ([0] * len(u), 0)
tl1 = []
tl2 = []
# calculates the partials only once, makes 0's for angular
# partials so the later code is body/particle indepedent
for j, w in enumerate(u):
tl1.append(vep.diff(w, N))
tl2.append(0)
partials.append((tl1, tl2))
else:
raise TypeError('The body list needs RigidBody or '
'Particle as list elements.')
rsts.append((rs, ts))
# Use R*, T* and partial velocities to form FR*
FRSTAR = zeros(o, 1)
# does this for each body in the list
for i, v in enumerate(rsts):
rs, ts = v # unpact r*, t*
vps, ops = partials[i] # unpack vel. partials, ang. vel. partials
# Computes the mass matrix entries from r*, there are from the list
# in the rstar tuple
ii = 0
for x in vps:
for w in rs[0]:
MM[ii] += w & x
ii += 1
# Computes the mass matrix entries from t*, there are from the list
# in the tstar tuple
ii = 0
for x in ops:
for w in ts[0]:
MM[ii] += w & x
ii += 1
# Non mass matrix entries from rstar, from the other in the rstar
# tuple
for j, w in enumerate(vps):
nonMM[j] += w & rs[1]
# Non mass matrix entries from tstar, from the other in the tstar
# tuple
for j, w in enumerate(ops):
nonMM[j] += w & ts[1]
FRSTAR = MM * Matrix(udot) + 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 len(self._udep) != 0:
FRSTARtilde = FRSTAR.extract(range(p), [0])
FRSTARold = FRSTAR.extract(range(p, o), [0])
FRSTARtilde += self._Ars.T * FRSTARold
FRSTAR = FRSTARtilde
MMi = MM.extract(range(p), range(o))
MMd = MM.extract(range(p, o), range(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
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 isinstance(bl, (list, tuple)):
raise TypeError('Bodies must be supplied in a list.')
if self._fr == None:
raise ValueError('Calculate Fr first, please.')
t = dynamicsymbols._t
N = self._inertial
self._bodylist = bl
u = self._u # all speeds
udep = self._udep # dependent speeds
o = len(u)
p = o - len(udep)
udot = self._udot
udotzero = dict(zip(udot, [0] * len(udot)))
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)))
# dictionary of derivatives of auxiliary speeds which are equal to zero
uadz = dict(zip(uauxdot, [0] * len(uauxdot)))
# Form R*, T* for each body or particle in the list
# This is stored as a list of tuples [(r*, t*),...]
# Each tuple is for a body or particle
# Within each rs is a tuple and ts is a tuple
# These have the same structure: ([list], value)
# The list is the coefficients of rs/ts wrt udots, value is everything
# else in the expression
# Partial velocities are stored as a list of tuple; a tuple for each
# body
# Each tuple has two elements, lists which represent the partial
# velocity for each ur; The first list is translational partial
# velocities, the second list is rotational translational velocities
MM = zeros(o, o)
nonMM = zeros(o, 1)
rsts = []
partials = []
for i, v in enumerate(bl): # go through list of bodies, particles
if isinstance(v, RigidBody):
om = v.frame.ang_vel_in(N).subs(uadz).subs(uaz) # ang velocity
omp = v.frame.ang_vel_in(N) # ang velocity, for partials
alp = v.frame.ang_acc_in(N).subs(uadz).subs(uaz) # ang acc
ve = v.masscenter.vel(N).subs(uadz).subs(uaz) # velocity
vep = v.masscenter.vel(N) # velocity, for partials
acc = v.masscenter.acc(N).subs(uadz).subs(uaz) # acceleration
m = (v.mass).subs(uadz).subs(uaz)
I, P = v.inertia
I = I.subs(uadz).subs(uaz)
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)
templist = []
# One could think of r star as a collection of coefficients of
# the udots plus another term. What we do here is get all of
# these coefficients and store them in a list, then we get the
# "other" term and put the list and other term in a tuple, for
# each body/particle. The same is done for t star. The reason
# for this is to not let the expressions get too large; so we
# keep them seperate for as long a possible
for j, w in enumerate(udot):
templist.append(-m * acc.diff(w, N))
other = -m.diff(t) * ve - m * acc.subs(udotzero)
rs = (templist, other)
templist = []
# see above note
for j, w in enumerate(udot):
templist.append(-I & alp.diff(w, N))
other = -((I.dt(v.frame) & om) + (I & alp.subs(udotzero))
+ (om ^ (I & om)))
ts = (templist, other)
tl1 = []
tl2 = []
# calculates the partials only once and stores them for later
for j, w in enumerate(u):
tl1.append(vep.diff(w, N))
tl2.append(omp.diff(w, N))
partials.append((tl1, tl2))
elif isinstance(v, Particle):
ve = v.point.vel(N).subs(uadz).subs(uaz)
vep = v.point.vel(N)
acc = v.point.acc(N).subs(uadz).subs(uaz)
m = v.mass.subs(uadz).subs(uaz)
templist = []
# see above note
for j, w in enumerate(udot):
templist.append(-m * acc.diff(w, N))
other = -m.diff(t) * ve - m * acc.subs(udotzero)
rs = (templist, other)
# We make an empty t star here so that way the later code
# doesn't care whether its operating on a body or particle
ts = ([0] * len(u), 0)
tl1 = []
tl2 = []
# calculates the partials only once, makes 0's for angular
# partials so the later code is body/particle indepedent
for j, w in enumerate(u):
tl1.append(vep.diff(w, N))
tl2.append(0)
partials.append((tl1, tl2))
else:
raise TypeError('The body list needs RigidBody or '
'Particle as list elements.')
rsts.append((rs, ts))
# Use R*, T* and partial velocities to form FR*
FRSTAR = zeros(o, 1)
# does this for each body in the list
for i, v in enumerate(rsts):
rs, ts = v # unpact r*, t*
vps, ops = partials[i] # unpack vel. partials, ang. vel. partials
# Computes the mass matrix entries from r*, there are from the list
# in the rstar tuple
ii = 0
for x in vps:
for w in rs[0]:
MM[ii] += w & x
ii += 1
# Computes the mass matrix entries from t*, there are from the list
# in the tstar tuple
ii = 0
for x in ops:
for w in ts[0]:
MM[ii] += w & x
ii += 1
# Non mass matrix entries from rstar, from the other in the rstar
# tuple
for j, w in enumerate(vps):
nonMM[j] += w & rs[1]
# Non mass matrix entries from tstar, from the other in the tstar
# tuple
for j, w in enumerate(ops):
nonMM[j] += w & ts[1]
FRSTAR = MM * Matrix(udot) + 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 len(self._udep) != 0:
FRSTARtilde = FRSTAR.extract(range(p), [0])
FRSTARold = FRSTAR.extract(range(p, o), [0])
FRSTARtilde += self._Ars.T * FRSTARold
FRSTAR = FRSTARtilde
MMi = MM.extract(range(p), range(o))
MMd = MM.extract(range(p, o), range(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
