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)