Exemple #1
0
 def test_dgm_rx90(self):
     print "######## test_dgm_rx90 ##########"
     T = geometry.dgm(self.robo, self.symo, 0, 6,
                      fast_form=True, trig_subs=True)
     self.symo.gen_func_string('DGM_generated1', T, self.robo.q_vec,
                               syntax = 'matlab')
     f06 = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec)
     T = geometry.dgm(self.robo, self.symo, 6, 0,
                      fast_form=True, trig_subs=True)
     f60 = self.symo.gen_func('DGM_generated2', T, self.robo.q_vec)
     for x in xrange(10):
         arg = random.normal(size=6)
         M = matrix(f06(arg))*matrix(f60(arg))-eye(4)
         self.assertLess(amax(M), 1e-12)
     t06 = matrix([[1, 0, 0, 1], [0, 1, 0, 0],
                   [0, 0, 1, 1], [0, 0, 0, 1]])
     self.assertLess(amax(matrix(f06(zeros(6)))-t06), 1e-12)
     T46 = geometry.dgm(self.robo, self.symo, 4, 6,
                        fast_form=False, trig_subs=True)
     C4, S4, C5, C6, S5, S6, RL4 = var("C4,S4,C5,C6,S5,S6,RL4")
     T_true46 = Matrix([[C5*C6, -C5*S6, -S5, 0], [S6, C6, 0, 0],
                        [S5*C6, -S5*S6, C5, 0], [0, 0, 0, 1]])
     self.assertEqual(T46, T_true46)
     T36 = geometry.dgm(self.robo, self.symo, 3, 6,
                        fast_form=False, trig_subs=True)
     T_true36 = Matrix([[C4*C5*C6-S4*S6, -C4*C5*S6-S4*C6, -C4*S5, 0],
                        [S5*C6, -S5*S6, C5, RL4],
                        [-S4*C5*C6-C4*S6, S4*C5*S6-C4*C6, S4*S5, 0],
                        [0, 0, 0, 1]])
     self.assertEqual(T36, T_true36)
Exemple #2
0
def solve_orientation_prismatic(robo, symo, X_joints):
    """
    Function that solves the orientation equation of the three prismatic joints case. (to find the three angles)

    Parameters:
    ===========
    1) X_joint: The three revolute joints for the prismatic case
    """
    [i,j,k] = X_joints                   # X joints vector
    [S,S1,S2,S3,x] = [0,0,0,0,0]        # Book convention indexes
    [N,N1,N2,N3,y] = [1,1,1,1,1]        # Book convention indexes
    [A,A1,A2,A3,z] = [2,2,2,2,2]        # Book convention indexes
    robo.theta[i] = robo.theta[i] + robo.gamma[robo.ant[i]]
    robo.theta[j] = robo.theta[j] + robo.gamma[robo.ant[j]]
    robo.theta[k] = robo.theta[k] + robo.gamma[robo.ant[k]]

    T6k = dgm(robo, symo, 6, k, fast_form=True,trig_subs=True)
    Ti0 = dgm(robo, symo, robo.ant[i], 0, fast_form=True, trig_subs=True)
    Tji = dgm(robo, symo, j-1, i, fast_form=True, trig_subs=True)
    Tjk = dgm(robo, symo, j, k-1, fast_form=True, trig_subs=True)

    S3N3A3 = _rot_trans(axis=x, th=-robo.alpha[i], p=0)*Ti0*T_GENERAL*T6k   # S3N3A3 = rot(x,-alphai)*rot(z,-gami)*aiTo*SNA
    S2N2A2 = _rot_trans(axis=x, th=-robo.alpha[j], p=0)*Tji                 # S2N2A2 = iTa(j)*rot(x,-alphaj)
    S1N1A1 = Tjk*_rot_trans(axis=x, th=-robo.alpha[k], p=0)                 # S1N1A1 = jTa(k)*rot(x,alphak)

    S3N3A3 = Matrix([ S3N3A3[:3, :3] ])
    S2N2A2 = Matrix([ S2N2A2[:3, :3] ])
    S1N1A1 = Matrix([ S1N1A1[:3, :3] ])
    SNA = Matrix([ T_GENERAL[:3, :3] ])
    SNA = symo.replace(trigsimp(SNA), 'SNA')

    # solve thetai
    # eq 1.100 (page 49)
    eq_type = 3
    offset = robo.gamma[robo.ant[i]]
    robo.r[i] = robo.r[i] + robo.b[robo.ant[i]]
    el1 = S2N2A2[z,S2]*S3N3A3[x,A3] + S2N2A2[z,N2]*S3N3A3[y,A3]
    el2 = S2N2A2[z,S2]*S3N3A3[y,A3] - S2N2A2[z,N2]*S3N3A3[x,A3]
    el3 = S2N2A2[z,A2]*S3N3A3[z,A3] - S1N1A1[z,A1]
    coef = [el1,el2,el3]
    _equation_solve(symo, coef, eq_type, robo.theta[i], offset)

    # solve thetaj
    # eq 1.102
    eq_type = 4
    offset = robo.gamma[robo.ant[j]]
    robo.r[j] = robo.r[j] + robo.b[robo.ant[j]]
    coef = [S1N1A1[x,A1] , -S1N1A1[y,A1] , -SNA[x,A] , S1N1A1[y,A1] , S1N1A1[x,A1] , -SNA[y,A] ]
    _equation_solve(symo, coef, eq_type, robo.theta[j], offset)

    # solve thetak
    # eq 1.103
    eq_type = 4
    offset = robo.gamma[robo.ant[k]]
    robo.r[k] = robo.r[k] + robo.b[robo.ant[k]]
    coef = [S1N1A1[z,S1] , S1N1A1[z,N1] , -SNA[z,S] , S1N1A1[z,N1] , -S1N1A1[z,S1] , -SNA[z,N] ]
    _equation_solve(symo, coef, eq_type, robo.theta[k], offset)

    return
