def simplify1DD(self, expr):
# Place the offsets of the objects relative to their reference
# frame position, so constant position objects have a relative
# position of zero
xb = 0 if isTimeConstant(self.objb['tr.x'], self.symbols) else self.objb['tr.x']
xa = 0 if isTimeConstant(self.obja['tr.x'], self.symbols) else self.obja['tr.x']
# b.x = a.x + (l + d)*sin(theta_a)
# d = b.x - a.x
return expr.subs(self.d, xb - xa)
def assertPairAngle(self, dyn):
x1 = dyn.obja["tr.x"]
y1 = dyn.obja["tr.y"]
x2 = dyn.objb["tr.x"]
y2 = dyn.objb["tr.y"]
a1 = dyn.obja["rt.angle"]
a2 = dyn.objb["rt.angle"]
thetaa = dyn.thetaa
name = dyn.name
if not isTimeConstant(thetaa, self.symbols):
return True
# Convert the attachments to polar, add the body angle
# and then reconvert to rectangular
att1 = dyn.getAttachment(dyn.obja, "p")
if att1[0] != 0:
att1 = (att1[0], a1 + att1[1], att1[2])
i1, j1, m1 = Globals.convertAttachment(att1, "r")
else:
i1 = 0
j1 = 0
att2 = dyn.getAttachment(dyn.objb, "p")
if att2[0] != 0:
att2 = (att2[0], a2 + att2[1], att2[2])
i2, j2, m2 = Globals.convertAttachment(att2, "r")
else:
i2 = 0
j2 = 0
dx = sympy.sympify((x2 + i2) - (x1 + i1))
dy = sympy.sympify((y2 + j2) - (y1 + j1))
if dx == 0 and sympy.simplify(sympy.Mod(thetaa, sympy.pi)) != 0:
self.printer.print_diagnostic(
2,
"thetaa assertion failed for dynamic %s, set=%s, inferred=%s."
% (name, str(thetaa), str(sympy.sympify(sympy.atan2(dx, dy)))),
)
return False
elif dy == 0 and sympy.simplify(sympy.Mod(thetaa + sympy.pi / 2, sympy.pi)) != 0:
self.printer.print_diagnostic(
2,
"thetaa assertion failed for dynamic %s, set=%s, inferred=%s."
% (name, str(thetaa), str(sympy.sympify(sympy.atan2(dx, dy)))),
)
return False
return True
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.")
def solveRefFrames(self):
# Reference frames
self.printer.print_diagnostic(3, "deducing reference frames...")
for expr in self.scene["fragments"]:
obj = expr["object"]
if len(expr["rhs"]) == 0:
self.printer.print_diagnostic(2, "%s has no dynamics associated." % obj["$.name"])
rmode = 0
tmode = 0
angle = 0
dir = 0
else:
cx = isTimeConstant(obj["tr.x"], self.symbols)
cy = isTimeConstant(obj["tr.y"], self.symbols)
ca = isTimeConstant(obj["rt.angle"], self.symbols)
if cx and cy and ca:
# All axis of movement are locked (all motion variables are constants)
self.printer.print_diagnostic(2, "%s is fully locked." % obj["$.name"])
rmode = 0
tmode = 0
angle = 0
dir = 1
else:
rmode = 1
tmode = 2
angle = 0
dir = 1
# See if rotation is locked
# if ca:
# if obj['rt.mass'] != 0:
# self.printer.print_diagnostic(2, '%s has no rotation but has moment of inertia.' % obj['$.name'])
# rmode = 0
# dir = 0
# else:
# rmode = 1
# dir = 1
## Deduce translations
# if cx and cy:
# if obj['tr.mass'] != 0:
# self.printer.print_diagnostic(2, '%s has no translation but has mass.' % obj['$.name'])
# tmode = 0
# angle = 0
# elif cx:
# # Translation along x axis
# tmode = 1
# angle = sympy.pi / 2
# elif cy:
# # Translation along y axis
# tmode = 1
# angle = 0
# else:
# # xy free, deduce from dynamics
# # Check if a 2D ref frame is necessary
# # for that all forces must be aligned on the same
# # axis and the angle cannot vary with time
# angle = expr['rhs'][0][1]
# if not isTimeConstant(angle, self.symbols):
# tmode = 2
# angle = 0
# else:
# tmode = 1
# for rhs in expr['rhs'][1:]:
# # simplification of cross product for unit vectors
# # to evaluate if two angles form parallel vectors
# a = rhs[1]
# if isTimeConstant(a, self.symbols) == False or sympy.sin(sympy.simplify(angle-a)) != 0:
# angle = 0
# tmode = 2
# break
self.printer.print_diagnostic(
4,
"%s - %d translation DOF (rotated by %s), %d rotation DOF (%s)."
% (obj["$.name"], tmode, str(angle), rmode, "CCW" if dir == 1 else "CW"),
)
self.scene["refs"][obj["$.name"]] = (tmode, rmode, angle, dir)
# Send the ref frame to timemachine
self.timemachine.add_rf(
obj, tmode, rmode, angle, dir, (str(obj["tr.x"]), str(obj["tr.y"]), str(obj["rt.angle"]))
)
def loadDynamic(self, name, props, data, aliases):
dyn = data["dynamic"]
bodies = [self.getObject(n[0]) for n in data["attach"]]
attmodes = [n[1] if len(n) > 1 else "p" for n in data["attach"]]
attoffs = [n[2] if len(n) > 1 else None for n in data["attach"]]
offs = data["offset"]
showangles = props.get("showangles", False)
# A few assert funtions...
def assert_bodies(name, n):
if len(bodies) != n:
raise Exception("%s dynamic expects %d body, provided %d" % (name, n, len(bodies)))
def assert_offs(name, n):
if len(offs) != n:
raise Exception("%s dynamic expects %d offsets, provided %d" % (name, n, len(offs)))
def aliasify(s, p):
alias = aliases.get(s, None)
if alias != None:
self.aliases[p] = alias
return str(p)
# Roll assertions
def get0Rolls(what):
roll = props.get("roll", None)
if roll == None:
roll = props.get("rolla", None)
if roll == "1" or roll == "1":
self.printer.print_diagnostic(2, "dynamic %s does not have roll because it is %s." % (name, what))
def get1Roll():
roll = props.get("roll", None)
if roll == None:
roll = props.get("rolla", None)
if roll == "1" or roll == "1":
self.printer.print_diagnostic(
3, "dynamic %s set to roll mode on body %s." % (name, bodies[0]["$.name"])
)
roll = True
else:
roll = False
return roll
def get2Rolls():
rolla = props.get("rolla", None)
if rolla == "1" or rolla == "1":
self.printer.print_diagnostic(
3, "dynamic %s set to roll mode on body %s." % (name, bodies[0]["$.name"])
)
rolla = True
else:
rolla = False
rollb = props.get("rollb", None)
if rollb == "1" or rollb == "1":
self.printer.print_diagnostic(
3, "dynamic %s set to roll mode on body %s." % (name, bodies[1]["$.name"])
)
rollb = True
else:
rollb = False
return (rolla, rollb)
# Resolve the attachments
for b, m, o in zip(bodies, attmodes, attoffs):
if o != None:
if m == "p":
ata = Globals.getAttachProp(b["$.name"], "d")
atb = Globals.getAttachProp(b["$.name"], "theta")
elif m == "r":
ata = Globals.getAttachProp(b["$.name"], "x")
atb = Globals.getAttachProp(b["$.name"], "y")
self.symbols.addReplacement(name, ata, o[0])
self.symbols.addReplacement(name, atb, o[1])
# Switch the dynamic type
if dyn == "force":
assert_bodies("force", 1)
assert_offs("force", 4)
pos = (offs["x1"], offs["y1"], offs["x2"], offs["y2"])
att = (bodies[0], attmodes[0])
# Constant Force has only one roll
roll = get1Roll()
d = Dynamics.ForceDynamic(name, att, roll, self.symbols)
t = aliasify("theta", d.theta)
title = aliasify("F", d.getFSym())
elif dyn == "weight":
# TODO ignore offset and force it to center of mass
assert_bodies("weight", 1)
assert_offs("weight", 4)
pos = (offs["x1"], offs["y1"], offs["x2"], offs["y2"])
# Weight Force only has an att if the center of mass is different
# from the fixture point, which means the translation axes MUST
# be fixed, enforce this rule
# By default the attachment must be zero even if nothing is
# specified (center of mass assumed)
mustlock = True
# Enforce att lock if the axes are not
if not isTimeConstant(bodies[0]["tr.x"], self.symbols) or not isTimeConstant(
bodies[0]["tr.y"], self.symbols
):
if attoffs[0] != None:
self.printer.print_diagnostic(
2,
"offset for %s will be ignored because the body %s is not locked in x and y."
% (name, bodies[0]["$.name"]),
)
else:
if attoffs[0] != None:
mustlock = False
if mustlock:
attmodes[0] = "p"
ata = Globals.getAttachProp(b["$.name"], "d")
atb = Globals.getAttachProp(b["$.name"], "theta")
self.symbols.addReplacement(name, ata, 0)
self.symbols.addReplacement(name, atb, 0)
att = (bodies[0], attmodes[0])
# Weight Force only has no rolls :/
get0Rolls("weight")
d = Dynamics.WeightDynamic(name, att, self.symbols)
title = str(d.getFSym())
showangles = False # Do not show theta for gravity
elif dyn == "rod":
assert_bodies("rod", 2)
assert_offs("rod", 4)
pos = (offs["x1"], offs["y1"], offs["x2"], offs["y2"])
att0 = (bodies[0], attmodes[0])
att1 = (bodies[1], attmodes[1])
# Rod has two rolls, for a and b points
rolla, rollb = get2Rolls()
d = Dynamics.RodDynamic(name, att0, rolla, att1, rollb, self.symbols)
aliasify("l", d.l)
t = aliasify("thetaa", d.thetaa)
title = aliasify("T", d.getTSym())
elif dyn == "spring":
assert_bodies("spring", 2)
assert_offs("spring", 4)
pos = (offs["x1"], offs["y1"], offs["x2"], offs["y2"])
att0 = (bodies[0], attmodes[0])
att1 = (bodies[1], attmodes[1])
# Spring has two rolls, for a and b points
rolla, rollb = get2Rolls()
d = Dynamics.SpringDynamic(name, att0, rolla, att1, rollb, self.symbols)
aliasify("l", d.l)
aliasify("d", d.d)
aliasify("T", d.getTSym())
t = aliasify("thetaa", d.thetaa)
title = aliasify("k", d.k)
elif dyn == "dampener":
assert_bodies("dampener", 2)
assert_offs("dampener", 4)
pos = (offs["x1"], offs["y1"], offs["x2"], offs["y2"])
att0 = (bodies[0], attmodes[0])
att1 = (bodies[1], attmodes[1])
# Dampener has two rolls, for a and b points
rolla, rollb = get2Rolls()
d = Dynamics.DampenerDynamic(name, att0, rolla, att1, rollb, self.symbols)
aliasify("l", d.l)
aliasify("d", d.d)
aliasify("T", d.getTSym())
t = aliasify("thetaa", d.thetaa)
title = aliasify("b", d.b)
elif dyn == "torque":
assert_bodies("torque", 1)
assert_offs("torque", 2)
flipped = int(props.get("direction", 1))
flipped = flipped != 0
pos = (offs["x1"], offs["y1"])
# Angular dynamics have no rolls
get0Rolls()
d = Dynamics.TorqueDynamic(name, obj, flipped, self.symbols)
title = aliasify("k", d.k)
elif dyn == "angularspring":
assert_bodies("angularspring", 1)
assert_offs("angularspring", 2)
pos = (offs["x1"], offs["y1"])
# Angular dynamics have no rolls
get0Rolls()
d = Dynamics.AngularSpringDynamic(name, obj, self.symbols)
title = aliasify("k", d.k)
elif dyn == "angulardampener":
assert_bodies("angulardampener", 1)
assert_offs("angulardampener", 2)
pos = (offs["x1"], offs["y1"])
# Angular dynamics have no rolls
get0Rolls()
d = Dynamics.AngularDampenerDynamic(name, obj, self.symbols)
title = aliasify("b", d.b)
elif dyn == "belt":
assert_bodies("belt", 2)
assert_offs("belt", 4)
pos = (offs["x1"], offs["y1"], offs["x2"], offs["y2"])
att0 = (bodies[0], attmodes[0])
att1 = (bodies[1], attmodes[1])
# Angular dynamics have no rolls
get0Rolls()
d = Dynamics.BeltDynamic(name, att0, att1, self.symbols)
title = aliasify("b", d.b)
else:
raise Exception("unknown dynamic type %s" % dyn)
# Add dynamic to the list of dynamic in the scene
self.scene["dynamics"].append(d)
# Add dynamic to the collection of dynamics of each body
for body in bodies:
self.scene["attachments"][body["$.name"]].append(d)
# Draw the dynamic
self.printer.print_dynamic(name, dyn, pos, title, props)
# Draw the angle of the dynamic, if specified
if showangles == "1" or showangles == True:
self.printer.print_angle(name, t)
