def getLEqns(self):
# Algebraic Links
a1 = self.obja['rt.angle']
a2 = self.objb['rt.angle']
# Convert the attachments to polar, add the body angle
# and then reconvert to rectangular
att1 = convertAttachment(self.atta, 'p')
if att1[0] != 0:
att1 = (att1[0], a1 + att1[1], att1[2])
i1, j1, m1 = convertAttachment(att1, 'r')
else:
raise Exception('coupling radius of %s to %s is zero' % (self.name, self.obja['$.name']))
att2 = convertAttachment(self.attb, 'p')
if att2[0] != 0:
att2 = (att2[0], a2 + att2[1], att2[2])
i2, j2, m2 = convertAttachment(att2, 'r')
else:
raise Exception('coupling radius of %s to %s is zero' % (self.name, self.objb['$.name']))
return [
sympy.Eq(a2, (i1 / i2) * a1) if not self.crossed else
sympy.Eq(a2, -(i1 / i2) * a1)
]
def getAttachment(self, obj, mode):
# (distance from center of mass, angle)
if obj == self.obja:
return convertAttachment(self.atta, mode)
if obj == self.objb:
return convertAttachment(self.attb, mode)
raise Exception('invalid object')
def getDExpr(self, obj):
# Select the proper attachment
if obj == self.obja:
att = self.atta
# Inverted because of the -1 inherent from the torque
# direction with respect to the reference
mul = 1 if self.crossed else -1
elif obj == self.objb:
att = self.attb
mul = 1
else:
raise Exception('invalid object')
att1 = convertAttachment(att, 'p')
if att1[0] != 0:
att1 = (att1[0], a1 + att1[1], att1[2])
i1, j1, m1 = convertAttachment(att1, 'r')
else:
raise Exception('coupling radius of %s to %s is zero' % (self.name, obj['$.name']))
# Return a pure torque
return (mul * i1 * self.getTSym(), None, '')
def getLEqns(self):
# Algebraic Links
d = self.getDSym()
l = self.l
td = self.thetaa
x1 = self.obja['tr.x']
y1 = self.obja['tr.y']
x2 = self.objb['tr.x']
y2 = self.objb['tr.y']
a1 = self.obja['rt.angle']
a2 = self.objb['rt.angle']
# Convert the attachments to polar, add the body angle
# and then reconvert to rectangular
att1 = convertAttachment(self.atta, 'p')
if att1[0] != 0:
if not self.rolla:
att1 = (att1[0], a1 + att1[1], att1[2])
i1, j1, m1 = convertAttachment(att1, 'r')
else:
i1, j1, m1 = convertAttachment(att1, 'r')
attr = (att1[0] * a1, att1[1] + sympy.pi / 2, att1[2])
ir, jr, mr = convertAttachment(attr, 'r')
i1 += ir
j1 += jr
else:
i1 = 0
j1 = 0
att2 = convertAttachment(self.attb, 'p')
if att2[0] != 0:
if not self.rollb:
att2 = (att2[0], a2 + att2[1], att2[2])
i2, j2, m2 = convertAttachment(att2, 'r')
else:
i2, j2, m2 = convertAttachment(att2, 'r')
attr = (att2[0] * a2, att2[1] + sympy.pi / 2, att2[2])
ir, jr, mr = convertAttachment(attr, 'r')
i2 += ir
j2 += jr
else:
i2 = 0
j2 = 0
return [
sympy.Eq(x2, x1 + i1 + (l+d)*sympy.sin(td) - i2),
sympy.Eq(y2, y1 + j1 + (l+d)*sympy.cos(td) - j2),
]
def getAttachment(self, obj, mode):
if obj != self.obj:
raise Exception('invalid object')
# (distance from center of mass, angle)
return convertAttachment(self.att, mode)