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 : symbolmgr.SymbolManager Instance of symbolic manager """ #init velocities and accelerations w = ParamsInit.init_w(robo) wdot, vdot = ParamsInit.init_wv_dot(robo, gravity) #init auxilary matrix U = ParamsInit.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, w[j] = _omega_ij(robo, j, jRant, w, qdj) symo.mat_replace(w[j], 'W', j) symo.mat_replace(wi, 'WI', 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 compute_vel_acc(robo, symo, antRj, antPj, forced=False, gravity=True, floating=False): """Internal function. Computes speeds and accelerations usitn Parameters ========== robo : Robot Instance of robot description container symo : symbolmgr.SymbolManager Instance of symbolic manager """ #init velocities and accelerations w = ParamsInit.init_w(robo) wdot, vdot = ParamsInit.init_wv_dot(robo, gravity) # decide first link first_link = 1 if floating or robo.is_floating or robo.is_mobile: first_link = 0 #init auxilary matrix U = ParamsInit.init_u(robo) for j in xrange(first_link, robo.NL): if j == 0: w[j] = symo.mat_replace(w[j], 'W', j) wdot[j] = symo.mat_replace(wdot[j], 'WP', j) vdot[j] = symo.mat_replace(vdot[j], 'VP', j) dv0 = ParamsInit.product_combinations(w[j]) symo.mat_replace(dv0, 'DV', j) hatw_hatw = Matrix([[-dv0[3] - dv0[5], dv0[1], dv0[2]], [dv0[1], -dv0[5] - dv0[0], dv0[4]], [dv0[2], dv0[4], -dv0[3] - dv0[0]]]) U[j] = hatw_hatw + tools.skew(wdot[j]) symo.mat_replace(U[j], 'U', j) else: jRant = antRj[j].T qdj = Z_AXIS * robo.qdot[j] qddj = Z_AXIS * robo.qddot[j] wi, w[j] = _omega_ij(robo, j, jRant, w, qdj) symo.mat_replace(w[j], 'W', j) symo.mat_replace(wi, 'WI', 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 compute_vel_acc( robo, symo, antRj, antPj, forced=False, gravity=True, floating=False ): """Internal function. Computes speeds and accelerations usitn Parameters ========== robo : Robot Instance of robot description container symo : symbolmgr.SymbolManager Instance of symbolic manager """ #init velocities and accelerations w = ParamsInit.init_w(robo) wdot, vdot = ParamsInit.init_wv_dot(robo, gravity) # decide first link first_link = 1 if floating or robo.is_floating or robo.is_mobile: first_link = 0 #init auxilary matrix U = ParamsInit.init_u(robo) for j in xrange(first_link, robo.NL): if j == 0: w[j] = symo.mat_replace(w[j], 'W', j) wdot[j] = symo.mat_replace(wdot[j], 'WP', j) vdot[j] = symo.mat_replace(vdot[j], 'VP', j) dv0 = ParamsInit.product_combinations(w[j]) symo.mat_replace(dv0, 'DV', j) hatw_hatw = Matrix([ [-dv0[3]-dv0[5], dv0[1], dv0[2]], [dv0[1], -dv0[5]-dv0[0], dv0[4]], [dv0[2], dv0[4], -dv0[3]-dv0[0]] ]) U[j] = hatw_hatw + tools.skew(wdot[j]) symo.mat_replace(U[j], 'U', j) else: jRant = antRj[j].T qdj = Z_AXIS * robo.qdot[j] qddj = Z_AXIS * robo.qddot[j] wi, w[j] = _omega_ij(robo, j, jRant, w, qdj) symo.mat_replace(w[j], 'W', j) symo.mat_replace(wi, 'WI', 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 jdot_qdot(robo): symo = symbolmgr.SymbolManager(None) symo.file_open(robo, 'jpqp') symo.write_params_table(robo, 'JdotQdot') antRj, antPj = compute_rot_trans(robo, symo) w = ParamsInit.init_w(robo) wdot, vdot = ParamsInit.init_wv_dot(robo, gravity=False) U = ParamsInit.init_u(robo) for j in xrange(1, robo.NL): jRant = antRj[j].T qdj = Z_AXIS * robo.qdot[j] qddj = Z_AXIS * tools.ZERO wi, w[j] = _omega_ij(robo, j, jRant, w, qdj) symo.mat_replace(w[j], 'W', j) symo.mat_replace(wi, 'WI', 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
def jdot_qdot(robo): """ Similar to compute_vel_acc. """ symo = symbolmgr.SymbolManager(None) symo.file_open(robo, 'jpqp') symo.write_params_table(robo, 'JdotQdot') antRj, antPj = compute_rot_trans(robo, symo) w = ParamsInit.init_w(robo) wdot, vdot = ParamsInit.init_wv_dot(robo, gravity=False) U = ParamsInit.init_u(robo) for j in xrange(1, robo.NL): jRant = antRj[j].T qdj = Z_AXIS * robo.qdot[j] qddj = Z_AXIS * tools.ZERO wi, w[j] = _omega_ij(robo, j, jRant, w, qdj) symo.mat_replace(w[j], 'W', j) symo.mat_replace(wi, 'WI', 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