Ejemplo n.º 1
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
Ejemplo n.º 2
0
def direct_geometric(robo, frames, trig_subs):
    """Computes trensformation matrix iTj.

    Parameters
    ==========
    robo: Robot
        Instance of robot description container
    frames: list of tuples of type (i,j)
        Defines list of required transformation matrices iTj
    trig_subs: bool, optional
        If True, all the sin(x) and cos(x) will be replaced by symbols
        SX and CX with adding them to the dictionary

    Returns
    =======
    symo: Symoro
        Instance that contains all the relations of the computed model
    """
    symo = Symoro()
    symo.file_open(robo, 'trm')
    symo.write_params_table(robo, 'Direct Geometrix model')
    for i, j in frames:
        symo.write_line('Tramsformation matrix %s T %s' % (i, j))
        print dgm(robo, symo, i, j, fast_form=False,
                  forced=True, trig_subs=trig_subs)
        symo.write_line()
    symo.file_close()
    return symo
Ejemplo n.º 3
0
def accelerations(robo):
    symo = Symoro(None)
    symo.file_open(robo, 'acc')
    symo.write_params_table(robo, 'Link accelerations')
    antRj, antPj = compute_rot_trans(robo, symo)
    compute_vel_acc(robo, symo, antRj, antPj, forced=True, gravity=False)
    symo.file_close()
    return symo
Ejemplo n.º 4
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
Ejemplo n.º 5
0
def igm_Paul(robo, T_ref, n):
    if isinstance(T_ref, list):
        T_ref = Matrix(4, 4, T_ref)
    symo = Symoro()
    symo.file_open(robo, "igm")
    symo.write_params_table(robo, "Inverse Geometrix Model for frame %s" % n)
    _paul_solve(robo, symo, T_ref, 0, n)
    symo.file_close()
    return symo
Ejemplo n.º 6
0
def jacobian(robo, n, i, j):
    symo = Symoro()
    symo.file_open(robo, 'jac')
    title = "Jacobian matrix for frame %s\n"
    title += "Projection frame %s, intermediat frame %s"
    symo.write_params_table(robo, title % (n, i, j))
    _jac(robo, symo, n, i, j, forced=True)
    symo.file_close()
    return symo
Ejemplo n.º 7
0
def jacobian_determinant(robo, n, i, j, rows, cols):
    symo = Symoro(None)
    J, L = _jac(robo, symo, n, i, j, trig_subs=False)
    J_reduced = zeros(len(rows), len(cols))
    for i, i_old in enumerate(rows):
        for j, j_old in enumerate(cols):
            J_reduced[i, j] = J[i_old, j_old]
    symo.file_open(robo, 'det')
    symo.write_params_table(robo, 'Jacobian determinant for frame %s' % n)
    symo.write_line(_jac_det(robo, symo, J=J_reduced))
    symo.file_close()
    return symo
Ejemplo n.º 8
0
def base_paremeters(robo_orig):
    """Computes grouped inertia parameters. New parametrization
    contains less parameters but generates the same dynamic model

    Parameters
    ==========
    robo : Robot
        Instance of robot description container

    Returns
    =======
    symo.sydi : dictionary
        Dictionary with the information of all the sybstitution
    """
    robo = copy(robo_orig)
    lam = [0 for i in xrange(robo.NL)]
    symo = Symoro()
    symo.file_open(robo, 'regp')
    title = 'Base parameters computation'
    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)):
        if robo.sigma[j] == 0:
            # general grouping
            compute_lambda(robo, symo, j, antRj, antPj, lam)
            group_param_rot(robo, symo, j, lam)
            # special grouping
            group_param_rot_spec(robo, symo, j, lam, antRj)
            pass
        elif robo.sigma[j] == 1:
            # general grouping
            group_param_prism(robo, symo, j, antRj)
            # special grouping
            group_param_prism_spec(robo, symo, j, antRj, antPj)
            pass
        elif robo.sigma[j] == 2:
            # fixed joint, group everuthing
            compute_lambda(robo, symo, j, antRj, antPj)
            group_param_fix(robo, symo, j, lam)
        pass
    symo.write_line('*=*')
    symo.write_line()
    title = robo.name + ' grouped inertia parameters'
    symo.write_params_table(robo, title, inert=True, equations=False)
    symo.file_close()
    return robo, symo.sydi
Ejemplo n.º 9
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
Ejemplo n.º 10
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 = 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
Ejemplo n.º 11
0
def inverse_dynamic_NE(robo):
    """Computes Inverse 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
    """
    symo = Symoro()
    symo.file_open(robo, 'idm')
    title = 'Inverse dynamic model using Newton - Euler Algorith'
    symo.write_params_table(robo, title, inert=True, dynam=True)
    Newton_Euler(robo, symo)
    symo.file_close()
    return symo
Ejemplo n.º 12
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
Ejemplo n.º 13
0
def direct_geometric_fast(robo, i, j):
    """Computes trensformation matrix iTj.

    Parameters
    ==========
    robo: Robot
        Instance of robot description container
    i: int
        the to-frame
    j: int
        the from-frame

    Returns
    =======
    symo: Symoro
        Instance that contains all the relations of the computed model
    """
    symo = Symoro()
    symo.file_open(robo, 'fgm')
    symo.write_params_table(robo, 'Direct Geometrix model')
    dgm(robo, symo, i, j, fast_form=True, forced=True)
    symo.file_close()
    return symo
Ejemplo n.º 14
0
def pseudo_force_NE(robo):
    """Computes Coriolis, Centrifugal, Gravity, Friction and external
    torques using Newton-Euler formulation

    Parameters
    ==========
    robo : Robot
        Instance of robot description container

    Returns
    =======
    symo.sydi : dictionary
        Dictionary with the information of all the sybstitution
    """
    robo_pseudo = deepcopy(robo)
    robo_pseudo.qddot = sympy.zeros(robo_pseudo.NL, 1)
    symo = Symoro()
    symo.file_open(robo, 'ccg')
    title = 'Pseudo forces using Newton - Euler Algorith'
    symo.write_params_table(robo, title, inert=True, dynam=True)
    Newton_Euler(robo_pseudo, symo)
    symo.file_close()
    return symo
Ejemplo n.º 15
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 = Init.init_vec(robo)
    Njnt = Init.init_vec(robo)
    # init file output, writing the robot description
    symo = Symoro()
    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 = Init.init_vec(robo)
        N = Init.init_vec(robo)
        for i in xrange(10):
            if param_vec[i] == 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