예제 #1
0
def rx90():
    """Generate Robot instance of RX90"""
    robo = Robot('RX90', 6, 6, 6, False)
    # table of geometric parameters RX90
    robo.sigma = [2, 0, 0, 0, 0, 0, 0, 0]
    robo.alpha = [0, 0, pi/2, 0, -pi/2, pi/2, -pi/2]
    robo.d = [0, 0, 0, var('D3'), 0, 0, 0]
    robo.theta = [0] + list(var('th1:7'))
    robo.r = [0, 0, 0, 0, var('RL4'), 0, 0]
    robo.b = [0, 0, 0, 0, 0, 0, 0]
    robo.gamma = [0, 0, 0, 0, 0, 0, 0]
    robo.mu = [0, 1, 1, 1, 1, 1, 1]
    robo.structure = tools.SIMPLE
#        robo.w0 = zeros(3, 1)
#        robo.wdot0 = zeros(3, 1)
#        robo.v0 = zeros(3, 1)
#        robo.vdot0 = zeros(3, 1)
#        robo.qdot = [var('QP{0}'.format(i)) for i in num]
#        robo.qddot = [var('QDP{0}'.format(i)) for i in num]
#        robo.Nex= [zeros(3, 1) for i in num]
#        robo.Nex[-1] = Matrix(var('CX{0}, CY{0}, CZ{0}'.format(robo.NJ)))
#        robo.Fex = [zeros(3, 1) for i in num]
#        robo.Fex[-1] = Matrix(var('FX{0}, FY{0}, FZ{0}'.format(robo.NJ)))
#        robo.FS = [var('FS{0}'.format(i)) for i in num]
#        robo.IA = [var('IA{0}'.format(i)) for i in num]
#        robo.FV = [var('FV{0}'.format(i)) for i in num]
#        robo.MS = [Matrix(var('MX{0}, MY{0}, MZ{0}'.format(i))) for i in num]
#        robo.M = [var('M{0}'.format(i)) for i in num]
#        robo.GAM = [var('GAM{0}'.format(i)) for i in num]
#        robo.J = [Matrix(3, 3, var(('XX{0}, XY{0}, XZ{0}, '
#                            'XY{0}, YY{0}, YZ{0}, '
#                            'XZ{0}, YZ{0}, ZZ{0}').format(i))) for i in num]
#        robo.G = Matrix([0, 0, var('G3')])
#        robo.num.append(0)
    return robo
예제 #2
0
def planar2r():
    """Generate Robot instance of 2R Planar robot"""
    robo = Robot('Planar2R', 2, 2, 3, False)
    robo.structure = tools.SIMPLE
    robo.sigma = [2, 0, 0, 2]
    robo.mu = [0, 1, 1, 0]
    robo.gamma = [0, 0, 0, 0]
    robo.b = [0, 0, 0, 0]
    robo.alpha = [0, 0, 0, 0]
    robo.d = [0, 0, var('L1'), var('L2')]
    robo.theta = [0, var('th1'), var('th2'), 0]
    robo.r = [0, 0, 0, 0]
    return robo
예제 #3
0
def sr400():
    #TODO: bring it to the new notation with 0-frame
    """Generate Robot instance of SR400"""
    robo = Robot('SR400', 8, 9, 10, False)
    robo.ant = [-1, 0, 1, 2, 3, 4, 5, 1, 7, 8, 3]
    robo.sigma = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2]
    robo.mu = [0, 1, 1, 0, 1, 1, 1, 1, 0, 0, 0]
    robo.alpha = [0, 0, -pi/2, 0, -pi/2, pi/2, -pi/2, -pi/2, 0, 0, 0]
    d_var = var('D:9')
    robo.d = [0, 0, d_var[2], d_var[3], d_var[4], 0, 0,
              d_var[2], d_var[8], d_var[3], -d_var[8]]
    robo.theta = [0] + list(var('th1:10')) + [0]
    robo.r = [0, 0, 0, 0, var('RL4'), 0, 0, 0, 0, 0, 0]
    robo.b = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
    robo.gamma = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, pi/2]
    robo.structure = tools.CLOSED_LOOP
    return robo
