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