Beispiel #1
0
def compute_rot_trans(robo, symo):
    #init transformation
    antRj = ParamsInit.init_mat(robo)
    antPj = ParamsInit.init_vec(robo)
    for j in xrange(robo.NL):
        compute_transform(robo, symo, j, antRj, antPj)
    return antRj, antPj
Beispiel #2
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 : 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
Beispiel #3
0
def mobile_inverse_dynmodel(robo, symo):
    """
    Compute the Inverse Dynamic Model using Newton-Euler algorithm for
    mobile robots.

    Parameters:
        robo: Robot - instance of robot description container
        symo: symbolmgr.SymbolManager - instance of symbol manager
    """
    # init external forces
    Fex = copy(robo.Fex)
    Nex = copy(robo.Nex)
    # init transformation
    antRj, antPj = compute_rot_trans(robo, symo)
    # init velocities and accelerations
    w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj)
    # init forces vectors
    F = ParamsInit.init_vec(robo)
    N = ParamsInit.init_vec(robo)
    Fjnt = ParamsInit.init_vec(robo)
    Njnt = ParamsInit.init_vec(robo)
    # init torque list
    torque = ParamsInit.init_scalar(robo)
    for j in xrange(0, robo.NL):
        compute_dynamic_wrench(robo, symo, j, w, wdot, U, vdot, F, N)
    for j in reversed(xrange(0, robo.NL)):
        compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, F, N, Fjnt,
                             Njnt, Fex, Nex)
    for j in xrange(1, robo.NL):
        compute_joint_torque(robo, symo, j, Fjnt, Njnt, torque)
Beispiel #4
0
def Newton_Euler(robo, symo):
    """Internal function. Computes Inverse Dynamic Model using
    Newton-Euler formulation

    Parameters
    ==========
    robo : Robot
        Instance of robot description container
    symo : symbolmgr.SymbolManager
        Instance of symbolic manager
    """
    # init external forces
    Fex = copy(robo.Fex)
    Nex = copy(robo.Nex)
    # init transformation
    antRj, antPj = compute_rot_trans(robo, symo)
    # init velocities and accelerations
    w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj)
    # init forces vectors
    F = ParamsInit.init_vec(robo)
    N = ParamsInit.init_vec(robo)
    Fjnt = ParamsInit.init_vec(robo)
    Njnt = ParamsInit.init_vec(robo)
    for j in xrange(1, robo.NL):
        compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N)
    for j in reversed(xrange(1, robo.NL)):
        compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, Fjnt, Njnt, F,
                             N, Fex, Nex)
    for j in xrange(1, robo.NL):
        compute_torque(robo, symo, j, Fjnt, Njnt)
Beispiel #5
0
def mobile_inverse_dynmodel(robo, symo):
    """
    Compute the Inverse Dynamic Model using Newton-Euler algorithm for
    mobile robots.

    Parameters:
        robo: Robot - instance of robot description container
        symo: symbolmgr.SymbolManager - instance of symbol manager
    """
    # init external forces
    Fex = copy(robo.Fex)
    Nex = copy(robo.Nex)
    # init transformation
    antRj, antPj = compute_rot_trans(robo, symo)
    # init velocities and accelerations
    w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj)
    # init forces vectors
    F = ParamsInit.init_vec(robo)
    N = ParamsInit.init_vec(robo)
    Fjnt = ParamsInit.init_vec(robo)
    Njnt = ParamsInit.init_vec(robo)
    # init torque list
    torque = ParamsInit.init_scalar(robo)
    for j in xrange(0, robo.NL):
        compute_dynamic_wrench(robo, symo, j, w, wdot, U, vdot, F, N)
    for j in reversed(xrange(0, robo.NL)):
        compute_joint_wrench(
            robo, symo, j, antRj, antPj, vdot,
            F, N, Fjnt, Njnt, Fex, Nex
        )
    for j in xrange(1, robo.NL):
        compute_joint_torque(robo, symo, j, Fjnt, Njnt, torque)