Exemple #3
0
def solve_orientation(robo, symo, pieper_joints):
    """
    Function that solves the orientation equation for the four spherical cases.

    Parameters:
    ===========
    1) pieper_joints: Joints that form the spherical wrist
    """
    m = pieper_joints[1]  # Center of the spherical joint
    [S, N, A] = [0, 1, 2]  # Book convention indexes
    [x, y, z] = [0, 1, 2]  # Book convention indexes

    t1 = dgm(robo, symo, m - 2, 0, fast_form=True, trig_subs=True)
    t2 = dgm(robo, symo, 6, m + 1, fast_form=True, trig_subs=True)

    Am2A0 = Matrix([t1[:3, :3]])
    A6Am1 = Matrix([t2[:3, :3]])

    A0 = T_GENERAL[:3, :3]

    SNA = _rot(axis=x, th=-robo.alpha[m - 1]) * Am2A0 * A0 * A6Am1
    SNA = symo.replace(trigsimp(SNA), 'SNA')

    # calculate theta(m-1) (i)
    # eq 1.68
    eq_type = 3
    offset = robo.gamma[robo.ant[m - 1]]
    robo.r[m - 1] = robo.r[m - 1] + robo.b[robo.ant[m - 1]]
    coef = [
        -SNA[y, A] * sin(robo.alpha[m]), SNA[x, A] * sin(robo.alpha[m]),
        SNA[z, A] * cos(robo.alpha[m]) - cos(robo.alpha[m + 1])
    ]
    _equation_solve(symo, coef, eq_type, robo.theta[m - 1], offset)

    # calculate theta(m) (j)
    # eq 1.72
    S1N1A1 = _rot(axis=x, th=-robo.alpha[m]) * _rot(
        axis=z, th=-robo.theta[m - 1]) * SNA
    eq_type = 4
    offset = robo.gamma[robo.ant[m]]
    robo.r[m] = robo.r[m] + robo.b[robo.ant[m]]
    symo.write_line("\r\n\r\n")
    B1 = symo.replace(trigsimp(-S1N1A1[x, A]), 'B1', robo.theta[m])
    B2 = symo.replace(trigsimp(S1N1A1[y, A]), 'B2', robo.theta[m])
    coef = [0, sin(robo.alpha[m + 1]), B1, sin(robo.alpha[m + 1]), 0, B2]
    _equation_solve(symo, coef, eq_type, robo.theta[m], offset)

    # calculate theta(m+1) (k)
    # eq 1.73
    eq_type = 4
    offset = robo.gamma[robo.ant[m + 1]]
    robo.r[m + 1] = robo.r[m + 1] + robo.b[robo.ant[m + 1]]
    symo.write_line("\r\n\r\n")
    B1 = symo.replace(trigsimp(-S1N1A1[z, S]), 'B1', robo.theta[m + 1])
    B2 = symo.replace(trigsimp(-S1N1A1[z, N]), 'B2', robo.theta[m + 1])
    coef = [0, sin(robo.alpha[m + 1]), B1, sin(robo.alpha[m + 1]), 0, B2]
    _equation_solve(symo, coef, eq_type, robo.theta[m + 1], offset)

    return
