コード例 #1
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
コード例 #2
0
ファイル: dynamics.py プロジェクト: BKhomutenko/SYMORO_python
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
コード例 #3
0
def kinematic_constraints(robo):
    symo = Symoro(None)
    res = _kinematic_loop_constraints(robo, symo)
    if res == FAIL:
        return FAIL
    W_a, W_p, W_ac, W_pc, W_c = res
    symo.file_open(robo, 'ckel')
    symo.write_params_table(robo, 'Constraint kinematic equations of loop',
                            equations=False)
    symo.write_line('Active joint variables')
    symo.write_line([robo.get_q(i) for i in robo.indx_active])
    symo.write_line()
    symo.write_line('Passive joints variables')
    symo.write_line([robo.get_q(i) for i in robo.indx_passive])
    symo.write_line()
    symo.write_line('Cut joints variables')
    symo.write_line([robo.get_q(i) for i in robo.indx_cut])
    symo.write_line()
    symo.mat_replace(W_a, 'WA', forced=True)
    symo.mat_replace(W_p, 'WP', forced=True)
    symo.mat_replace(W_ac, 'WPA', forced=True)
    symo.mat_replace(W_pc, 'WPC', forced=True)
    symo.mat_replace(W_c, 'WC', 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