예제 #4
0
def cart_pole():
    """Generate Robot instance of classical CartPole dynamic system."""
    #TODO: bring it to the new notation with 0-frame
    robo = Robot()
    robo.name = 'CartPole'
    robo.ant = (-1, 0)
    robo.sigma = (1, 0)
    robo.alpha = (pi/2, pi/2)
    robo.d = (0, 0)
    robo.theta = (pi/2, var('Th2'))
    robo.r = (var('R1'), 0)
    robo.b = (0, 0)
    robo.gamma = (0, 0)
    robo.num = range(1, 3)
    robo.NJ = 2
    robo.NL = 2
    robo.NF = 2
    robo.Nex = [zeros(3, 1) for i in robo.num]
    robo.Fex = [zeros(3, 1) for i in robo.num]
    robo.FS = [0 for i in robo.num]
    robo.IA = [0 for i in robo.num]
    robo.FV = [var('FV{0}'.format(i)) for i in robo.num]
    robo.MS = [zeros(3, 1) for i in robo.num]
    robo.MS[1][0] = var('MX2')
    robo.M = [var('M{0}'.format(i)) for i in robo.num]
    robo.GAM = [var('GAM{0}'.format(i)) for i in robo.num]
    robo.J = [zeros(3) for i in robo.num]
    robo.J[1][2, 2] = var('ZZ2')
    robo.G = Matrix([0, 0, -var('G3')])
    robo.w0 = zeros(3, 1)
    robo.wdot0 = zeros(3, 1)
    robo.v0 = zeros(3, 1)
    robo.vdot0 = zeros(3, 1)
    robo.q = var('R1, Th2')
    robo.qdot = var('R1d, Th2d')
    robo.qddot = var('R1dd, Th2dd')
    robo.num.append(0)
    return robo
예제 #5
0
 def OnNew(self, event):
     dialog = ui_definition.DialogDefinition(
         ui_labels.MAIN_WIN['prog_name'], self.robo.name, self.robo.nl,
         self.robo.nj, self.robo.structure, self.robo.is_mobile)
     if dialog.ShowModal() == wx.ID_OK:
         result = dialog.get_values()
         new_robo = Robot(*result['init_pars'])
         if result['keep_geo']:
             nf = min(self.robo.NF, new_robo.NF)
             new_robo.ant[:nf] = self.robo.ant[:nf]
             new_robo.sigma[:nf] = self.robo.sigma[:nf]
             new_robo.mu[:nf] = self.robo.mu[:nf]
             new_robo.gamma[:nf] = self.robo.gamma[:nf]
             new_robo.alpha[:nf] = self.robo.alpha[:nf]
             new_robo.theta[:nf] = self.robo.theta[:nf]
             new_robo.b[:nf] = self.robo.b[:nf]
             new_robo.d[:nf] = self.robo.d[:nf]
             new_robo.r[:nf] = self.robo.r[:nf]
         if result['keep_dyn']:
             nl = min(self.robo.NL, new_robo.NL)
             new_robo.Nex[:nl] = self.robo.Nex[:nl]
             new_robo.Fex[:nl] = self.robo.Fex[:nl]
             new_robo.FS[:nl] = self.robo.FS[:nl]
             new_robo.IA[:nl] = self.robo.IA[:nl]
             new_robo.FV[:nl] = self.robo.FV[:nl]
             new_robo.MS[:nl] = self.robo.MS[:nl]
             new_robo.M[:nl] = self.robo.M[:nl]
             new_robo.J[:nl] = self.robo.J[:nl]
         if result['keep_base']:
             new_robo.Z = self.robo.Z
             new_robo.w0 = self.robo.w0
             new_robo.wdot0 = self.robo.wdot0
             new_robo.v0 = self.robo.v0
             new_robo.vdot0 = self.robo.vdot0
             new_robo.G = self.robo.G
         self.robo = new_robo
         self.robo.directory = filemgr.get_folder_path(self.robo.name)
         self.feed_data()
     dialog.Destroy()
    def Export(self, name):
        NJ = len([i for i in self.elements if (isinstance(i, SuperRevoluteJoint) or isinstance(i, SuperPrismaticJoint)) and not i.virtual_joint] )
        NL = len([i for i in self.elements if isinstance(i, Point) and not i.virtual_joint] )-1
        NF = NL+2*(NJ-NL)
        
        if not self.structure:
            return
        if not self.structure[0]-self.structure[1]:
            structure = SIMPLE
        elif not NJ-NL:
            structure = TREE
        else:
            structure = CLOSED_LOOP

        robot = Robot(name, NJ=NJ, NL=NL, NF=NF, is_mobile=False,
                 structure=structure)
        
        robot.G = Matrix([var('G1'), var('G2'), var('G3')])
        if self.structure[4][0]:
            robot.Z = transpose(self.elements[self.structure[4][0]-1].T)

        frames = []
        end = []
        cut = []
        
        for branch in self.branches[self.structure[0]:self.structure[1]+1]:
            frames += [i for i in branch[1:-2] if not isinstance(self.elements[i-1], Point)]
           
            if not self.elements[branch[-2]-1].virtual_joint:
                end.append(branch[-2])
            else:
                cut.append(branch[-2])


        frames = [ i for i in frames if not i in end and not i in cut]
        frames += end + cut
        
        sigma, mu, theta, alpha, gamma, d, r, b, ant = [],[],[],[],[],[],[],[],[]

        for frame in frames:
