def compute_vel_acc(robo, symo, antRj, antPj, forced=False, gravity=True): """Internal function. Computes speeds and accelerations usitn Parameters ========== robo : Robot Instance of robot description container symo : Symoro Instance of symbolic manager """ #init velocities and accelerations w = Init.init_w(robo) wdot, vdot = Init.init_wv_dot(robo, gravity) #init auxilary matrix U = Init.init_U(robo) for j in xrange(1, robo.NL): jRant = antRj[j].T qdj = Z_AXIS * robo.qdot[j] qddj = Z_AXIS * robo.qddot[j] wi = _omega_i(robo, symo, j, jRant, w) symo.mat_replace(wi, 'WI', j) w[j] = _omega_j(robo, j, jRant, w, wi, qdj) symo.mat_replace(w[j], 'W', j) _omega_dot_j(robo, j, jRant, w, wi, wdot, qdj, qddj) symo.mat_replace(wdot[j], 'WP', j, forced) _v_dot_j(robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj) symo.mat_replace(vdot[j], 'VP', j, forced) return w, wdot, vdot, U
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 velocities(robo): symo = Symoro(None) symo.file_open(robo, 'vel') symo.write_params_table(robo, 'Link velocities') antRj, antPj = compute_rot_trans(robo, symo) w = Init.init_w(robo) v = Init.init_v(robo) for j in xrange(1, robo.NL): jRant = antRj[j].T qdj = Z_AXIS * robo.qdot[j] wi = _omega_i(robo, symo, j, jRant, w) w[j] = _omega_j(robo, j, jRant, w, wi, qdj) symo.mat_replace(w[j], 'W', j, forced=True) _v_j(robo, j, antPj, jRant, v, w, qdj) symo.mat_replace(v[j], 'V', j, forced=True) symo.file_close() return symo
def jdot_qdot(robo): symo = Symoro(None) symo.file_open(robo, 'jpqp') symo.write_params_table(robo, 'JdotQdot') antRj, antPj = compute_rot_trans(robo, symo) w = Init.init_w(robo) wdot, vdot = Init.init_wv_dot(robo, gravity=False) U = Init.init_U(robo) for j in xrange(1, robo.NL): jRant = antRj[j].T qdj = Z_AXIS * robo.qdot[j] qddj = Z_AXIS * ZERO wi = _omega_i(robo, symo, j, jRant, w) symo.mat_replace(wi, 'WI', j) w[j] = _omega_j(robo, j, jRant, w, wi, qdj) symo.mat_replace(w[j], 'W', j) _omega_dot_j(robo, j, jRant, w, wi, wdot, qdj, qddj) symo.mat_replace(wdot[j], 'WPJ', j, forced=True) _v_dot_j(robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj) symo.mat_replace(vdot[j], 'VPJ', j, forced=True) symo.file_close() return symo