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)
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)
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
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
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)
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)
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)