Exemple #4
0
def solve_orientation(robo, symo, pieper_joints):
    """
    Function that solves the orientation equation for the four spherical cases.

    Parameters:
    ===========
    1) pieper_joints: Joints that form the spherical wrist
    """
    m = pieper_joints[1]        # Center of the spherical joint
    [S,N,A] = [0,1,2]           # Book convention indexes
    [x,y,z] = [0,1,2]           # Book convention indexes

    t1 = dgm(robo, symo, m-2, 0, fast_form=True, trig_subs=True)
    t2 = dgm(robo, symo, 6, m+1, fast_form=True, trig_subs=True)

    Am2A0 = Matrix([ t1[:3,:3] ])
    A6Am1 = Matrix([ t2[:3,:3] ])

    A0 = T_GENERAL[:3,:3]

    SNA = _rot(axis=x, th=-robo.alpha[m-1])*Am2A0*A0*A6Am1
    SNA = symo.replace(trigsimp(SNA), 'SNA')

    # calculate theta(m-1) (i)
    # eq 1.68
    eq_type = 3
    offset = robo.gamma[robo.ant[m-1]]
    robo.r[m-1] = robo.r[m-1] + robo.b[robo.ant[m-1]]
    coef = [-SNA[y,A]*sin(robo.alpha[m]) , SNA[x,A]*sin(robo.alpha[m]) , SNA[z,A]*cos(robo.alpha[m])-cos(robo.alpha[m+1])]
    _equation_solve(symo, coef, eq_type, robo.theta[m-1], offset)

    # calculate theta(m) (j)
    # eq 1.72
    S1N1A1 = _rot(axis=x, th=-robo.alpha[m])*_rot(axis=z, th=-robo.theta[m-1])*SNA
    eq_type = 4
    offset = robo.gamma[robo.ant[m]]
    robo.r[m] = robo.r[m] + robo.b[robo.ant[m]]
    symo.write_line("\r\n\r\n")
    B1 = symo.replace(trigsimp(-S1N1A1[x,A]), 'B1', robo.theta[m])
    B2 = symo.replace(trigsimp(S1N1A1[y,A]), 'B2', robo.theta[m])
    coef = [0, sin(robo.alpha[m+1]), B1, sin(robo.alpha[m+1]), 0, B2]
    _equation_solve(symo, coef, eq_type, robo.theta[m], offset)

    # calculate theta(m+1) (k)
    # eq 1.73
    eq_type = 4
    offset = robo.gamma[robo.ant[m+1]]
    robo.r[m+1] = robo.r[m+1] + robo.b[robo.ant[m+1]]
    symo.write_line("\r\n\r\n")
    B1 = symo.replace(trigsimp(-S1N1A1[z,S]), 'B1', robo.theta[m+1])
    B2 = symo.replace(trigsimp(-S1N1A1[z,N]), 'B2', robo.theta[m+1])
    coef = [0, sin(robo.alpha[m+1]), B1, sin(robo.alpha[m+1]), 0, B2]
    _equation_solve(symo, coef, eq_type, robo.theta[m+1], offset)

    return
Exemple #5
0
 def test_igm_2r(self):
     print "######## test_igm ##########"
     robo = samplerobots.planar2r()
     nTm = Matrix(4, 4, 12 * [invgeom.EMPTY] + [0, 0, 0, 1])
     nTm[0, 3], nTm[1, 3] = var('p1, p2')
     invgeom._paul_solve(robo, self.symo, nTm, 0, robo.nf)
     self.symo.gen_func_string('IGM_gen',
                               robo.q_vec,
                               var('p1, p2'),
                               syntax='matlab')
     igm_f = self.symo.gen_func('IGM_gen', robo.q_vec, var('p1, p2'))
     T = geometry.dgm(robo,
                      self.symo,
                      0,
                      robo.nf,
                      fast_form=True,
                      trig_subs=True)
     f06 = self.symo.gen_func('DGM_generated1', (T[0, 3], T[1, 3]),
                              robo.q_vec)
     for x in xrange(100):
         arg = random.normal(size=robo.nj)
         Ttest = f06(arg)
         solution = igm_f(Ttest)
         for q in solution:
             self.assertLess(amax(matrix(f06(q)) - Ttest), 1e-12)
Exemple #6
0
 def test_dgm_sr400(self):
     print "######## test_dgm_sr400 ##########"
     self.robo = samplerobots.sr400()
     T = geometry.dgm(self.robo, self.symo, 0, 6,
                      fast_form=True, trig_subs=True)
     f06 = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec)
     T = geometry.dgm(self.robo, self.symo, 6, 0,
                      fast_form=True, trig_subs=True)
     f60 = self.symo.gen_func('DGM_generated2', T, self.robo.q_vec)
     for x in xrange(10):
         arg = random.normal(size=9)
         M = matrix(f06(arg))*matrix(f60(arg))-eye(4)
         self.assertLess(amax(M), 1e-12)
     t06 = matrix([[1, 0, 0, 3], [0, -1, 0, 0],
                   [0, 0, -1, -1], [0, 0, 0, 1]])
     self.assertLess(amax(matrix(f06(zeros(9))) - t06), 1e-12)