##            
            if isinstance(self.elements[frame-1], SuperPrismaticJoint):
                sigma.append(1)
            elif isinstance(self.elements[frame-1], SuperRevoluteJoint):
                sigma.append(0)
            else:
                sigma.append(2)
                
            i = [i for i in range(len(frames)) if frames[i] == self.elements[frame-1].ant]
            
            if len(i)==0 and self.elements[frame-1].ant==self.structure[4][0]:
                ant.append(0)
            elif len(i)>0:
                ant.append(i[0]+1)
            else:
                msg = wx.MessageDialog (None, 'Export error, cannot deifne the parameters.', style=wx.OK|wx.CENTRE)
                msg.ShowModal()
                return 

            if self.elements[frame-1].active and not self.elements[frame-1].cut_joint:
                mu.append(1)
            else:
                mu.append(0)
        
            if isinstance(self.elements[frame-1], SuperRevoluteJoint) and not self.elements[frame-1].virtual_joint:
                theta.append(var('th%s' % (len(theta)+1)))
            else:
                theta.append(self.elements[frame-1].theta)
                
            if isinstance(self.elements[frame-1], SuperPrismaticJoint) and not self.elements[frame-1].virtual_joint:
                r.append(var('r%s' % (len(theta)+1)))
            else:
                r.append(self.elements[frame-1].r)
                
            alpha.append(self.elements[frame-1].alpha)
            d.append(self.elements[frame-1].d)
            gamma.append(self.elements[frame-1].gamma)
            b.append(self.elements[frame-1].b)

        robot.sigma[1:] = sigma
        robot.theta[1:] = theta
        robot.alpha[1:] = alpha
        robot.gamma[1:] = gamma
        robot.d[1:] = d
        robot.r[1:] = r
        robot.b[1:] = b
        robot.ant[1:] = ant
        robot.mu[1:] = mu
                    
        parfile.writepar(robot)