Beispiel #6
0
def Newton_Euler(robo, symo):
    """Internal function. Computes Inverse Dynamic Model using
    Newton-Euler formulation

    Parameters
    ==========
    robo : Robot
        Instance of robot description container
    symo : symbolmgr.SymbolManager
        Instance of symbolic manager
    """
    # init external forces
    Fex = copy(robo.Fex)
    Nex = copy(robo.Nex)
    # init transformation
    antRj, antPj = compute_rot_trans(robo, symo)
    # init velocities and accelerations
    w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj)
    # init forces vectors
    F = ParamsInit.init_vec(robo)
    N = ParamsInit.init_vec(robo)
    Fjnt = ParamsInit.init_vec(robo)
    Njnt = ParamsInit.init_vec(robo)
    for j in xrange(1, robo.NL):
        compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N)
    for j in reversed(xrange(1, robo.NL)):
        compute_joint_wrench(robo, symo, j, antRj, antPj, vdot,
                             Fjnt, Njnt, F, N, Fex, Nex)
    for j in xrange(1, robo.NL):
        compute_torque(robo, symo, j, Fjnt, Njnt)
Beispiel #7
0
def compute_rot_trans(robo, symo):
    #init transformation
    antRj = ParamsInit.init_mat(robo)
    antPj = ParamsInit.init_vec(robo)
    for j in xrange(robo.NL):
        compute_transform(robo, symo, j, antRj, antPj)
    return antRj, antPj