Exemple #7
0
 def test_igm_rx90(self):
     print "######## test_igm ##########"
     robo = samplerobots.rx90()
     #robo.r[6] = var('R6')
     #robo.gamma[6] = var('G6')  # invgeom.T_GENERAL
     nTm = invgeom.T_GENERAL
     invgeom._paul_solve(robo, self.symo, nTm, 0, robo.nf)
     self.symo.gen_func_string('IGM_gen',
                               robo.q_vec,
                               invgeom.T_GENERAL,
                               syntax='matlab')
     igm_f = self.symo.gen_func('IGM_gen', robo.q_vec, invgeom.T_GENERAL)
     T = geometry.dgm(robo,
                      self.symo,
                      0,
                      robo.nf,
                      fast_form=True,
                      trig_subs=True)
     f06 = self.symo.gen_func('DGM_generated1', T, robo.q_vec)
     for x in xrange(100):
         arg = random.normal(size=robo.nj)
         Ttest = f06(arg)
         solution = igm_f(Ttest)
         for q in solution:
             self.assertLess(amax(matrix(f06(q)) - Ttest), 1e-12)
Exemple #8
0
def solve_position_prismatic(robo, symo, pieper_joints):
    """
    Function that solves the position equation of the three prismatic joints case. (to find the three angles)

    Parameters:
    ===========
    1) Pieper_joints: The three prismatic joints for the prismatic case
    """
    eq_type = 0  # Type 0: Linear system
    [i, j, k] = pieper_joints  # Pieper joints vector
    [x, y, z] = [0, 1, 2]  # Book convention indexes
    robo.theta[i] = robo.theta[i] + robo.gamma[robo.ant[i]]
    robo.theta[j] = robo.theta[j] + robo.gamma[robo.ant[j]]
    robo.theta[k] = robo.theta[k] + robo.gamma[robo.ant[k]]

    T6k = dgm(robo, symo, 6, k, fast_form=True, trig_subs=True)
    Ti0 = dgm(robo, symo, robo.ant[i], 0, fast_form=True, trig_subs=True)
    Tji = dgm(robo, symo, j - 1, i, fast_form=True, trig_subs=True)
    Tjk = dgm(robo, symo, j, k - 1, fast_form=True, trig_subs=True)

    S3N3A3P3 = _rot_trans(axis=z, th=-robo.theta[i], p=0) * _rot_trans(
        axis=x, th=-robo.alpha[i], p=-robo.d[i]
    ) * Ti0 * T_GENERAL * T6k  # S3N3A3 = rot(z,-thetai)*Trans(x,-di)*rot(x,-alphai)*a(i)To*SNAP*6Tk
    S2N2A2P2 = _rot_trans(axis=z, th=-robo.theta[j], p=0) * _rot_trans(
        axis=x, th=-robo.alpha[j], p=-robo.d[j]
    ) * Tji  # S2N2A2 = rot(z,-thetaj)*Trans(x,-dj)*rot(x,-alphaj)*a(j)Ti
    S1N1A1P1 = Tjk * _rot_trans(
        axis=x, th=robo.alpha[k], p=robo.d[k]) * _rot_trans(
            axis=z, th=robo.theta[k],
            p=0)  # S1N1A1 = jTa(k)*rot(x,alphak)*Trans(x,dk)*rot(z,thetak)

    S2N2A2 = array(S2N2A2P2[:3, :3])
    P3 = array(S3N3A3P3[:3, 3])
    P2 = array(S2N2A2P2[:3, 3])
    P1 = array(S1N1A1P1[:3, 3])

    t2 = S2N2A2[:3, 2]
    P4 = P1 - dot(S2N2A2, P3) - P2
    t1 = S1N1A1P1[:3, 2]
    coef = Matrix([[t1[0], t1[1], t1[2]], [t2[0], t2[1], t2[2]],
                   [P4[0][0], P4[1][0], P4[2][0]]])
    rijk = [robo.r[i], robo.r[j], robo.r[k]]
    offset = [robo.b[robo.ant[k]], robo.b[robo.ant[j]], robo.b[robo.ant[i]]]
    _equation_solve(symo, coef, eq_type, rijk, offset)

    return