예제 #7
0
def planar2r():
    """Generate Robot instance of 2R Planar robot"""
    robo = Robot("Planar2R", 2, 2, 2, False)
    robo.structure = tools.SIMPLE
    robo.sigma = [2, 0, 0]
    robo.mu = [0, 1, 1]
    robo.gamma = [0, 0, 0]
    robo.b = [0, 0, 0]
    robo.alpha = [0, 0, 0]
    robo.d = [0, 0, var("L1")]
    robo.theta = [0, var("q1"), var("q2")]
    robo.r = [0, 0, 0]
    robo.num = range(0, 3)
    robo.Nex = [zeros(3, 1) for i in robo.num]
    robo.Fex = [zeros(3, 1) for i in robo.num]
    robo.FS = [0 for i in robo.num]
    robo.IA = [0 for i in robo.num]
    robo.FV = [var("FV{0}".format(i)) for i in robo.num]
    robo.MS = [Matrix(var("MX{0}, MY{0}, MZ{0}".format(i))) for i in robo.num]
    robo.M = [var("M{0}".format(i)) for i in robo.num]
    robo.GAM = [var("GAM{0}".format(i)) for i in robo.num]
    inertia_matrix_terms = ("XX{0}, XY{0}, XZ{0}, ") + ("XY{0}, YY{0}, YZ{0}, ") + ("XZ{0}, YZ{0}, ZZ{0}")
    robo.J = [Matrix(3, 3, var(inertia_matrix_terms.format(i))) for i in robo.num]
    robo.G = Matrix([0, 0, -var("G3")])
    robo.w0 = zeros(3, 1)
    robo.wdot0 = zeros(3, 1)
    robo.v0 = zeros(3, 1)
    robo.vdot0 = zeros(3, 1)
    robo.q = [0, var("q1"), var("q2")]
    robo.qdot = [0, var("QP1"), var("QP2")]
    robo.qddot = [0, var("QDP1"), var("QDP2")]
    return robo
예제 #8
0
def cart_pole():
    """Generate Robot instance of classical CartPole dynamic system."""
    #TODO: bring it to the new notation with 0-frame
    robo = Robot()
    robo.name = 'CartPole'
    robo.ant = (-1, 0)
    robo.sigma = (1, 0)
    robo.alpha = (pi / 2, pi / 2)
    robo.d = (0, 0)
    robo.theta = (pi / 2, var('Th2'))
    robo.r = (var('R1'), 0)
    robo.b = (0, 0)
    robo.gamma = (0, 0)
    robo.num = range(1, 3)
    robo.NJ = 2
    robo.NL = 2
    robo.NF = 2
    robo.Nex = [zeros(3, 1) for i in robo.num]
    robo.Fex = [zeros(3, 1) for i in robo.num]
    robo.FS = [0 for i in robo.num]
    robo.IA = [0 for i in robo.num]
    robo.FV = [var('FV{0}'.format(i)) for i in robo.num]
    robo.MS = [zeros(3, 1) for i in robo.num]
    robo.MS[1][0] = var('MX2')
    robo.M = [var('M{0}'.format(i)) for i in robo.num]
    robo.GAM = [var('GAM{0}'.format(i)) for i in robo.num]
    robo.J = [zeros(3) for i in robo.num]
    robo.J[1][2, 2] = var('ZZ2')
    robo.G = Matrix([0, 0, -var('G3')])
    robo.w0 = zeros(3, 1)
    robo.wdot0 = zeros(3, 1)
    robo.v0 = zeros(3, 1)
    robo.vdot0 = zeros(3, 1)
    robo.q = var('R1, Th2')
    robo.qdot = var('R1d, Th2d')
    robo.qddot = var('R1dd, Th2dd')
    robo.num.append(0)
    return robo
예제 #9
0
def rx90():
    """Generate Robot instance of RX90"""
    robo = Robot("RX90", 6, 6, 6, False)
    # table of geometric parameters RX90
    robo.sigma = [2, 0, 0, 0, 0, 0, 0]
    robo.alpha = [0, 0, pi / 2, 0, -pi / 2, pi / 2, -pi / 2]
    robo.d = [0, 0, 0, var("D3"), 0, 0, 0]
    robo.theta = [0] + list(var("th1:7"))
    robo.r = [0, 0, 0, 0, var("RL4"), 0, 0]
    robo.b = [0, 0, 0, 0, 0, 0, 0]
    robo.gamma = [0, 0, 0, 0, 0, 0, 0]
    robo.mu = [0, 1, 1, 1, 1, 1, 1]
    robo.structure = tools.SIMPLE
    robo.w0 = zeros(3, 1)
    robo.wdot0 = zeros(3, 1)
    robo.v0 = zeros(3, 1)
    robo.vdot0 = zeros(3, 1)
    num = range(0, 7)
    robo.qdot = [var("QP{0}".format(i)) for i in num]
    robo.qddot = [var("QDP{0}".format(i)) for i in num]
    robo.Nex = [zeros(3, 1) for i in num]
    robo.Nex[-1] = Matrix(var("CX{0}, CY{0}, CZ{0}".format(robo.NJ)))
    robo.Fex = [zeros(3, 1) for i in num]
    robo.Fex[-1] = Matrix(var("FX{0}, FY{0}, FZ{0}".format(robo.NJ)))
    robo.FS = [var("FS{0}".format(i)) for i in num]
    robo.IA = [var("IA{0}".format(i)) for i in num]
    robo.FV = [var("FV{0}".format(i)) for i in num]
    robo.MS = [Matrix(var("MX{0}, MY{0}, MZ{0}".format(i))) for i in num]
    robo.M = [var("M{0}".format(i)) for i in num]
    robo.GAM = [var("GAM{0}".format(i)) for i in num]
    inertia_matrix_terms = ("XX{0}, XY{0}, XZ{0}, ") + ("XY{0}, YY{0}, YZ{0}, ") + ("XZ{0}, YZ{0}, ZZ{0}")
    robo.J = [Matrix(3, 3, var(inertia_matrix_terms.format(i))) for i in num]
    robo.G = Matrix([0, 0, var("G3")])
    return robo
