def group_param_rot_spec(robo, symo, j, lam, antRj):
"""Internal function. Groups inertia parameters according to the
special rule for a rotational joint.
Notes
=====
robo is the output paramete
"""
chainj = robo.chain(j)
r1, r2, orthog = Transform.find_r12(robo, chainj, antRj, j)
kRj, all_paral = Transform.kRj(robo, antRj, r1, chainj)
Kj = robo.get_inert_param(j)
to_replace = {0, 1, 2, 4, 5, 6, 7}
if Transform.z_paral(kRj):
Kj[0] = 0 # XX
Kj[1] = 0 # XY
Kj[2] = 0 # XZ
Kj[4] = 0 # YZ
to_replace -= {0, 1, 2, 4}
joint_axis = antRj[chainj[-1]].col(2)
if all_paral and robo.G.norm() == sympy.Abs(joint_axis.dot(robo.G)):
Kj[6] = 0 # MX
Kj[7] = 0 # MY
to_replace -= {6, 7}
if j == r1 or (j == r2 and orthog):
Kj[5] += robo.IA[j] # ZZ
robo.IA[j] = 0
for i in to_replace:
Kj[i] = symo.replace(Kj[i], inert_names[i], j)
robo.put_inert_param(Kj, j)
def group_param_rot_spec(robo, symo, j, lam, antRj, antPj):
"""Internal function. Groups inertia parameters according to the
special rule for a rotational joint.
Notes
=====
robo is the output paramete
"""
chainj = robo.chain(j)
r1, r2, orthog = Transform.find_r12(robo, chainj, antRj, j)
kRj, all_paral = Transform.kRj(robo, antRj, r1, chainj)
r1_Px_j, r1_Py_j, r1_Pz_j = Transform.kPj(
robo, antPj, antRj, r1, chainj
)
Kj = robo.get_inert_param(j)
to_replace = {0, 1, 2, 4, 5, 6, 7}
if Transform.z_paral(kRj):
Kj[0] = 0 # XX
Kj[1] = 0 # XY
Kj[2] = 0 # XZ
Kj[4] = 0 # YZ
to_replace -= {0, 1, 2, 4}
joint_axis = antRj[chainj[-1]].col(2)
if all_paral and \
(robo.G.norm() == sympy.Abs(joint_axis.dot(robo.G))) and \
(r1_Px_j == 0) and (r1_Py_j == 0):
Kj[6] = 0 # MX
Kj[7] = 0 # MY
to_replace -= {6, 7}
if j == r1 or(j == r2 and orthog):
Kj[5] += robo.IA[j] # ZZ
robo.IA[j] = 0
for i in to_replace:
Kj[i] = symo.replace(Kj[i], inert_names[i], j)
robo.put_inert_param(Kj, j)
def group_param_prism_spec(robo, symo, j, antRj, antPj):
"""Internal function. Groups inertia parameters according to the
special rule for a prismatic joint.
Notes
=====
robo is the output paramete
"""
chainj = robo.chain(j)
r1, r2, orthog = Transform.find_r12(robo, chainj, antRj, j)
Kj = robo.get_inert_param(j)
kRj, all_paral = Transform.kRj(robo, antRj, r1, chainj)
to_replace = {6, 7, 8, 9}
if r1 < j and j < r2:
if Transform.z_paral(kRj):
Kj[8] = 0 # MZ
for i in (6, 7):
Kj[i] = symo.replace(Kj[i], inert_names[i], j)
robo.MS[robo.ant[j]] += antRj[j] * Matrix([Kj[6], Kj[7], 0])
robo.JJ[2, 2] -= Kj[6] * antPj[j][0] + Kj[7] * antPj[j][1]
Kj[6] = 0 # MX
Kj[7] = 0 # MY
to_replace -= {6, 7, 8}
else:
jar1 = kRj.row(2)
if jar1[2] != 0:
Kj[6] -= jar1[0] / jar1[2] * Kj[8]
Kj[7] -= jar1[1] / jar1[2] * Kj[8]
Kj[8] = 0 # MZ
to_replace -= {8}
elif jar1[0] * jar1[1] != 0:
Kj[6] -= jar1[0] / jar1[1] * Kj[7]
Kj[7] = 0 # MY
to_replace -= {7}
elif jar1[0] != 0:
Kj[7] = 0 # MY
to_replace -= {7}
else:
Kj[6] = 0 # MX
to_replace -= {6}
elif j < r1:
Kj[6] = 0 # MX
Kj[7] = 0 # MY
Kj[8] = 0 # MZ
to_replace -= {6, 7, 8}
#TOD: rewrite
dotGa = Transform.sna(antRj[j])[2].dot(robo.G)
if dotGa == tools.ZERO:
revol_align = robo.ant[robo.ant[j]] == 0 and robo.ant[j] == tools.ZERO
if robo.ant[j] == 0 or revol_align:
Kj[9] += robo.IA[j]
robo.IA[j] = 0
for i in to_replace:
Kj[i] = symo.replace(Kj[i], inert_names[i], j)
robo.put_inert_param(Kj, j)
def group_param_prism_spec(robo, symo, j, antRj, antPj):
"""Internal function. Groups inertia parameters according to the
special rule for a prismatic joint.
Notes
=====
robo is the output paramete
"""
chainj = robo.chain(j)
r1, r2, orthog = Transform.find_r12(robo, chainj, antRj, j)
Kj = robo.get_inert_param(j)
kRj, all_paral = Transform.kRj(robo, antRj, r1, chainj)
to_replace = {6, 7, 8, 9}
if r1 < j and j < r2:
if Transform.z_paral(kRj):
Kj[8] = 0 # MZ
for i in (6, 7):
Kj[i] = symo.replace(Kj[i], inert_names[i], j)
robo.MS[robo.ant[j]] += antRj[j]*Matrix([Kj[6], Kj[7], 0])
robo.JJ[2, 2] -= Kj[6]*antPj[j][0] + Kj[7]*antPj[j][1]
Kj[6] = 0 # MX
Kj[7] = 0 # MY
to_replace -= {6, 7, 8}
else:
jar1 = kRj.row(2)
if jar1[2] != 0:
Kj[6] -= jar1[0]/jar1[2]*Kj[8]
Kj[7] -= jar1[1]/jar1[2]*Kj[8]
Kj[8] = 0 # MZ
to_replace -= {8}
elif jar1[0]*jar1[1] != 0:
Kj[6] -= jar1[0]/jar1[1]*Kj[7]
Kj[7] = 0 # MY
to_replace -= {7}
elif jar1[0] != 0:
Kj[7] = 0 # MY
to_replace -= {7}
else:
Kj[6] = 0 # MX
to_replace -= {6}
elif j < r1:
Kj[6] = 0 # MX
Kj[7] = 0 # MY
Kj[8] = 0 # MZ
to_replace -= {6, 7, 8}
#TOD: rewrite
dotGa = Transform.sna(antRj[j])[2].dot(robo.G)
if dotGa == tools.ZERO:
revol_align = robo.ant[robo.ant[j]] == 0 and robo.ant[j] == tools.ZERO
if robo.ant[j] == 0 or revol_align:
Kj[9] += robo.IA[j]
robo.IA[j] = 0
for i in to_replace:
Kj[i] = symo.replace(Kj[i], inert_names[i], j)
robo.put_inert_param(Kj, j)
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 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)
