def mkobject(self, name):
s = str(name)
return {
"$.name": Globals.getObjName(s),
"tr": self.symbols.getFunction(s, "tr", [Globals.time(self.symbols)]),
"tr.x": self.symbols.getFunction(s, "tr.x", [Globals.time(self.symbols)]),
"tr.y": self.symbols.getFunction(s, "tr.y", [Globals.time(self.symbols)]),
"tr.mass": self.symbols.getSymbol(s, "tr.mass", nonnegative=True),
"tr.frame": {"theta": self.symbols.getSymbol(s, "tr.theta")},
"rt.mass": self.symbols.getSymbol(s, "rt.mass", nonnegative=True),
"rt.angle": self.symbols.getFunction(s, "rt.angle", [Globals.time(self.symbols)]),
"rt.frame": {"dir": 1},
}
def getTSym(self):
return -self.b * self.getDSym().diff(Globals.time(self.symbols))
def __init__(self, name, atta, rolla, attb, rollb, symbols):
RodDynamic.__init__(self, name, atta, rolla, attb, rollb, symbols)
self.d = self.symbols.getFunction(self.name, 'd', [Globals.time(self.symbols)])
def __init__(self, name, atta, rolla, attb, rollb, symbols):
PairDynamic.__init__(self, name, atta, rolla, attb, rollb, symbols)
self.l = self.symbols.getSymbol(self.name, 'l', nonnegative=True)
self.thetaa = self.symbols.getFunction(self.name, 'thetaa', [Globals.time(self.symbols)])
self.thetab = self.thetaa + sympy.pi
def solveEquations(self):
# System of equations
self.printer.print_diagnostic(3, "Finishing system of equations...")
for ei, expr in enumerate(self.scene["fragments"]):
obj = expr["object"]
self.printer.print_diagnostic(3, "(%d/%d) %s..." % (ei + 1, len(self.scene["fragments"]), obj["$.name"]))
rftmode, rfrmode, rfangle, rfdir = self.scene["refs"][obj["$.name"]]
rhsx = 0
rhsy = 0
rhst = 0
mxa = obj["tr.mass"] * sympy.Derivative(obj["tr.x"], Globals.time(self.symbols), Globals.time(self.symbols))
mya = obj["tr.mass"] * sympy.Derivative(obj["tr.y"], Globals.time(self.symbols), Globals.time(self.symbols))
mta = obj["rt.mass"] * sympy.Derivative(
obj["rt.angle"], Globals.time(self.symbols), Globals.time(self.symbols)
)
cx = isTimeConstant(obj["tr.x"], self.symbols)
cy = isTimeConstant(obj["tr.y"], self.symbols)
ca = isTimeConstant(obj["rt.angle"], self.symbols)
for i in expr["rhs"]:
force = i[0]
dyn = i[-1]
roll = i[2] == "roll"
angle = i[1]
# Compute the force components
if angle != None:
if rftmode == 1:
x = sympy.simplify(force * sympy.sin(i[1]))
rhsx += dyn.simplify1DD(x)
# Send expression to timemachine
if obj["tr.mass"] != 0 and not cx and not cy:
self.timemachine.add_highlight_eqn(obj, dyn, [sympy.Eq(mxa, rhsx)])
elif rftmode == 2:
if not cx:
x = sympy.simplify(force * sympy.sin(i[1]))
rhsx += x
# Send expression to timemachine
if obj["tr.mass"] != 0 and not cx:
self.timemachine.add_highlight_eqn(obj, dyn, [sympy.Eq(mxa, rhsx)])
if not cy:
y = sympy.simplify(force * sympy.cos(i[1]))
rhsy += y
# Send expression to timemachine
if obj["tr.mass"] != 0 and not cy:
self.timemachine.add_highlight_eqn(obj, dyn, [sympy.Eq(mya, rhsy)])
if rfrmode != 0 and not ca:
# Compute the torque components
atd, ata, atm = dyn.getAttachment(obj, "p")
if atd != 0:
# Roll dynamics will assume a force perpendicular to the radius vector
if not roll:
# Compute the torque angle
tangle = ata + sympy.pi / 2 + obj["rt.angle"]
else:
# Compute the torque angle without considering the body rotation
tangle = ata + sympy.pi / 2
# Compute the projection of the force onto the torque direction
torque = sympy.simplify(force * atd * sympy.cos(angle - tangle))
rhst += torque
# Send expression to timemachine
if obj["rt.mass"] != 0 and not ca:
self.timemachine.add_highlight_eqn(obj, dyn, [sympy.Eq(mta, rhst)])
elif not ca:
# The force is actually a torque...
rhst += force
# Well this is weird...
if obj["rt.mass"] == 0:
self.printer.print_diagnostic(
2,
"pure torque from %s applied to %s which has no moment of inertia."
% (dyn.name, obj["$.name"]),
)
# Send expression to timemachine
if obj["rt.mass"] != 0 and not ca:
self.timemachine.add_highlight_eqn(obj, dyn, [sympy.Eq(mta, rhst)])
# Prepare to show the final set of equations
tmexprs = []
# Append the translation equations
if rftmode != 0:
if obj["tr.mass"] == 0:
self.printer.print_diagnostic(
3, "ignoring translational equations for body %s bacause it has no mass." % obj["$.name"]
)
else:
if rftmode == 1:
expr = sympy.Eq(mxa, rhsx)
self.scene["equations"].append(expr)
tmexprs.append(expr)
if rftmode == 2:
if cx:
self.printer.print_diagnostic(
3,
"ignoring x-axis translational equation for body %s bacause it is locked."
% obj["$.name"],
)
else:
expr = sympy.Eq(mxa, rhsx)
self.scene["equations"].append(expr)
tmexprs.append(expr)
if cy:
self.printer.print_diagnostic(
3,
"ignoring y-axis translational equation for body %s bacause it is locked."
% obj["$.name"],
)
else:
expr = sympy.Eq(mya, rhsy)
self.scene["equations"].append(expr)
tmexprs.append(expr)
else:
raise Exception("unknown reference frame mode")
# Append the rotation equations
if rfrmode != 0:
if obj["rt.mass"] == 0:
self.printer.print_diagnostic(
3,
"ignoring rotational equations for body %s bacause it has no moment of inertia."
% obj["$.name"],
)
else:
if rfrmode == 1:
if ca:
self.printer.print_diagnostic(
3, "ignoring rotational equation for body %s bacause it is locked." % obj["$.name"]
)
else:
expr = sympy.Eq(mta, rhst)
self.scene["equations"].append(expr)
tmexprs.append(expr)
else:
raise Exception("unknown reference frame mode")
# Send expressions to timemachine
self.timemachine.add_highlight_eqn(obj, None, tmexprs)
# Finish with the link equations
for di, dyn in enumerate(self.scene["dynamics"]):
self.printer.print_diagnostic(3, "(%d/%d) %s..." % (di + 1, len(self.scene["dynamics"]), dyn.name))
# Prepare to show the final set of link equations
tmexprs = []
les = dyn.getLEqns()
for le in les:
le = sympy.simplify(le)
if le == True:
self.printer.print_diagnostic(
3, "link equation was reduced to 0 == 0 due to constraints and will be ignored."
)
elif le == False:
raise Exception("inconsistent system detected while processing dynamic %s" % dyn.name)
else:
self.scene["equations"].append(le)
tmexprs.append(le)
# Send the system of link equations to timemachine
self.timemachine.add_highlight_eqn(None, dyn, tmexprs)
# Send the whole system to timemachine
self.timemachine.add_highlight_eqn(None, None, self.scene["equations"])
self.printer.print_diagnostic(3, "system ready.")