예제 #10
0
def sr400():
    #TODO: bring it to the new notation with 0-frame
    """Generate Robot instance of SR400"""
    robo = Robot('SR400', 8, 9, 10, False)
    robo.ant = [-1, 0, 1, 2, 3, 4, 5, 1, 7, 8, 3]
    robo.sigma = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2]
    robo.mu = [0, 1, 1, 0, 1, 1, 1, 1, 0, 0, 0]
    robo.alpha = [0, 0, -pi / 2, 0, -pi / 2, pi / 2, -pi / 2, -pi / 2, 0, 0, 0]
    d_var = var('D:9')
    robo.d = [
        0, 0, d_var[2], d_var[3], d_var[4], 0, 0, d_var[2], d_var[8], d_var[3],
        -d_var[8]
    ]
    robo.theta = [0] + list(var('th1:10')) + [0]
    robo.r = [0, 0, 0, 0, var('RL4'), 0, 0, 0, 0, 0, 0]
    robo.b = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
    robo.gamma = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, pi / 2]
    robo.structure = tools.CLOSED_LOOP
    return robo
예제 #11
0
def planar2r():
    """Generate Robot instance of 2R Planar robot"""
    robo = Robot('Planar2R', 2, 2, 2, False)
    robo.structure = tools.SIMPLE
    robo.sigma = [2, 0, 0]
    robo.mu = [0, 1, 1]
    robo.gamma = [0, 0, 0]
    robo.b = [0, 0, 0]
    robo.alpha = [0, 0, 0]
    robo.d = [0, 0, var('L1')]
    robo.theta = [0, var('q1'), var('q2')]
    robo.r = [0, 0, 0]
    robo.num = range(0, 3)
    robo.Nex = [zeros(3, 1) for i in robo.num]
    robo.Fex = [zeros(3, 1) for i in robo.num]
    robo.FS = [0 for i in robo.num]
    robo.IA = [0 for i in robo.num]
    robo.FV = [var('FV{0}'.format(i)) for i in robo.num]
    robo.MS = [Matrix(var('MX{0}, MY{0}, MZ{0}'.format(i))) for i in robo.num]
    robo.M = [var('M{0}'.format(i)) for i in robo.num]
    robo.GAM = [var('GAM{0}'.format(i)) for i in robo.num]
    inertia_matrix_terms = ("XX{0}, XY{0}, XZ{0}, ") + \
        ("XY{0}, YY{0}, YZ{0}, ") + \
        ("XZ{0}, YZ{0}, ZZ{0}")
    robo.J = [
        Matrix(3, 3, var(inertia_matrix_terms.format(i))) \
        for i in robo.num
    ]
    robo.G = Matrix([0, 0, -var('G3')])
    robo.w0 = zeros(3, 1)
    robo.wdot0 = zeros(3, 1)
    robo.v0 = zeros(3, 1)
    robo.vdot0 = zeros(3, 1)
    robo.q = [0, var('q1'), var('q2')]
    robo.qdot = [0, var('QP1'), var('QP2')]
    robo.qddot = [0, var('QDP1'), var('QDP2')]
    return robo
