Пример #1
0
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
Пример #2
0
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
Пример #3
0
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
Пример #4
0
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