Beispiel #8
0
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
Beispiel #9
0
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
Beispiel #10
0
def velocities(robo):
    symo = symbolmgr.SymbolManager(None)
    symo.file_open(robo, 'vel')
    symo.write_params_table(robo, 'Link velocities')
    antRj, antPj = compute_rot_trans(robo, symo)
    w = ParamsInit.init_w(robo)
    v = ParamsInit.init_v(robo)
    for j in xrange(1, robo.NL):
        jRant = antRj[j].T
        qdj = Z_AXIS * robo.qdot[j]
        _omega_ij(robo, j, jRant, w, 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
Beispiel #11
0
def velocities(robo):
    symo = symbolmgr.SymbolManager(None)
    symo.file_open(robo, 'vel')
    symo.write_params_table(robo, 'Link velocities')
    antRj, antPj = compute_rot_trans(robo, symo)
    w = ParamsInit.init_w(robo)
    v = ParamsInit.init_v(robo)
    for j in xrange(1, robo.NL):
        jRant = antRj[j].T
        qdj = Z_AXIS * robo.qdot[j]
        _omega_ij(robo, j, jRant, w, 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
Beispiel #12
0
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
Beispiel #13
0
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
Beispiel #14
0
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
Beispiel #15
0
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
Beispiel #16
0
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
Beispiel #17
0
def _v_dot_j(robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj):
    DV = ParamsInit.product_combinations(w[j])
    symo.mat_replace(DV, 'DV', j)
    hatw_hatw = Matrix([[-DV[3] - DV[5], DV[1], DV[2]],
                        [DV[1], -DV[5] - DV[0], DV[4]],
                        [DV[2], DV[4], -DV[3] - DV[0]]])
    U[j] = hatw_hatw + tools.skew(wdot[j])
    symo.mat_replace(U[j], 'U', j)
    vsp = vdot[robo.ant[j]] + U[robo.ant[j]] * antPj[j]
    symo.mat_replace(vsp, 'VSP', j)
    vdot[j] = jRant * vsp
    if robo.sigma[j] == 1:  # prismatic joint
        vdot[j] += qddj + 2 * tools.skew(wi) * qdj
    return vdot[j]
Beispiel #18
0
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
Beispiel #19
0
def _v_dot_j(
    robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj
):
    DV = ParamsInit.product_combinations(w[j])
    symo.mat_replace(DV, 'DV', j)
    hatw_hatw = Matrix([[-DV[3]-DV[5], DV[1], DV[2]],
                        [DV[1], -DV[5]-DV[0], DV[4]],
                        [DV[2], DV[4], -DV[3]-DV[0]]])
    U[j] = hatw_hatw + tools.skew(wdot[j])
    symo.mat_replace(U[j], 'U', j)
    vsp = vdot[robo.ant[j]] + U[robo.ant[j]]*antPj[j]
    symo.mat_replace(vsp, 'VSP', j)
    vdot[j] = jRant*vsp
    if robo.sigma[j] == 1:    # prismatic joint
        vdot[j] += qddj + 2*tools.skew(wi)*qdj
    return vdot[j]
Beispiel #20
0
def dynamic_identification_model(robo, symo):
    """
    Compute the Dynamic Identification model of a robot using
    Newton-Euler algorithm.
    """
    # init forces vectors
    Fjnt = ParamsInit.init_vec(robo)
    Njnt = ParamsInit.init_vec(robo)
    # init transformation
    antRj, antPj = compute_rot_trans(robo, symo)
    # init velocities and accelerations
    w, wdot, vdot, U = compute_vel_acc(
        robo, symo, antRj, antPj, floating=True
    )
    # virtual robot with only one non-zero parameter at once
    robo_tmp = copy.deepcopy(robo)
    robo_tmp.IA = sympy.zeros(robo.NL, 1)
    robo_tmp.FV = sympy.zeros(robo.NL, 1)
    robo_tmp.FS = sympy.zeros(robo.NL, 1)
    # start link number
    is_fixed = False if robo.is_floating or robo.is_mobile else True
    start_link = 0
    for k in xrange(start_link, robo.NL):
        param_vec = robo.get_inert_param(k)
        F = ParamsInit.init_vec(robo)
        N = ParamsInit.init_vec(robo)
        for i in xrange(10):
            if param_vec[i] == tools.ZERO:
                continue
            # change link names according to current non-zero parameter
            name = '{index}{element}' + str(param_vec[i])
            # set the parameter to 1
            mask = sympy.zeros(10, 1)
            mask[i] = 1
            robo_tmp.put_inert_param(mask, k)
            # compute the total forcec of the link k
            _compute_dynamic_wrench(
                robo_tmp, symo, name, k, w, wdot, U, vdot, F, N
            )
            # init external forces
            Fex = ParamsInit.init_vec(robo)
            Nex = ParamsInit.init_vec(robo)
            for j in reversed(xrange(1, k + 1)):
                _compute_reaction_wrench(
                    robo_tmp, symo, name, j, antRj, antPj,
                    vdot, F, N, Fjnt, Njnt, Fex, Nex
                )
            # reaction wrench for base
            _compute_base_reaction_wrench(
                robo_tmp, symo, name, antRj,antPj,
                vdot, F, N, Fex, Nex, Fjnt, Njnt
            )
            for j in xrange(1, k + 1):
                _compute_joint_torque(robo_tmp, symo, name, j, Fjnt, Njnt)
        # reset all the parameters to zero
        robo_tmp.put_inert_param(sympy.zeros(10, 1), k)
        # compute model for the joint parameters
        # avoid these parameters for link 0
        if k == 0: continue
        _compute_joint_torque_deriv(
            symo, robo.IA[k], robo.qddot[k], k
        )
        _compute_joint_torque_deriv(
            symo, robo.FS[k], sympy.sign(robo.qdot[k]), k
        )
        _compute_joint_torque_deriv(
            symo, robo.FV[k], robo.qdot[k], k
        )
    return symo
Beispiel #21
0
def dynamic_identification_NE(robo):
    """Computes Dynamic Identification 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
    """

    # init forces vectors
    Fjnt = ParamsInit.init_vec(robo)
    Njnt = ParamsInit.init_vec(robo)
    # init file output, writing the robot description
    symo = symbolmgr.SymbolManager()
    symo.file_open(robo, 'dim')
    title = "Dynamic identification model using Newton - Euler Algorith"
    symo.write_params_table(robo, title, inert=True, dynam=True)
    # init transformation
    antRj, antPj = compute_rot_trans(robo, symo)
    # init velocities and accelerations
    w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj)
    # virtual robot with only one non-zero parameter at once
    robo_tmp = deepcopy(robo)
    robo_tmp.IA = sympy.zeros(robo.NL, 1)
    robo_tmp.FV = sympy.zeros(robo.NL, 1)
    robo_tmp.FS = sympy.zeros(robo.NL, 1)
    for k in xrange(1, robo.NL):
        param_vec = robo.get_inert_param(k)
        F = ParamsInit.init_vec(robo)
        N = ParamsInit.init_vec(robo)
        for i in xrange(10):
            if param_vec[i] == tools.ZERO:
                continue
            # change link names according to current non-zero parameter
            robo_tmp.num = [str(l) + str(param_vec[i])
                            for l in xrange(k + 1)]
            # set the parameter to 1
            mask = sympy.zeros(10, 1)
            mask[i] = 1
            robo_tmp.put_inert_param(mask, k)
            # compute the total forcec of the link k
            compute_wrench(robo_tmp, symo, k, w, wdot, U, vdot, F, N)
            # init external forces
            Fex = copy(robo.Fex)
            Nex = copy(robo.Nex)
            for j in reversed(xrange(k + 1)):
                compute_joint_wrench(robo_tmp, symo, j, antRj, antPj,
                                     vdot, Fjnt, Njnt, F, N, Fex, Nex)
            for j in xrange(k + 1):
                compute_torque(robo_tmp, symo, j, Fjnt, Njnt, 'DG')
        # reset all the parameters to zero
        robo_tmp.put_inert_param(sympy.zeros(10, 1), k)
        # compute model for the joint parameters
        compute_joint_torque_deriv(symo, robo.IA[k],
                                   robo.qddot[k], k)
        compute_joint_torque_deriv(symo, robo.FS[k],
                                   sympy.sign(robo.qdot[k]), k)
        compute_joint_torque_deriv(symo, robo.FV[k],
                                   robo.qdot[k], k)
    # closing the output file
    symo.file_close()
    return symo
Beispiel #22
0
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)
Beispiel #23
0
def direct_dynmodel(robo, symo):
    """
    Compute the Direct Dynamic Model using Newton-Euler algorithm for
    robots with floating and fixed 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)
    qddot = ParamsInit.init_scalar(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)
        # 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)
    # 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,
                           replace=True)
        if j == 0:
            continue
        compute_tau(robo, symo, j, jaj, star_beta, tau)
        compute_star_terms(robo, symo, j, jaj, jTant, gamma, tau, h_inv, jah,
                           star_inertia, star_beta)
        if j == first_link:
            continue
    # 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):
        # 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)
Beispiel #24
0
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)
Beispiel #25
0
def dynamic_identification_NE(robo):
    """Computes Dynamic Identification 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
    """

    # init forces vectors
    Fjnt = ParamsInit.init_vec(robo)
    Njnt = ParamsInit.init_vec(robo)
    # init file output, writing the robot description
    symo = symbolmgr.SymbolManager()
    symo.file_open(robo, 'dim')
    title = "Dynamic identification model using Newton - Euler Algorith"
    symo.write_params_table(robo, title, inert=True, dynam=True)
    # init transformation
    antRj, antPj = compute_rot_trans(robo, symo)
    # init velocities and accelerations
    w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj)
    # virtual robot with only one non-zero parameter at once
    robo_tmp = deepcopy(robo)
    robo_tmp.IA = sympy.zeros(robo.NL, 1)
    robo_tmp.FV = sympy.zeros(robo.NL, 1)
    robo_tmp.FS = sympy.zeros(robo.NL, 1)
    for k in xrange(1, robo.NL):
        param_vec = robo.get_inert_param(k)
        F = ParamsInit.init_vec(robo)
        N = ParamsInit.init_vec(robo)
        for i in xrange(10):
            if param_vec[i] == tools.ZERO:
                continue
            # change link names according to current non-zero parameter
            robo_tmp.num = [str(l) + str(param_vec[i]) for l in xrange(k + 1)]
            # set the parameter to 1
            mask = sympy.zeros(10, 1)
            mask[i] = 1
            robo_tmp.put_inert_param(mask, k)
            # compute the total forcec of the link k
            compute_wrench(robo_tmp, symo, k, w, wdot, U, vdot, F, N)
            # init external forces
            Fex = copy(robo.Fex)
            Nex = copy(robo.Nex)
            for j in reversed(xrange(k + 1)):
                compute_joint_wrench(robo_tmp, symo, j, antRj, antPj, vdot,
                                     Fjnt, Njnt, F, N, Fex, Nex)
            for j in xrange(k + 1):
                compute_torque(robo_tmp, symo, j, Fjnt, Njnt, 'DG')
        # reset all the parameters to zero
        robo_tmp.put_inert_param(sympy.zeros(10, 1), k)
        # compute model for the joint parameters
        compute_joint_torque_deriv(symo, robo.IA[k], robo.qddot[k], k)
        compute_joint_torque_deriv(symo, robo.FS[k], sympy.sign(robo.qdot[k]),
                                   k)
        compute_joint_torque_deriv(symo, robo.FV[k], robo.qdot[k], k)
    # closing the output file
    symo.file_close()
    return symo
Beispiel #26
0
def direct_dynmodel(robo, symo):
    """
    Compute the Direct Dynamic Model using Newton-Euler algorithm for
    robots with floating and fixed 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)
    qddot = ParamsInit.init_scalar(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)
        # 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)
    # 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, replace=True
        )
        if j == 0:
            continue
        compute_tau(robo, symo, j, jaj, star_beta, tau)
        compute_star_terms(
            robo, symo, j, jaj, jTant, gamma, tau,
            h_inv, jah, star_inertia, star_beta
        )
        if j == first_link:
            continue
    # 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):
        # 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
        )