Exemple #9
0
def _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False):
    """
    Computes jacobian of frame n (with origin On in Oj) projected to frame i
    """
    #  symo.write_geom_param(robo, 'Jacobian')
    # TODO: Check projection frames, rewrite DGM call for higher efficiency
    J_col_list = []
    if chain is None:
        chain = robo.chain(n)
        chain.reverse()
    # chain_ext = chain + [robo.ant[min(chain)]]
    # if not i in chain_ext:
    #     i = min(chain_ext)
    # if not j in chain_ext:
    #     j = max(chain_ext)
    kTj_dict = dgm(robo, symo, chain[0], j, key='left', trig_subs=trig_subs)
    kTj_tmp = dgm(robo, symo, chain[-1], j, key='left', trig_subs=trig_subs)
    kTj_dict.update(kTj_tmp)
    iTk_dict = dgm(robo, symo, i, chain[0], key='right', trig_subs=trig_subs)
    iTk_tmp = dgm(robo, symo, i, chain[-1], key='right', trig_subs=trig_subs)
    iTk_dict.update(iTk_tmp)
    for k in chain:
        kTj = kTj_dict[k, j]
        iTk = iTk_dict[i, k]
        isk, ink, iak = Transform.sna(iTk)
        sigm = robo.sigma[k]
        if sigm == 1:
            dvdq = iak
            J_col = dvdq.col_join(Matrix([0, 0, 0]))
        elif sigm == 0:
            dvdq = kTj[0, 3]*ink-kTj[1, 3]*isk
            J_col = dvdq.col_join(iak)
        else:
            J_col = Matrix([0, 0, 0, 0, 0, 0])
        J_col_list.append(J_col.T)
    Jac = Matrix(J_col_list).T
    Jac = Jac.applyfunc(symo.simp)
    iRj = Transform.R(iTk_dict[i, j])
    jTn = dgm(robo, symo, j, n, fast_form=False, trig_subs=trig_subs)
    jPn = Transform.P(jTn)
    L = -tools.skew(iRj*jPn)
    L = L.applyfunc(trigsimp)
    if forced:
        symo.mat_replace(Jac, 'J', '', forced)
        L = symo.mat_replace(L, 'L', '', forced)
    return Jac, L
Exemple #10
0
def _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False):
    """
    Computes jacobian of frame n (with origin On in Oj) projected to frame i
    """
    #  symo.write_geom_param(robo, 'Jacobian')
    # TODO: Check projection frames, rewrite DGM call for higher efficiency
    J_col_list = []
    if chain is None:
        chain = robo.chain(n)
        chain.reverse()
    # chain_ext = chain + [robo.ant[min(chain)]]
    # if not i in chain_ext:
    #     i = min(chain_ext)
    # if not j in chain_ext:
    #     j = max(chain_ext)
    kTj_dict = dgm(robo, symo, chain[0], j, key='left', trig_subs=trig_subs)
    kTj_tmp = dgm(robo, symo, chain[-1], j, key='left', trig_subs=trig_subs)
    kTj_dict.update(kTj_tmp)
    iTk_dict = dgm(robo, symo, i, chain[0], key='right', trig_subs=trig_subs)
    iTk_tmp = dgm(robo, symo, i, chain[-1], key='right', trig_subs=trig_subs)
    iTk_dict.update(iTk_tmp)
    for k in chain:
        kTj = kTj_dict[k, j]
        iTk = iTk_dict[i, k]
        isk, ink, iak = Transform.sna(iTk)
        sigm = robo.sigma[k]
        if sigm == 1:
            dvdq = iak
            J_col = dvdq.col_join(Matrix([0, 0, 0]))
        elif sigm == 0:
            dvdq = kTj[0, 3] * ink - kTj[1, 3] * isk
            J_col = dvdq.col_join(iak)
        else:
            J_col = Matrix([0, 0, 0, 0, 0, 0])
        J_col_list.append(J_col.T)
    Jac = Matrix(J_col_list).T
    Jac = Jac.applyfunc(symo.simp)
    iRj = Transform.R(iTk_dict[i, j])
    jTn = dgm(robo, symo, j, n, fast_form=False, trig_subs=trig_subs)
    jPn = Transform.P(jTn)
    L = -tools.skew(iRj * jPn)
    L = L.applyfunc(trigsimp)
    if forced:
        symo.mat_replace(Jac, 'J', '', forced)
        L = symo.mat_replace(L, 'L', '', forced)
    return Jac, L
Exemple #11
0
 def dgm_for_frame(self, i):
     if i not in self.dgms:
         jnt = self.jnt_objs[i]
         if i > 0 and jnt.r == 0 and jnt.d == 0 and jnt.b == 0:
             self.dgms[i] = self.dgm_for_frame(self.robo.ant[i])
         else:
             symo = symbolmgr.SymbolManager(sydi=self.pars_num)
             T = dgm(self.robo, symo, 0, i, fast_form=True, trig_subs=True)
             self.dgms[i] = symo.gen_func('dgm_generated', T, self.q_sym)
     return self.dgms[i]
