Ejemplo n.º 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)
     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)
Ejemplo n.º 2
0
 def test_dgm_SR400(self):
     print "######## test_dgm_SR400 ##########"
     self.robo = symoro.Robot.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)
Ejemplo n.º 3
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
    M = []
    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])
        M.append(J_col.T)
    Jac = Matrix(M).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 = -hat(iRj*jPn)
    if forced:
        symo.mat_replace(Jac, 'J', '', forced)
        L = symo.mat_replace(L, 'L', '', forced)
    return Jac, L
Ejemplo n.º 4
0
def _paul_solve(robo, symo, nTm, n, m, known=set()):
    chain = robo.loop_chain(m, n)
    iTn = dgm(robo, symo, m, n, key="left", trig_subs=False)
    iTm = dgm(robo, symo, n, m, key="left", trig_subs=False)
    #    mTi = dgm(robo, symo, m, n, key='right', trig_subs=False)
    #    nTi = dgm(robo, symo, n, m, key='right', trig_subs=False)
    th_all = set()
    r_all = set()
    for i in chain:
        if i >= 0:
            if robo.sigma[i] == 0:
                th_all.add(robo.theta[i])
            if robo.sigma[i] == 1:
                r_all.add(robo.r[i])
        if isinstance(robo.gamma[i], Expr):
            known |= robo.gamma[i].atoms(Symbol)
        if isinstance(robo.alpha[i], Expr):
            known |= robo.alpha[i].atoms(Symbol)
    while True:
        repeat = False
        for i in reversed(chain):
            M_eq = iTn[(i, n)] * nTm - iTm[(i, m)]
            while True:
                found = _look_for_eq(symo, M_eq, known, th_all, r_all)
                repeat |= found
                if not found or th_all | r_all <= known:
                    break
            if th_all | r_all <= known:
                break
        #        if th_all | r_all <= known:
        #            break
        #        for i in reversed(chain):
        #            while True:
        #                found = _look_for_eq(symo, M_eq, known, th_all, r_all)
        #                repeat |= found
        #                if not found or th_all | r_all <= known:
        #                    break
        #            if th_all | r_all <= known:
        #                break
        if not repeat or th_all | r_all <= known:
            break
    return known
Ejemplo n.º 5
0
 def test_loop(self):
     print "######## test_loop ##########"
     self.robo = symoro.Robot.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)
Ejemplo n.º 6
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(10):
         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)
Ejemplo n.º 7
0
 def test_jac(self):
     print "######## test_jac ##########"
     kinematics.jacobian(self.robo, 6, 3, 6)
     for j in xrange(5, 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)