Beispiel #27
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 = ParamsInit.init_vec(robo)
        # antecedent angular velocity, projected into jth frame
    w = ParamsInit.init_w(robo)
    jaj = ParamsInit.init_vec(robo, 6)
    jTant = ParamsInit.init_mat(robo, 6)   # Twist transform list of Matrices 6x6
    beta_star = ParamsInit.init_vec(robo, 6)
    grandJ = ParamsInit.init_mat(robo, 6)
    link_acc = ParamsInit.init_vec(robo, 6)
    H_inv = ParamsInit.init_scalar(robo)
    juj = ParamsInit.init_vec(robo, 6)   # Jj*aj / Hj
    Tau = ParamsInit.init_scalar(robo)
    grandVp = ParamsInit.init_vec(robo, 6)
    grandVp.append(Matrix([robo.vdot0 - robo.G, robo.w0]))
    symo = symbolmgr.SymbolManager()
    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
Beispiel #28
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 = ParamsInit.init_vec(robo)
    # antecedent angular velocity, projected into jth frame
    w = ParamsInit.init_w(robo)
    jaj = ParamsInit.init_vec(robo, 6)
    jTant = ParamsInit.init_mat(robo,
                                6)  # Twist transform list of Matrices 6x6
    beta_star = ParamsInit.init_vec(robo, 6)
    grandJ = ParamsInit.init_mat(robo, 6)
    link_acc = ParamsInit.init_vec(robo, 6)
    H_inv = ParamsInit.init_scalar(robo)
    juj = ParamsInit.init_vec(robo, 6)  # Jj*aj / Hj
    Tau = ParamsInit.init_scalar(robo)
    grandVp = ParamsInit.init_vec(robo, 6)
    grandVp.append(Matrix([robo.vdot0 - robo.G, robo.w0]))
    symo = symbolmgr.SymbolManager()
    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