Esempio n. 1
0
    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)
        ]
Esempio n. 2
0
 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')
Esempio n. 3
0
    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, '')
Esempio n. 4
0
    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),
        ]
Esempio n. 5
0
 def getAttachment(self, obj, mode):
     if obj != self.obj:
         raise Exception('invalid object')
     # (distance from center of mass, angle)
     return convertAttachment(self.att, mode)