Exemple #12
0
 def dgm_for_frame(self, i):
     if i not in self.dgms:
         jnt = self.jnt_objs[i]
         if i > 0 and jnt.r == 0 and jnt.d == 0 and jnt.b == 0:
             self.dgms[i] = self.dgm_for_frame(self.robo.ant[i])
         else:
             symo = symbolmgr.SymbolManager(sydi=self.pars_num)
             T = dgm(self.robo, symo, 0, i, fast_form=True, trig_subs=True)
             self.dgms[i] = symo.gen_func('dgm_generated', T, self.q_sym)
     return self.dgms[i]
Exemple #13
0
def solve_position_prismatic(robo, symo, pieper_joints):
    """
    Function that solves the position equation of the three prismatic joints case. (to find the three angles)

    Parameters:
    ===========
    1) Pieper_joints: The three prismatic joints for the prismatic case
    """
    eq_type = 0                       # Type 0: Linear system
    [i,j,k] = pieper_joints           # Pieper joints vector
    [x,y,z] = [0,1,2]                 # Book convention indexes
    robo.theta[i] = robo.theta[i] + robo.gamma[robo.ant[i]]
    robo.theta[j] = robo.theta[j] + robo.gamma[robo.ant[j]]
    robo.theta[k] = robo.theta[k] + robo.gamma[robo.ant[k]]

    T6k = dgm(robo, symo, 6, k, fast_form=True, trig_subs=True)
    Ti0 = dgm(robo, symo, robo.ant[i], 0, fast_form=True, trig_subs=True)
    Tji = dgm(robo, symo, j-1, i, fast_form=True, trig_subs=True)
    Tjk = dgm(robo, symo, j, k-1, fast_form=True, trig_subs=True)

    S3N3A3P3 = _rot_trans(axis=z, th=-robo.theta[i], p=0)*_rot_trans(axis=x, th=-robo.alpha[i], p=-robo.d[i])*Ti0*T_GENERAL*T6k # S3N3A3 = rot(z,-thetai)*Trans(x,-di)*rot(x,-alphai)*a(i)To*SNAP*6Tk
    S2N2A2P2 = _rot_trans(axis=z, th=-robo.theta[j], p=0)*_rot_trans(axis=x, th=-robo.alpha[j], p=-robo.d[j])*Tji               # S2N2A2 = rot(z,-thetaj)*Trans(x,-dj)*rot(x,-alphaj)*a(j)Ti
    S1N1A1P1 = Tjk*_rot_trans(axis=x, th=robo.alpha[k], p=robo.d[k])*_rot_trans(axis=z, th=robo.theta[k], p=0)                  # S1N1A1 = jTa(k)*rot(x,alphak)*Trans(x,dk)*rot(z,thetak)

    S2N2A2 = array( S2N2A2P2[:3, :3] )
    P3 = array( S3N3A3P3[:3, 3] )
    P2 = array( S2N2A2P2[:3, 3] )
    P1 = array( S1N1A1P1[:3, 3] )

    t2 = S2N2A2[:3, 2]
    P4 = P1 - dot(S2N2A2, P3) - P2
    t1 = S1N1A1P1[:3,2]
    coef = Matrix([[t1[0],t1[1],t1[2]],[t2[0],t2[1],t2[2]],[P4[0][0],P4[1][0],P4[2][0]]])
    rijk = [robo.r[i], robo.r[j], robo.r[k]]
    offset = [robo.b[robo.ant[k]], robo.b[robo.ant[j]], robo.b[robo.ant[i]]]
    _equation_solve(symo, coef, eq_type, rijk, offset)

    return
Exemple #14
0
 def test_loop(self):
     print "######## test_loop ##########"
     self.robo = samplerobots.sr400()
     invgeom.loop_solve(self.robo, self.symo)
     l_solver = self.symo.gen_func('IGM_gen', self.robo.q_vec,
                                   self.robo.q_active)
     T = geometry.dgm(self.robo, self.symo, 9, 10,
                      fast_form=True, trig_subs=True)
     t_loop = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec)
     for x in xrange(10):
         arg = random.normal(size=6)
         solution = l_solver(arg)
         for q in solution:
             self.assertLess(amax(matrix(t_loop(q))-eye(4)), 1e-12)