예제 #12
0
def cart_pole():
    """Generate Robot instance of classical CartPole dynamic system."""
    #TODO: bring it to the new notation with 0-frame
    robo = Robot('CartPole', 2, 2, 2, False)
    robo.ant = (-1, 0, 1)
    robo.sigma = (0, 1, 0)
    robo.alpha = (0, pi / 2, pi / 2)
    robo.d = (0, 0, 0)
    robo.theta = (0, pi / 2, var('th2'))
    robo.r = (0, var('r1'), 0)
    robo.b = (0, 0, 0)
    robo.gamma = (0, 0, 0)
    robo.structure = tools.SIMPLE
    robo.num = range(0, 3)
    robo.Nex = [zeros(3, 1) for i in robo.num]
    robo.Fex = [zeros(3, 1) for i in robo.num]
    robo.FS = [0 for i in robo.num]
    robo.IA = [0 for i in robo.num]
    robo.FV = [var('FV{0}'.format(i)) for i in robo.num]
    robo.MS = [zeros(3, 1) for i in robo.num]
    robo.MS[1][0] = var('MX2')
    robo.M = [var('M{0}'.format(i)) for i in robo.num]
    robo.GAM = [var('GAM{0}'.format(i)) for i in robo.num]
    inertia_matrix_terms = ("XX{0}, XY{0}, XZ{0}, ") + \
        ("XY{0}, YY{0}, YZ{0}, ") + \
        ("XZ{0}, YZ{0}, ZZ{0}")
    robo.J = [
        Matrix(3, 3, var(inertia_matrix_terms.format(i))) \
        for i in robo.num
    ]
    robo.G = Matrix([0, 0, -var('G3')])
    robo.w0 = zeros(3, 1)
    robo.wdot0 = zeros(3, 1)
    robo.v0 = zeros(3, 1)
    robo.vdot0 = zeros(3, 1)
    robo.q = [0, var('r1'), var('th2')]
    robo.qdot = [0, var('r1d'), var('th2d')]
    robo.qddot = [0, var('r1dd'), var('th2dd')]
    return robo
예제 #13
0
def rx90():
    """Generate Robot instance of RX90"""
    robo = Robot('RX90', 6, 6, 6, False)
    # table of geometric parameters RX90
    robo.sigma = [2, 0, 0, 0, 0, 0, 0]
    robo.alpha = [0, 0, pi / 2, 0, -pi / 2, pi / 2, -pi / 2]
    robo.d = [0, 0, 0, var('D3'), 0, 0, 0]
    robo.theta = [0] + list(var('th1:7'))
    robo.r = [0, 0, 0, 0, var('RL4'), 0, 0]
    robo.b = [0, 0, 0, 0, 0, 0, 0]
    robo.gamma = [0, 0, 0, 0, 0, 0, 0]
    robo.mu = [0, 1, 1, 1, 1, 1, 1]
    robo.structure = tools.SIMPLE
    robo.w0 = zeros(3, 1)
    robo.wdot0 = zeros(3, 1)
    robo.v0 = zeros(3, 1)
    robo.vdot0 = zeros(3, 1)
    num = range(0, 7)
    robo.qdot = [var('QP{0}'.format(i)) for i in num]
    robo.qddot = [var('QDP{0}'.format(i)) for i in num]
    robo.Nex = [zeros(3, 1) for i in num]
    robo.Nex[-1] = Matrix(var('CX{0}, CY{0}, CZ{0}'.format(robo.NJ)))
    robo.Fex = [zeros(3, 1) for i in num]
    robo.Fex[-1] = Matrix(var('FX{0}, FY{0}, FZ{0}'.format(robo.NJ)))
    robo.FS = [var('FS{0}'.format(i)) for i in num]
    robo.IA = [var('IA{0}'.format(i)) for i in num]
    robo.FV = [var('FV{0}'.format(i)) for i in num]
    robo.MS = [Matrix(var('MX{0}, MY{0}, MZ{0}'.format(i))) for i in num]
    robo.M = [var('M{0}'.format(i)) for i in num]
    robo.GAM = [var('GAM{0}'.format(i)) for i in num]
    inertia_matrix_terms = ("XX{0}, XY{0}, XZ{0}, ") + \
        ("XY{0}, YY{0}, YZ{0}, ") + \
        ("XZ{0}, YZ{0}, ZZ{0}")
    robo.J = [
        Matrix(3, 3, var(inertia_matrix_terms.format(i))) \
        for i in num
    ]
    robo.G = Matrix([0, 0, var('G3')])
    return robo
