def vec_mut_M(P):
"""Internal function. Needed for inertia parameters transformation
Parameters
==========
P : Matrix 3x1
position vector
Returns : Matrix 6x1
"""
U = -hat(P)*hat(P)
return Matrix([U[0, 0], U[0, 1], U[0, 2], U[1, 1], U[1, 2], U[2, 2]])
def _v_dot_j(robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj):
DV = Init.product_combinations(w[j])
symo.mat_replace(DV, 'DV', j)
hatw_hatw = Matrix([[-DV[3]-DV[5], DV[1], DV[2]],
[DV[1], -DV[5]-DV[0], DV[4]],
[DV[2], DV[4], -DV[3]-DV[0]]])
U[j] = hatw_hatw + hat(wdot[j])
symo.mat_replace(U[j], 'U', j)
vsp = vdot[robo.ant[j]] + U[robo.ant[j]]*antPj[j]
symo.mat_replace(vsp, 'VSP', j)
vdot[j] = jRant*vsp
if robo.sigma[j] == 1: # prismatic joint
vdot[j] += qddj + 2*hat(wi)*qdj
return vdot[j]
def vec_mut_MS(v, P):
"""Internal function. Needed for inertia parameters transformation
Parameters
==========
v : Matrix 3x1
axis vector
P : Matrix 3x1
position vector
Returns : Matrix 6x1
"""
U = - hat(v)*hat(P)
return Matrix([2*U[0, 0], U[0, 1] + U[1, 0], U[0, 2] + U[2, 0],
2*U[1, 1], U[1, 2] + U[2, 1], 2*U[2, 2]])
def compute_beta(robo, symo, j, w, beta_star):
"""Internal function. Computes link's wrench when
the joint accelerations are zero
Notes
=====
beta_star is the output parameter
"""
E1 = symo.mat_replace(robo.J[j]*w[j], 'JW', j)
E2 = symo.mat_replace(hat(w[j])*E1, 'KW', j)
E3 = hat(w[j])*robo.MS[j]
E4 = symo.mat_replace(hat(w[j])*E3, 'SW', j)
E5 = -robo.Nex[j] - E2
E6 = -robo.Fex[j] - E4
beta_star[j] = Matrix([E6, E5])
def compute_Jplus(robo, symo, j, antRj, antPj, Jplus, MSplus, Mplus, AJE1):
"""Internal function. Computes inertia parameters of composed link
Notes
=====
Jplus, MSplus, Mplus are the output parameters
"""
hat_antPj = hat(antPj[j])
antMSj = symo.mat_replace(antRj[j]*MSplus[j], 'AS', j)
E1 = symo.mat_replace(antRj[j]*Jplus[j], 'AJ', j)
AJE1[j] = E1[:, 2]
E2 = symo.mat_replace(E1*antRj[j].T, 'AJA', j)
E3 = symo.mat_replace(hat_antPj*hat(antMSj), 'PAS', j)
Jplus[robo.ant[j]] += E2 - (E3 + E3.T) + hat_antPj*hat_antPj.T*Mplus[j]
MSplus[robo.ant[j]] += antMSj + antPj[j]*Mplus[j]
Mplus[robo.ant[j]] += Mplus[j]
def compute_joint_wrench(robo, symo, j, antRj, antPj, vdot,
Fjnt, Njnt, F, N, Fex, Nex):
"""Internal function. Computes wrench (torques and forces)
of the joint j
Notes
=====
Fjnt, Njnt, Fex, Nex are the output parameters
"""
Fjnt[j] = symo.mat_replace(F[j] + Fex[j], 'E', j)
Njnt[j] = N[j] + Nex[j] + hat(robo.MS[j])*vdot[j]
symo.mat_replace(Njnt[j], 'N', j)
f_ant = symo.mat_replace(antRj[j]*Fjnt[j], 'FDI', j)
if robo.ant[j] != - 1:
Fex[robo.ant[j]] += f_ant
Nex[robo.ant[j]] += antRj[j]*Njnt[j] + hat(antPj[j])*f_ant
def compute_link_acc(robo, symo, j, antRj, antPj, link_acc, w, wi):
"""Internal function. Computes link's accelerations when
the joint accelerations are zero
Notes
=====
link_acc is the output parameter
"""
E1 = symo.mat_replace(hat(wi[j])*Matrix([0, 0, robo.qdot[j]]),
'WQ', j)
E2 = (1 - robo.sigma[j])*E1
E3 = 2*robo.sigma[j]*E1
E4 = hat(w[robo.ant[j]])*antPj[j]
E5 = hat(w[robo.ant[j]])*E4
E6 = antRj[j].T*E5
E7 = symo.mat_replace(E6 + E3, 'LW', j)
link_acc[j] = Matrix([E7, E2])
def compute_screw_transform(robo, symo, j, antRj, antPj, jTant):
"""Internal function. Computes the screw transformation matrix
between ant[j] and j frames.
Notes
=====
jTant is an output parameter
"""
jRant = antRj[j].T
ET = symo.mat_replace(-jRant*hat(antPj[j]), 'JPR', j)
jTant[j] = (Matrix([jRant.row_join(ET),
zeros(3, 3).row_join(jRant)]))
def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N):
"""Internal function. Computes total wrench (torques and forces)
of the link j
Notes
=====
F, N are the output parameters
"""
F[j] = robo.M[j]*vdot[j] + U[j]*robo.MS[j]
symo.mat_replace(F[j], 'F', j)
Psi = symo.mat_replace(robo.J[j]*w[j], 'PSI', j)
N[j] = robo.J[j]*wdot[j] + hat(w[j])*Psi
symo.mat_replace(N[j], 'No', j)
def compute_A_triangle(robo, symo, j, k, ka, antRj, antPj, f, n, A, AJE1):
"""Internal function. Computes elements below and above diagonal
of the inertia matrix
Notes
=====
f, n, A are the output parameters
"""
f[ka] = antRj[k]*f[k]
if k == j and robo.sigma[j] == 0:
n[ka] = AJE1[k] + hat(antPj[k])*f[k]
else:
n[ka] = antRj[k]*n[k] + hat(antPj[k])*f[k]
if ka == - 1:
symo.mat_replace(f[ka], 'AV0')
symo.mat_replace(n[ka], 'AW0')
else:
symo.mat_replace(f[ka], 'E' + chars[j], ka)
symo.mat_replace(n[ka], 'N' + chars[j], ka)
if robo.sigma[ka] == 0:
A[j, ka] = n[ka][2]
elif robo.sigma[ka] == 1:
A[j, ka] = f[ka][2]
A[ka, j] = A[j, ka]
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 inertia_spatial(J, MS, M):
return Matrix([(M*sympy.eye(3)).row_join(hat(MS).T), hat(MS).row_join(J)])
def _v_j(robo, j, antPj, jRant, v, w, qdj, forced=False):
ant = robo.ant[j]
v[j] = jRant*(hat(w[ant])*antPj[j] + v[ant])
if robo.sigma[j] == 1: # prismatic joint
v[j] += qdj
return v[j]
def _omega_dot_j(robo, j, jRant, w, wi, wdot, qdj, qddj):
wdot[j] = jRant*wdot[robo.ant[j]]
if robo.sigma[j] == 0: # revolute joint
wdot[j] += (qddj + hat(wi)*qdj)
return wdot[j]
