Example #1
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
Example #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 = 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
Example #3
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
        )
Example #4
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)
Example #5
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)
Example #6
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)