Exemple #15
0
 def test_igm(self):
     print "######## test_igm ##########"
     invgeom._paul_solve(self.robo, self.symo, invgeom.T_GENERAL, 0, 6)
     igm_f = self.symo.gen_func('IGM_gen', self.robo.q_vec,
                                invgeom.T_GENERAL)
     T = geometry.dgm(self.robo, self.symo, 0, 6,
                      fast_form=True, trig_subs=True)
     f06 = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec)
     for x in xrange(100):
         arg = random.normal(size=6)
         Ttest = f06(arg)
         solution = igm_f(Ttest)
         for q in solution:
             self.assertLess(amax(matrix(f06(q))-Ttest), 1e-12)
Exemple #16
0
 def test_loop(self):
     print "######## test_loop ##########"
     self.robo = samplerobots.sr400()
     invgeom.loop_solve(self.robo, self.symo)
     self.symo.gen_func_string('IGM_gen', self.robo.q_vec,
                               self.robo.q_active, syntax = 'matlab')
     l_solver = self.symo.gen_func('IGM_gen', self.robo.q_vec,
                                   self.robo.q_active)
     T = geometry.dgm(self.robo, self.symo, 9, 10,
                      fast_form=True, trig_subs=True)
     t_loop = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec)
     for x in xrange(10):
         arg = random.normal(size=6)
         solution = l_solver(arg)
         for q in solution:
             self.assertLess(amax(matrix(t_loop(q))-eye(4)), 1e-12)
Exemple #17
0
 def test_igm(self):
     print "######## test_igm ##########"
     self.robo.r[6] = var('R6')
     self.robo.gamma[6] = var('G6')
     invgeom._paul_solve(self.robo, self.symo, invgeom.T_GENERAL, 0, 6)
     self.symo.gen_func_string('IGM_gen', self.robo.q_vec,
                                invgeom.T_GENERAL, syntax = 'matlab')
     igm_f = self.symo.gen_func('IGM_gen', self.robo.q_vec,
                                invgeom.T_GENERAL)
     T = geometry.dgm(self.robo, self.symo, 0, 6,
                      fast_form=True, trig_subs=True)
     f06 = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec)
     for x in xrange(100):
         arg = random.normal(size=6)
         Ttest = f06(arg)
         solution = igm_f(Ttest)
         for q in solution:
             self.assertLess(amax(matrix(f06(q))-Ttest), 1e-12)
Exemple #18
0
 def test_jac(self):
     print "######## test_jac ##########"
     kinematics.jacobian(self.robo, 6, 3, 6)
     for j in xrange(1, 7):
         print "######## Jac validation through DGM ##########"
         #compute Jac
         J, l = kinematics._jac(self.robo, self.symo, j, 0, j)
         jacj = self.symo.gen_func('JacRX90', J, self.robo.q_vec)
         #compute DGM
         T = geometry.dgm(self.robo, self.symo, 0, j,
                          fast_form=True, trig_subs=True)
         T0j = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec)
         for i in xrange(10):
             dq = random.normal(size=6, scale=1e-7)
             q = random.normal(size=6)
             dX = matrix(jacj(q)) * matrix(dq[:j]).T
             T = (matrix(T0j(q+dq)) - T0j(q))
             self.assertLess(amax(dX[:3] - trns.P(T)), 1e-12)
Exemple #19
0
 def test_igm_rx90(self):
     robo = samplerobots.rx90()
     #robo.r[6] = var('R6')
     #robo.gamma[6] = var('G6')  # invgeom.T_GENERAL
     nTm = invgeom.T_GENERAL
     invgeom._paul_solve(robo, self.symo, nTm, 0, robo.nf)
     self.symo.gen_func_string('IGM_gen', robo.q_vec,
                               invgeom.T_GENERAL, syntax='matlab')
     igm_f = self.symo.gen_func('IGM_gen', robo.q_vec,
                                invgeom.T_GENERAL)
     T = geometry.dgm(robo, self.symo, 0, robo.nf,
                      fast_form=True, trig_subs=True)
     f06 = self.symo.gen_func('DGM_generated1', T, robo.q_vec)
     for x in xrange(100):
         arg = random.normal(size=robo.nj)
         Ttest = f06(arg)
         solution = igm_f(Ttest)
         for q in solution:
             self.assertLess(amax(matrix(f06(q))-Ttest), 1e-12)