예제 #14
0
def planar2r():
    """Generate Robot instance of 2R Planar robot"""
    robo = Robot('Planar2R', 2, 2, 3, False)
    robo.structure = tools.SIMPLE
    robo.sigma = [2, 0, 0, 2]
    robo.mu = [0, 1, 1, 0]
    robo.gamma = [0, 0, 0, 0]
    robo.b = [0, 0, 0, 0]
    robo.alpha = [0, 0, 0, 0]
    robo.d = [0, 0, var('L1'), var('L2')]
    robo.theta = [0, var('th1'), var('th2'), 0]
    robo.r = [0, 0, 0, 0]
    return robo
예제 #15
0
def cart_pole():
    """Generate Robot instance of classical CartPole dynamic system."""
    # TODO: bring it to the new notation with 0-frame
    robo = Robot("CartPole", 2, 2, 2, False)
    robo.ant = (-1, 0, 1)
    robo.sigma = (0, 1, 0)
    robo.alpha = (0, pi / 2, pi / 2)
    robo.d = (0, 0, 0)
    robo.theta = (0, pi / 2, var("th2"))
    robo.r = (0, var("r1"), 0)
    robo.b = (0, 0, 0)
    robo.gamma = (0, 0, 0)
    robo.structure = tools.SIMPLE
    robo.num = range(0, 3)
    robo.Nex = [zeros(3, 1) for i in robo.num]
    robo.Fex = [zeros(3, 1) for i in robo.num]
    robo.FS = [0 for i in robo.num]
    robo.IA = [0 for i in robo.num]
    robo.FV = [var("FV{0}".format(i)) for i in robo.num]
    robo.MS = [zeros(3, 1) for i in robo.num]
    robo.MS[1][0] = var("MX2")
    robo.M = [var("M{0}".format(i)) for i in robo.num]
    robo.GAM = [var("GAM{0}".format(i)) for i in robo.num]
    inertia_matrix_terms = ("XX{0}, XY{0}, XZ{0}, ") + ("XY{0}, YY{0}, YZ{0}, ") + ("XZ{0}, YZ{0}, ZZ{0}")
    robo.J = [Matrix(3, 3, var(inertia_matrix_terms.format(i))) for i in robo.num]
    robo.G = Matrix([0, 0, -var("G3")])
    robo.w0 = zeros(3, 1)
    robo.wdot0 = zeros(3, 1)
    robo.v0 = zeros(3, 1)
    robo.vdot0 = zeros(3, 1)
    robo.q = [0, var("r1"), var("th2")]
    robo.qdot = [0, var("r1d"), var("th2d")]
    robo.qddot = [0, var("r1dd"), var("th2dd")]
    return robo
