def floating_inertia_matrix(robo, symo): """ Compute Inertia Matrix for robots with floating or mobile base. This function computes the A11, A12 and A22 matrices when the inertia matrix A = [A11, A12; A12.transpose(), A22] """ # init terms comp_inertia3, comp_ms, comp_mass = ParamsInit.init_jplus(robo) aje1 = ParamsInit.init_vec(robo) forces = ParamsInit.init_vec(robo, ext=1) moments = ParamsInit.init_vec(robo, ext=1) inertia_a12 = ParamsInit.init_vec(robo, num=6) inertia_a22 = sympy.zeros(robo.nl, robo.nl) # init transformation antRj, antPj = compute_rot_trans(robo, symo) for j in reversed(xrange(0, robo.NL)): replace_composite_terms( symo, j, comp_inertia3, comp_ms, comp_mass ) if j != 0: compute_composite_inertia( robo, symo, j, antRj, antPj, aje1, comp_inertia3, comp_ms, comp_mass ) for j in xrange(1, robo.NL): compute_diagonal_elements( robo, symo, j, comp_inertia3, comp_ms, comp_mass, forces, moments, inertia_a22 ) ka = j while ka != 0: k = ka ka = robo.ant[ka] compute_triangle_elements( robo, symo, j, k, ka, antRj, antPj, aje1, forces, moments, inertia_a12, inertia_a22 ) symo.mat_replace(inertia_a22, 'A', forced=True, symmet=True) inertia_a11 = inertia_spatial( comp_inertia3[0], comp_ms[0], comp_mass[0] ) inertia_a11 = symo.mat_replace( inertia_a11, 'Jcomp', 0, forced=True, symmet=True ) # setup inertia_a12 in Matrix form a12mat = sympy.zeros(6, robo.NL) for j in xrange(1, robo.NL): a12mat[:, j] = inertia_a12[j] a12mat = a12mat[:, 1:] # setup the complete inertia matrix inertia = Matrix([ inertia_a11.row_join(a12mat), a12mat.transpose().row_join(inertia_a22) ]) return inertia
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 = ParamsInit.init_jplus(robo) AJE1 = ParamsInit.init_vec(robo) f = ParamsInit.init_vec(robo, ext=1) n = ParamsInit.init_vec(robo, ext=1) A = sympy.zeros(robo.NL, robo.NL) symo = symbolmgr.SymbolManager() 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 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 = ParamsInit.init_jplus(robo) AJE1 = ParamsInit.init_vec(robo) f = ParamsInit.init_vec(robo, ext=1) n = ParamsInit.init_vec(robo, ext=1) A = sympy.zeros(robo.NL, robo.NL) symo = symbolmgr.SymbolManager() 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 fixed_inertia_matrix(robo, symo): """ Compute Inertia Matrix for robots with fixed base. This function computes just the A22 matrix when the inertia matrix A = [A11, A12; A12.transpose(), A22]. """ # init terms comp_inertia3, comp_ms, comp_mass = ParamsInit.init_jplus(robo) aje1 = ParamsInit.init_vec(robo) forces = ParamsInit.init_vec(robo, ext=1) moments = ParamsInit.init_vec(robo, ext=1) inertia_a12 = ParamsInit.init_vec(robo, num=6) inertia_a22 = sympy.zeros(robo.nl, robo.nl) # init transformation antRj, antPj = compute_rot_trans(robo, symo) for j in reversed(xrange(1, robo.NL)): replace_composite_terms( symo, j, comp_inertia3, comp_ms, comp_mass ) if j != 1: compute_composite_inertia( robo, symo, j, antRj, antPj, aje1, comp_inertia3, comp_ms, comp_mass ) for j in xrange(1, robo.NL): compute_diagonal_elements( robo, symo, j, comp_inertia3, comp_ms, comp_mass, forces, moments, inertia_a22 ) ka = j while ka != 1: k = ka ka = robo.ant[ka] compute_triangle_elements( robo, symo, j, k, ka, antRj, antPj, aje1, forces, moments, inertia_a12, inertia_a22 ) symo.mat_replace(inertia_a22, 'A', forced=True, symmet=True) return inertia_a22
def flexible_inverse_dynmodel(robo, symo): """ Compute the Inverse Dynamic Model using Newton-Euler algorithm for robots with flexible joints (fixed and floating base). Parameters: robo: Robot - instance of robot description container symo: symbolmgr.SymbolManager - instance of symbol manager """ # antecedent angular velocity, projected into jth frame # j^omega_i wi = ParamsInit.init_vec(robo) # j^omega_j w = ParamsInit.init_w(robo) # j^a_j -- joint axis in screw form jaj = ParamsInit.init_vec(robo, 6) # Twist transform list of Matrices 6x6 grandJ = ParamsInit.init_mat(robo, 6) jTant = ParamsInit.init_mat(robo, 6) gamma = ParamsInit.init_vec(robo, 6) beta = ParamsInit.init_vec(robo, 6) zeta = ParamsInit.init_vec(robo, 6) h_inv = ParamsInit.init_scalar(robo) jah = ParamsInit.init_vec(robo, 6) # Jj*aj*Hinv_j tau = ParamsInit.init_scalar(robo) star_inertia = ParamsInit.init_mat(robo, 6) star_beta = ParamsInit.init_vec(robo, 6) comp_inertia3, comp_ms, comp_mass = ParamsInit.init_jplus(robo) qddot = ParamsInit.init_scalar(robo) grandVp = ParamsInit.init_vec(robo, 6) react_wrench = ParamsInit.init_vec(robo, 6) torque = ParamsInit.init_scalar(robo) # flag variables use_composite = True # init transformation antRj, antPj = compute_rot_trans(robo, symo) # first forward recursion for j in xrange(1, robo.NL): # compute spatial inertia matrix for use in backward recursion grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) # set jaj vector 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]) # compute j^omega_j and j^omega_i compute_omega(robo, symo, j, antRj, w, wi) # compute j^S_i : screw transformation matrix compute_screw_transform(robo, symo, j, antRj, antPj, jTant) # compute j^gamma_j : gyroscopic acceleration (6x1) compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma) # compute j^beta_j : external+coriolis+centrifugal wrench (6x1) compute_beta(robo, symo, j, w, beta) if not robo.eta[j]: # when rigid # compute j^zeta_j : relative acceleration (6x1) compute_zeta(robo, symo, j, gamma, jaj, zeta) # decide first link first_link = 0 if robo.is_floating else 1 # first backward recursion - initialisation step for j in reversed(xrange(first_link, robo.NL)): if j == first_link and robo.is_floating: # compute spatial inertia matrix for base grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) # compute 0^beta_0 compute_beta(robo, symo, j, w, beta) replace_star_terms( symo, grandJ, beta, j, star_inertia, star_beta ) # second backward recursion - compute star terms for j in reversed(xrange(first_link, robo.NL)): replace_star_terms( symo, star_inertia, star_beta, j, star_inertia, star_beta ) if j == first_link: continue # set composite flag to false when flexible if robo.eta[j]: use_composite = False if use_composite: # use composite compute_composite_inertia( robo, symo, j, antRj, antPj, comp_inertia3, comp_ms, comp_mass, star_inertia ) compute_composite_beta( robo, symo, j, jTant, zeta, star_inertia, star_beta ) else: # use star if robo.eta[j]: compute_tau( robo, symo, j, jaj, star_beta, tau, flex=True ) compute_star_terms( robo, symo, j, jaj, jTant, gamma, tau, h_inv, jah, star_inertia, star_beta, flex=True ) # compute base acceleration : this returns the correct value for # fixed base and floating base robots compute_base_accel( robo, symo, star_inertia, star_beta, grandVp ) # second forward recursion for j in xrange(1, robo.NL): if robo.eta[j]: # when flexible # compute qddot_j : joint acceleration compute_joint_accel( robo, symo, j, jaj, jTant, h_inv, jah, gamma, tau, grandVp, star_beta, star_inertia, qddot ) # compute j^zeta_j : relative acceleration (6x1) compute_zeta(robo, symo, j, gamma, jaj, zeta, qddot) # compute j^Vdot_j : link acceleration compute_link_accel(robo, symo, j, jTant, zeta, grandVp) # compute j^F_j : reaction wrench compute_reaction_wrench( robo, symo, j, grandVp, star_inertia, star_beta, react_wrench ) if not robo.eta[j]: # when rigid compute torque compute_torque(robo, symo, j, jaj, react_wrench, torque)
def composite_inverse_dynmodel(robo, symo): """ Compute the Inverse Dynamic Model using Composite link Newton-Euler algorithm for tree structure robots with fixed and floating base. Parameters: robo: Robot - instance of robot description container symo: symbolmgr.SymbolManager - instance of symbol manager """ # antecedent angular velocity, projected into jth frame # j^omega_i wi = ParamsInit.init_vec(robo) # j^omega_j w = ParamsInit.init_w(robo) # j^a_j -- joint axis in screw form jaj = ParamsInit.init_vec(robo, 6) # Twist transform list of Matrices 6x6 grandJ = ParamsInit.init_mat(robo, 6) jTant = ParamsInit.init_mat(robo, 6) gamma = ParamsInit.init_vec(robo, 6) beta = ParamsInit.init_vec(robo, 6) zeta = ParamsInit.init_vec(robo, 6) composite_inertia = ParamsInit.init_mat(robo, 6) composite_beta = ParamsInit.init_vec(robo, 6) comp_inertia3, comp_ms, comp_mass = ParamsInit.init_jplus(robo) grandVp = ParamsInit.init_vec(robo, 6) react_wrench = ParamsInit.init_vec(robo, 6) torque = ParamsInit.init_scalar(robo) # init transformation antRj, antPj = compute_rot_trans(robo, symo) # first forward recursion for j in xrange(1, robo.NL): # compute spatial inertia matrix for use in backward recursion grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) # set jaj vector 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]) # compute j^omega_j and j^omega_i compute_omega(robo, symo, j, antRj, w, wi) # compute j^S_i : screw transformation matrix compute_screw_transform(robo, symo, j, antRj, antPj, jTant) # first forward recursion (still) for j in xrange(1, robo.NL): # compute j^gamma_j : gyroscopic acceleration (6x1) compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma) # compute j^beta_j : external+coriolis+centrifugal wrench (6x1) compute_beta(robo, symo, j, w, beta) # compute j^zeta_j : relative acceleration (6x1) compute_zeta(robo, symo, j, gamma, jaj, zeta) # first backward recursion - initialisation step for j in reversed(xrange(0, robo.NL)): if j == 0: # compute spatial inertia matrix for base grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) # compute 0^beta_0 compute_beta(robo, symo, j, w, beta) replace_composite_terms( symo, grandJ, beta, j, composite_inertia, composite_beta ) # second backward recursion - compute composite term for j in reversed(xrange(0, robo.NL)): replace_composite_terms( symo, composite_inertia, composite_beta, j, composite_inertia, composite_beta, replace=True ) if j == 0: continue compute_composite_inertia( robo, symo, j, antRj, antPj, comp_inertia3, comp_ms, comp_mass, composite_inertia ) compute_composite_beta( robo, symo, j, jTant, zeta, composite_inertia, composite_beta ) # compute base acceleration : this returns the correct value for # fixed base and floating base robots compute_base_accel_composite( robo, symo, composite_inertia, composite_beta, grandVp ) # second forward recursion for j in xrange(1, robo.NL): # compute j^Vdot_j : link acceleration compute_link_accel(robo, symo, j, jTant, zeta, grandVp) # compute j^F_j : reaction wrench compute_reaction_wrench( robo, symo, j, grandVp, composite_inertia, composite_beta, react_wrench ) # second forward recursion still - to make the output pretty for j in xrange(1, robo.NL): # compute torque compute_torque(robo, symo, j, jaj, react_wrench, torque)
def flexible_inverse_dynmodel(robo, symo): """ Compute the Inverse Dynamic Model using Newton-Euler algorithm for robots with flexible joints (fixed and floating base). Parameters: robo: Robot - instance of robot description container symo: symbolmgr.SymbolManager - instance of symbol manager """ # antecedent angular velocity, projected into jth frame # j^omega_i wi = ParamsInit.init_vec(robo) # j^omega_j w = ParamsInit.init_w(robo) # j^a_j -- joint axis in screw form jaj = ParamsInit.init_vec(robo, 6) # Twist transform list of Matrices 6x6 grandJ = ParamsInit.init_mat(robo, 6) jTant = ParamsInit.init_mat(robo, 6) gamma = ParamsInit.init_vec(robo, 6) beta = ParamsInit.init_vec(robo, 6) zeta = ParamsInit.init_vec(robo, 6) h_inv = ParamsInit.init_scalar(robo) jah = ParamsInit.init_vec(robo, 6) # Jj*aj*Hinv_j tau = ParamsInit.init_scalar(robo) star_inertia = ParamsInit.init_mat(robo, 6) star_beta = ParamsInit.init_vec(robo, 6) comp_inertia3, comp_ms, comp_mass = ParamsInit.init_jplus(robo) qddot = ParamsInit.init_scalar(robo) grandVp = ParamsInit.init_vec(robo, 6) react_wrench = ParamsInit.init_vec(robo, 6) torque = ParamsInit.init_scalar(robo) # flag variables use_composite = True # init transformation antRj, antPj = compute_rot_trans(robo, symo) # first forward recursion for j in xrange(1, robo.NL): # compute spatial inertia matrix for use in backward recursion grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) # set jaj vector 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]) # compute j^omega_j and j^omega_i compute_omega(robo, symo, j, antRj, w, wi) # compute j^S_i : screw transformation matrix compute_screw_transform(robo, symo, j, antRj, antPj, jTant) # compute j^gamma_j : gyroscopic acceleration (6x1) compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma) # compute j^beta_j : external+coriolis+centrifugal wrench (6x1) compute_beta(robo, symo, j, w, beta) if not robo.eta[j]: # when rigid # compute j^zeta_j : relative acceleration (6x1) compute_zeta(robo, symo, j, gamma, jaj, zeta) # decide first link first_link = 0 if robo.is_floating else 1 # first backward recursion - initialisation step for j in reversed(xrange(first_link, robo.NL)): if j == first_link and robo.is_floating: # compute spatial inertia matrix for base grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) # compute 0^beta_0 compute_beta(robo, symo, j, w, beta) replace_star_terms(symo, grandJ, beta, j, star_inertia, star_beta) # second backward recursion - compute star terms for j in reversed(xrange(first_link, robo.NL)): replace_star_terms(symo, star_inertia, star_beta, j, star_inertia, star_beta) if j == first_link: continue # set composite flag to false when flexible if robo.eta[j]: use_composite = False if use_composite: # use composite compute_composite_inertia(robo, symo, j, antRj, antPj, comp_inertia3, comp_ms, comp_mass, star_inertia) compute_composite_beta(robo, symo, j, jTant, zeta, star_inertia, star_beta) else: # use star if robo.eta[j]: compute_tau(robo, symo, j, jaj, star_beta, tau, flex=True) compute_star_terms(robo, symo, j, jaj, jTant, gamma, tau, h_inv, jah, star_inertia, star_beta, flex=True) # compute base acceleration : this returns the correct value for # fixed base and floating base robots compute_base_accel(robo, symo, star_inertia, star_beta, grandVp) # second forward recursion for j in xrange(1, robo.NL): if robo.eta[j]: # when flexible # compute qddot_j : joint acceleration compute_joint_accel(robo, symo, j, jaj, jTant, h_inv, jah, gamma, tau, grandVp, star_beta, star_inertia, qddot) # compute j^zeta_j : relative acceleration (6x1) compute_zeta(robo, symo, j, gamma, jaj, zeta, qddot) # compute j^Vdot_j : link acceleration compute_link_accel(robo, symo, j, jTant, zeta, grandVp) # compute j^F_j : reaction wrench compute_reaction_wrench(robo, symo, j, grandVp, star_inertia, star_beta, react_wrench) if not robo.eta[j]: # when rigid compute torque compute_torque(robo, symo, j, jaj, react_wrench, torque)
def composite_inverse_dynmodel(robo, symo): """ Compute the Inverse Dynamic Model using Composite link Newton-Euler algorithm for tree structure robots with fixed and floating base. Parameters: robo: Robot - instance of robot description container symo: symbolmgr.SymbolManager - instance of symbol manager """ # antecedent angular velocity, projected into jth frame # j^omega_i wi = ParamsInit.init_vec(robo) # j^omega_j w = ParamsInit.init_w(robo) # j^a_j -- joint axis in screw form jaj = ParamsInit.init_vec(robo, 6) # Twist transform list of Matrices 6x6 grandJ = ParamsInit.init_mat(robo, 6) jTant = ParamsInit.init_mat(robo, 6) gamma = ParamsInit.init_vec(robo, 6) beta = ParamsInit.init_vec(robo, 6) zeta = ParamsInit.init_vec(robo, 6) composite_inertia = ParamsInit.init_mat(robo, 6) composite_beta = ParamsInit.init_vec(robo, 6) comp_inertia3, comp_ms, comp_mass = ParamsInit.init_jplus(robo) grandVp = ParamsInit.init_vec(robo, 6) react_wrench = ParamsInit.init_vec(robo, 6) torque = ParamsInit.init_scalar(robo) # init transformation antRj, antPj = compute_rot_trans(robo, symo) # first forward recursion for j in xrange(1, robo.NL): # compute spatial inertia matrix for use in backward recursion grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) # set jaj vector 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]) # compute j^omega_j and j^omega_i compute_omega(robo, symo, j, antRj, w, wi) # compute j^S_i : screw transformation matrix compute_screw_transform(robo, symo, j, antRj, antPj, jTant) # first forward recursion (still) for j in xrange(1, robo.NL): # compute j^gamma_j : gyroscopic acceleration (6x1) compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma) # compute j^beta_j : external+coriolis+centrifugal wrench (6x1) compute_beta(robo, symo, j, w, beta) # compute j^zeta_j : relative acceleration (6x1) compute_zeta(robo, symo, j, gamma, jaj, zeta) # first backward recursion - initialisation step for j in reversed(xrange(0, robo.NL)): if j == 0: # compute spatial inertia matrix for base grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) # compute 0^beta_0 compute_beta(robo, symo, j, w, beta) replace_composite_terms(symo, grandJ, beta, j, composite_inertia, composite_beta) # second backward recursion - compute composite term for j in reversed(xrange(0, robo.NL)): replace_composite_terms(symo, composite_inertia, composite_beta, j, composite_inertia, composite_beta, replace=True) if j == 0: continue compute_composite_inertia(robo, symo, j, antRj, antPj, comp_inertia3, comp_ms, comp_mass, composite_inertia) compute_composite_beta(robo, symo, j, jTant, zeta, composite_inertia, composite_beta) # compute base acceleration : this returns the correct value for # fixed base and floating base robots compute_base_accel_composite(robo, symo, composite_inertia, composite_beta, grandVp) # second forward recursion for j in xrange(1, robo.NL): # compute j^Vdot_j : link acceleration compute_link_accel(robo, symo, j, jTant, zeta, grandVp) # compute j^F_j : reaction wrench compute_reaction_wrench(robo, symo, j, grandVp, composite_inertia, composite_beta, react_wrench) # second forward recursion still - to make the output pretty for j in xrange(1, robo.NL): # compute torque compute_torque(robo, symo, j, jaj, react_wrench, torque)