def Newton_Euler(robo, symo): """Internal function. Computes Inverse Dynamic Model using Newton-Euler formulation Parameters ========== robo : Robot Instance of robot description container symo : Symoro Instance of symbolic manager """ # init external forces Fex = copy(robo.Fex) Nex = copy(robo.Nex) # init transformation antRj, antPj = compute_rot_trans(robo, symo) # init velocities and accelerations w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj) # init forces vectors F = Init.init_vec(robo) N = Init.init_vec(robo) Fjnt = Init.init_vec(robo) Njnt = Init.init_vec(robo) for j in xrange(1, robo.NL): compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N) for j in reversed(xrange(1, robo.NL)): compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, Fjnt, Njnt, F, N, Fex, Nex) for j in xrange(1, robo.NL): compute_torque(robo, symo, j, Fjnt, Njnt)
def compute_rot_trans(robo, symo): #init transformation antRj = Init.init_mat(robo) antPj = Init.init_vec(robo) for j in xrange(robo.NL): compute_transform(robo, symo, j, antRj, antPj) return antRj, antPj
def inertia_matrix(robo): """Computes Inertia Matrix using composed link Parameters ========== robo : Robot Instance of robot description container Returns ======= symo.sydi : dictionary Dictionary with the information of all the sybstitution """ Jplus, MSplus, Mplus = Init.init_Jplus(robo) AJE1 = Init.init_vec(robo) f = Init.init_vec(robo, ext=1) n = Init.init_vec(robo, ext=1) A = sympy.zeros(robo.NL, robo.NL) symo = Symoro() symo.file_open(robo, 'inm') title = 'Inertia Matrix using composite links' symo.write_params_table(robo, title, inert=True, dynam=True) # init transformation antRj, antPj = compute_rot_trans(robo, symo) for j in reversed(xrange(-1, robo.NL)): replace_Jplus(robo, symo, j, Jplus, MSplus, Mplus) if j != - 1: compute_Jplus(robo, symo, j, antRj, antPj, Jplus, MSplus, Mplus, AJE1) for j in xrange(1, robo.NL): compute_A_diagonal(robo, symo, j, Jplus, MSplus, Mplus, f, n, A) ka = j while ka != - 1: k = ka ka = robo.ant[ka] compute_A_triangle(robo, symo, j, k, ka, antRj, antPj, f, n, A, AJE1) symo.mat_replace(A, 'A', forced=True, symmet=True) J_base = inertia_spatial(Jplus[-1], MSplus[-1], Mplus[-1]) symo.mat_replace(J_base, 'JP', 0, forced=True, symmet=True) symo.file_close() return symo
def direct_dynamic_NE(robo): """Computes Direct Dynamic Model using Newton-Euler formulation Parameters ========== robo : Robot Instance of robot description container Returns ======= symo.sydi : dictionary Dictionary with the information of all the sybstitution """ wi = Init.init_vec(robo) # antecedent angular velocity, projected into jth frame w = Init.init_w(robo) jaj = Init.init_vec(robo, 6) jTant = Init.init_mat(robo, 6) # Twist transform list of Matrices 6x6 beta_star = Init.init_vec(robo, 6) grandJ = Init.init_mat(robo, 6) link_acc = Init.init_vec(robo, 6) H_inv = Init.init_scalar(robo) juj = Init.init_vec(robo, 6) # Jj*aj / Hj Tau = Init.init_scalar(robo) grandVp = Init.init_vec(robo, 6) grandVp.append(Matrix([robo.vdot0 - robo.G, robo.w0])) symo = Symoro() symo.file_open(robo, 'ddm') title = 'Direct dynamic model using Newton - Euler Algorith' symo.write_params_table(robo, title, inert=True, dynam=True) # init transformation antRj, antPj = compute_rot_trans(robo, symo) for j in xrange(1, robo.NL): compute_omega(robo, symo, j, antRj, w, wi) compute_screw_transform(robo, symo, j, antRj, antPj, jTant) if robo.sigma[j] == 0: jaj[j] = Matrix([0, 0, 0, 0, 0, 1]) elif robo.sigma[j] == 1: jaj[j] = Matrix([0, 0, 1, 0, 0, 0]) for j in xrange(1, robo.NL): compute_beta(robo, symo, j, w, beta_star) compute_link_acc(robo, symo, j, antRj, antPj, link_acc, w, wi) grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) for j in reversed(xrange(1, robo.NL)): replace_beta_J_star(robo, symo, j, grandJ, beta_star) compute_Tau(robo, symo, j, grandJ, beta_star, jaj, juj, H_inv, Tau) if robo.ant[j] != - 1: compute_beta_J_star(robo, symo, j, grandJ, jaj, juj, Tau, beta_star, jTant, link_acc) for j in xrange(1, robo.NL): compute_acceleration(robo, symo, j, jTant, grandVp, juj, H_inv, jaj, Tau, link_acc) for j in xrange(1, robo.NL): compute_coupled_forces(robo, symo, j, grandVp, grandJ, beta_star) symo.file_close() return symo
def dynamic_identification_NE(robo): """Computes Dynamic Identification Model using Newton-Euler formulation Parameters ========== robo : Robot Instance of robot description container Returns ======= symo.sydi : dictionary Dictionary with the information of all the sybstitution """ # init forces vectors Fjnt = Init.init_vec(robo) Njnt = Init.init_vec(robo) # init file output, writing the robot description symo = Symoro() symo.file_open(robo, 'dim') title = "Dynamic identification model using Newton - Euler Algorith" symo.write_params_table(robo, title, inert=True, dynam=True) # init transformation antRj, antPj = compute_rot_trans(robo, symo) # init velocities and accelerations w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj) # virtual robot with only one non-zero parameter at once robo_tmp = deepcopy(robo) robo_tmp.IA = sympy.zeros(robo.NL, 1) robo_tmp.FV = sympy.zeros(robo.NL, 1) robo_tmp.FS = sympy.zeros(robo.NL, 1) for k in xrange(1, robo.NL): param_vec = robo.get_inert_param(k) F = Init.init_vec(robo) N = Init.init_vec(robo) for i in xrange(10): if param_vec[i] == ZERO: continue # change link names according to current non-zero parameter robo_tmp.num = [str(l) + str(param_vec[i]) for l in xrange(k + 1)] # set the parameter to 1 mask = sympy.zeros(10, 1) mask[i] = 1 robo_tmp.put_inert_param(mask, k) # compute the total forcec of the link k compute_wrench(robo_tmp, symo, k, w, wdot, U, vdot, F, N) # init external forces Fex = copy(robo.Fex) Nex = copy(robo.Nex) for j in reversed(xrange(k + 1)): compute_joint_wrench(robo_tmp, symo, j, antRj, antPj, vdot, Fjnt, Njnt, F, N, Fex, Nex) for j in xrange(k + 1): compute_torque(robo_tmp, symo, j, Fjnt, Njnt, 'DG') # reset all the parameters to zero robo_tmp.put_inert_param(sympy.zeros(10, 1), k) # compute model for the joint parameters compute_joint_torque_deriv(symo, robo.IA[k], robo.qddot[k], k) compute_joint_torque_deriv(symo, robo.FS[k], sympy.sign(robo.qdot[k]), k) compute_joint_torque_deriv(symo, robo.FV[k], robo.qdot[k], k) # closing the output file symo.file_close() return symo