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_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 == ZERO:
revol_align = robo.ant[robo.ant[j]] == 0 and robo.ant[j] == 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)
