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)