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]