Exemple #20
0
 def test_igm_2r(self):
     robo = samplerobots.planar2r()
     nTm = Matrix(4, 4, 12 * [invgeom.EMPTY] + [0, 0, 0, 1])
     nTm[0, 3], nTm[1, 3] = var('p1, p2')
     invgeom._paul_solve(robo, self.symo, nTm, 0, robo.nf)
     self.symo.gen_func_string('IGM_gen', robo.q_vec,
                               var('p1, p2'), syntax='matlab')
     igm_f = self.symo.gen_func('IGM_gen', robo.q_vec,
                                var('p1, p2'))
     T = geometry.dgm(robo, self.symo, 0, robo.nf,
                      fast_form=True, trig_subs=True)
     f06 = self.symo.gen_func('DGM_generated1', (T[0, 3], T[1, 3]),
                              robo.q_vec)
     for x in xrange(100):
         arg = random.normal(size=robo.nj)
         Ttest = f06(arg)
         solution = igm_f(Ttest)
         for q in solution:
             self.assertLess(amax(matrix(f06(q))-Ttest), 1e-12)
Exemple #21
0
def solve_orientation_prismatic(robo, symo, X_joints):
    """
    Function that solves the orientation equation of the three prismatic joints case. (to find the three angles)

    Parameters:
    ===========
    1) X_joint: The three revolute joints for the prismatic case
    """
    [i, j, k] = X_joints  # X joints vector
    [S, S1, S2, S3, x] = [0, 0, 0, 0, 0]  # Book convention indexes
    [N, N1, N2, N3, y] = [1, 1, 1, 1, 1]  # Book convention indexes
    [A, A1, A2, A3, z] = [2, 2, 2, 2, 2]  # Book convention indexes
    robo.theta[i] = robo.theta[i] + robo.gamma[robo.ant[i]]
    robo.theta[j] = robo.theta[j] + robo.gamma[robo.ant[j]]
    robo.theta[k] = robo.theta[k] + robo.gamma[robo.ant[k]]

    T6k = dgm(robo, symo, 6, k, fast_form=True, trig_subs=True)
    Ti0 = dgm(robo, symo, robo.ant[i], 0, fast_form=True, trig_subs=True)
    Tji = dgm(robo, symo, j - 1, i, fast_form=True, trig_subs=True)
    Tjk = dgm(robo, symo, j, k - 1, fast_form=True, trig_subs=True)

    S3N3A3 = _rot_trans(
        axis=x, th=-robo.alpha[i], p=0
    ) * Ti0 * T_GENERAL * T6k  # S3N3A3 = rot(x,-alphai)*rot(z,-gami)*aiTo*SNA
    S2N2A2 = _rot_trans(axis=x, th=-robo.alpha[j],
                        p=0) * Tji  # S2N2A2 = iTa(j)*rot(x,-alphaj)
    S1N1A1 = Tjk * _rot_trans(axis=x, th=-robo.alpha[k],
                              p=0)  # S1N1A1 = jTa(k)*rot(x,alphak)

    S3N3A3 = Matrix([S3N3A3[:3, :3]])
    S2N2A2 = Matrix([S2N2A2[:3, :3]])
    S1N1A1 = Matrix([S1N1A1[:3, :3]])
    SNA = Matrix([T_GENERAL[:3, :3]])
    SNA = symo.replace(trigsimp(SNA), 'SNA')

    # solve thetai
    # eq 1.100 (page 49)
    eq_type = 3
    offset = robo.gamma[robo.ant[i]]
    robo.r[i] = robo.r[i] + robo.b[robo.ant[i]]
    el1 = S2N2A2[z, S2] * S3N3A3[x, A3] + S2N2A2[z, N2] * S3N3A3[y, A3]
    el2 = S2N2A2[z, S2] * S3N3A3[y, A3] - S2N2A2[z, N2] * S3N3A3[x, A3]
    el3 = S2N2A2[z, A2] * S3N3A3[z, A3] - S1N1A1[z, A1]
    coef = [el1, el2, el3]
    _equation_solve(symo, coef, eq_type, robo.theta[i], offset)

    # solve thetaj
    # eq 1.102
    eq_type = 4
    offset = robo.gamma[robo.ant[j]]
    robo.r[j] = robo.r[j] + robo.b[robo.ant[j]]
    coef = [
        S1N1A1[x, A1], -S1N1A1[y, A1], -SNA[x, A], S1N1A1[y, A1],
        S1N1A1[x, A1], -SNA[y, A]
    ]
    _equation_solve(symo, coef, eq_type, robo.theta[j], offset)

    # solve thetak
    # eq 1.103
    eq_type = 4
    offset = robo.gamma[robo.ant[k]]
    robo.r[k] = robo.r[k] + robo.b[robo.ant[k]]
    coef = [
        S1N1A1[z, S1], S1N1A1[z, N1], -SNA[z, S], S1N1A1[z, N1],
        -S1N1A1[z, S1], -SNA[z, N]
    ]
    _equation_solve(symo, coef, eq_type, robo.theta[k], offset)

    return