예제 #16
0
 def OnNew(self, event):
     dialog = ui_definition.DialogDefinition(
         ui_labels.MAIN_WIN['prog_name'],
         self.robo.name, self.robo.nl,
         self.robo.nj, self.robo.structure, self.robo.is_mobile
     )
     if dialog.ShowModal() == wx.ID_OK:
         result = dialog.get_values()
         new_robo = Robot(*result['init_pars'])
         if result['keep_geo']:
             nf = min(self.robo.NF, new_robo.NF)
             new_robo.ant[:nf] = self.robo.ant[:nf]
             new_robo.sigma[:nf] = self.robo.sigma[:nf]
             new_robo.mu[:nf] = self.robo.mu[:nf]
             new_robo.gamma[:nf] = self.robo.gamma[:nf]
             new_robo.alpha[:nf] = self.robo.alpha[:nf]
             new_robo.theta[:nf] = self.robo.theta[:nf]
             new_robo.b[:nf] = self.robo.b[:nf]
             new_robo.d[:nf] = self.robo.d[:nf]
             new_robo.r[:nf] = self.robo.r[:nf]
         if result['keep_dyn']:
             nl = min(self.robo.NL, new_robo.NL)
             new_robo.Nex[:nl] = self.robo.Nex[:nl]
             new_robo.Fex[:nl] = self.robo.Fex[:nl]
             new_robo.FS[:nl] = self.robo.FS[:nl]
             new_robo.IA[:nl] = self.robo.IA[:nl]
             new_robo.FV[:nl] = self.robo.FV[:nl]
             new_robo.MS[:nl] = self.robo.MS[:nl]
             new_robo.M[:nl] = self.robo.M[:nl]
             new_robo.J[:nl] = self.robo.J[:nl]
         if result['keep_base']:
             new_robo.Z = self.robo.Z
             new_robo.w0 = self.robo.w0
             new_robo.wdot0 = self.robo.wdot0
             new_robo.v0 = self.robo.v0
             new_robo.vdot0 = self.robo.vdot0
             new_robo.G = self.robo.G
         self.robo = new_robo
         self.robo.directory = filemgr.get_folder_path(self.robo.name)
         self.feed_data()
     dialog.Destroy()
예제 #17
0
 def OnNew(self, event):
     dialog = ui_definition.DialogDefinition(
         ui_labels.MAIN_WIN['prog_name'],
         self.robo.name, self.robo.nl,
         self.robo.nj, self.robo.structure,
         self.robo.is_floating, self.robo.is_mobile
     )
     if dialog.ShowModal() == wx.ID_OK:
         result = dialog.get_values()
         new_robo = Robot(
             name=result['name'],
             NL=result['num_links'],
             NJ=result['num_joints'],
             NF=result['num_frames'],
             structure=result['structure'],
             is_floating=result['is_floating'],
             is_mobile=result['is_mobile']
         )
         new_robo.set_defaults(base=True)
         if result['keep_geo']:
             nf = min(self.robo.NF, new_robo.NF)
             new_robo.ant[:nf] = self.robo.ant[:nf]
             new_robo.sigma[:nf] = self.robo.sigma[:nf]
             new_robo.mu[:nf] = self.robo.mu[:nf]
             new_robo.gamma[:nf] = self.robo.gamma[:nf]
             new_robo.alpha[:nf] = self.robo.alpha[:nf]
             new_robo.theta[:nf] = self.robo.theta[:nf]
             new_robo.b[:nf] = self.robo.b[:nf]
             new_robo.d[:nf] = self.robo.d[:nf]
             new_robo.r[:nf] = self.robo.r[:nf]
         if result['keep_dyn']:
             nl = min(self.robo.NL, new_robo.NL)
             new_robo.Nex[:nl] = self.robo.Nex[:nl]
             new_robo.Fex[:nl] = self.robo.Fex[:nl]
             new_robo.FS[:nl] = self.robo.FS[:nl]
             new_robo.IA[:nl] = self.robo.IA[:nl]
             new_robo.FV[:nl] = self.robo.FV[:nl]
             new_robo.MS[:nl] = self.robo.MS[:nl]
             new_robo.M[:nl] = self.robo.M[:nl]
             new_robo.J[:nl] = self.robo.J[:nl]
         if result['keep_joint']:
             nj = min(self.robo.NJ, new_robo.NJ)
             new_robo.eta[:nj] = self.robo.eta[:nj]
             new_robo.k[:nj] = self.robo.k[:nj]
             new_robo.qdot[:nj] = self.robo.qdot[:nj]
             new_robo.qddot[:nj] = self.robo.qddot[:nj]
             new_robo.GAM[:nj] = self.robo.GAM[:nj]
         if result['keep_base']:
             new_robo.Z = self.robo.Z
             new_robo.w0 = self.robo.w0
             new_robo.wdot0 = self.robo.wdot0
             new_robo.v0 = self.robo.v0
             new_robo.vdot0 = self.robo.vdot0
             new_robo.G = self.robo.G
         new_robo.set_defaults(joint=True)
         self.robo = new_robo
         self.robo.directory = filemgr.get_folder_path(self.robo.name)
         self.feed_data()
     dialog.Destroy()