Пример #1
0
def _compute_gyroscopic_acceleration(model, robo, j, i):
    """
    Compute the gyroscopic acceleration of link j.

    Args:
        model: An instance of DynModel
        robo: An instance of Robot
        j: link number
        i: antecendent value

    Returns:
        An instance of DynModel that contains all the new values.
    """
    j_gamma_j = Screw()
    # local variables
    j_rot_i = robo.geos[j].tmat.inv_rot
    i_trans_j = robo.geos[j].tmat.trans
    i_omega_i = model.vels[i].ang
    sigma_j = robo.geos[j].sigma
    sigma_dash_j = 1 - sigma_j
    j_z_j = robo.geos[j].zunit
    qdot_j = robo.qdots[j]
    # actual computation
    j_omega_i = j_rot_i * i_omega_i
    # term1 = i_omega_i x (i_omega_i x i_trans_j)
    term1 = skew(i_omega_i) * (skew(i_omega_i) * i_trans_j)
    # term2 = j_omega_i x (qdot_j * j_z_j)
    term2 = skew(j_omega_i) * (qdot_j * j_z_j)
    j_gamma_j.lin = (j_rot_i * term1) + (2 * sigma_j * term2)
    j_gamma_j.ang = sigma_dash_j * term2
    # store computed acceleration in model
    model.gammas[j] = j_gamma_j
    return model
Пример #2
0
def _compute_beta_wrench(model, robo, j):
    """
    Compute the wrench for link j which combines the external forces,
    Coriolis forces and centrifugal forces.

    Args:
        model: An instance of DynModel
        robo: An instance of Robot
        j: link number

    Returns:
        An instance of DynModel that contains all the new values.
    """
    j_beta_j = Screw()
    # local variables
    j_omega_j = model.vels[j].ang
    j_fe_j = robo.dyns[j].wrench.val
    j_ms_j = robo.dyns[j].mass_tensor
    j_inertia_j = robo.dyns[j].inertia
    # actual computation
    # lin_term = j_omega_j x (j_omega_j x j_ms_j)
    lin_term = skew(j_omega_j) * (skew(j_omega_j) * j_ms_j)
    # ang_term = j_omega_j x (j_inertia_j * j_omega_j)
    ang_term = skew(j_omega_j) * (j_inertia_j * j_omega_j)
    term = Screw(lin=lin_term, ang=ang_term)
    j_beta_j.val = - j_fe_j - term.val
    # store computed wrench in model
    model.betas[j] = j_beta_j
    return model
Пример #3
0
def compute_triangle_elements(
    robo, symo, j, k, ka, antRj, antPj, aje1,
    forces, moments, inertia_a12, inertia_a22
):
    """
    Compute elements below and above diagonal of the inertia matrix
    (internal function).

    Note:
        forces, moments, inertia_a12, inertia_a22 are the output
        parameters
    """
    forces[ka] = antRj[k] * forces[k]
    if k == j and robo.sigma[j] == 0:
        moments[ka] = aje1[k] + \
            (tools.skew(antPj[k]) * antRj[k] * forces[k])
    else:
        moments[ka] = (antRj[k] * moments[k]) + \
            (tools.skew(antPj[k]) * antRj[k] * forces[k])
    if ka == 0:
        inertia_a12[j][:3, 0] = symo.mat_replace(
            forces[ka], 'AV0', j, forced=True
        )
        inertia_a12[j][3:, 0] = symo.mat_replace(
            moments[ka], 'AW0', j, forced=True
        )
    else:
        symo.mat_replace(forces[ka], 'E' + CHARSYMS[j], ka)
        symo.mat_replace(moments[ka], 'N' + CHARSYMS[j], ka)
        if robo.sigma[ka] == 0:
            inertia_a22[j-1, ka-1] = moments[ka][2]
        elif robo.sigma[ka] == 1:
            inertia_a22[j-1, ka-1] = forces[ka][2]
        inertia_a22[ka-1, j-1] = inertia_a22[j-1, ka-1]
Пример #4
0
def _compute_beta_wrench(model, robo, j):
    """
    Compute the wrench for link j which combines the external forces,
    Coriolis forces and centrifugal forces.

    Args:
        model: An instance of DynModel
        robo: An instance of Robot
        j: link number

    Returns:
        An instance of DynModel that contains all the new values.
    """
    j_beta_j = Screw()
    # local variables
    j_omega_j = model.vels[j].ang
    j_fe_j = robo.dyns[j].wrench.val
    j_ms_j = robo.dyns[j].mass_tensor
    j_inertia_j = robo.dyns[j].inertia
    # actual computation
    # lin_term = j_omega_j x (j_omega_j x j_ms_j)
    lin_term = skew(j_omega_j) * (skew(j_omega_j) * j_ms_j)
    # ang_term = j_omega_j x (j_inertia_j * j_omega_j)
    ang_term = skew(j_omega_j) * (j_inertia_j * j_omega_j)
    term = Screw(lin=lin_term, ang=ang_term)
    j_beta_j.val = -j_fe_j - term.val
    # store computed wrench in model
    model.betas[j] = j_beta_j
    return model
Пример #5
0
def _compute_gyroscopic_acceleration(model, robo, j, i):
    """
    Compute the gyroscopic acceleration of link j.

    Args:
        model: An instance of DynModel
        robo: An instance of Robot
        j: link number
        i: antecendent value

    Returns:
        An instance of DynModel that contains all the new values.
    """
    j_gamma_j = Screw()
    # local variables
    j_rot_i = robo.geos[j].tmat.inv_rot
    i_trans_j = robo.geos[j].tmat.trans
    i_omega_i = model.vels[i].ang
    sigma_j = robo.geos[j].sigma
    sigma_dash_j = 1 - sigma_j
    j_z_j = robo.geos[j].zunit
    qdot_j = robo.qdots[j]
    # actual computation
    j_omega_i = j_rot_i * i_omega_i
    # term1 = i_omega_i x (i_omega_i x i_trans_j)
    term1 = skew(i_omega_i) * (skew(i_omega_i) * i_trans_j)
    # term2 = j_omega_i x (qdot_j * j_z_j)
    term2 = skew(j_omega_i) * (qdot_j * j_z_j)
    j_gamma_j.lin = (j_rot_i * term1) + (2 * sigma_j * term2)
    j_gamma_j.ang = sigma_dash_j * term2
    # store computed acceleration in model
    model.gammas[j] = j_gamma_j
    return model
Пример #6
0
def compute_composite_inertia(
    robo, symo, j, antRj, antPj,
    aje1, comp_inertia3, comp_ms, comp_mass
):
    """
    Compute composite inertia (internal function).

    Note:
        aje1, comp_inertia3, comp_ms, comp_mass are the output
        parameters.
    """
    i = robo.ant[j]
    i_ms_j_c = antRj[j] * comp_ms[j]
    i_ms_j_c = symo.mat_replace(i_ms_j_c, 'AS', j)
    expr1 = antRj[j] * comp_inertia3[j]
    expr1 = symo.mat_replace(expr1, 'AJ', j)
    aje1[j] = expr1[:, 2]
    expr2 = expr1 * antRj[j].transpose()
    expr2 = symo.mat_replace(expr2, 'AJA', j)
    expr3 = tools.skew(antPj[j]) * tools.skew(i_ms_j_c)
    expr3 = symo.mat_replace(expr3, 'PAS', j)
    comp_inertia3[i] += expr2 - (expr3 + expr3.transpose()) + \
        (comp_mass[j] * tools.skew(antPj[j]) * \
        tools.skew(antPj[j]).transpose())
    comp_ms[i] = comp_ms[i] + i_ms_j_c + (antPj[j] * comp_mass[j])
    comp_mass[i] = comp_mass[i] + comp_mass[j]
Пример #7
0
def inertia_spatial(inertia, ms_tensor, mass):
    """
    Setup spatial inertia matrix (internal function).
    """
    return Matrix([
        (mass * sympy.eye(3)).row_join(tools.skew(ms_tensor).transpose()),
        tools.skew(ms_tensor).row_join(inertia)
    ])
Пример #8
0
 def init_u(cls, robo):
     """Generates a list of auxiliary U matrices"""
     u_aux_matrix = ParamsInit.init_mat(robo)
     # the value for the -1th base frame
     u_aux_matrix.append(
         tools.skew(robo.w0)**2 + tools.skew(robo.wdot0)
     )
     return u_aux_matrix
Пример #9
0
 def init_u(cls, robo):
     """Generates a list of auxiliary U matrices"""
     u_aux_matrix = ParamsInit.init_mat(robo)
     # the value for the -1th base frame
     u_aux_matrix.append(
         tools.skew(robo.w0)**2 + tools.skew(robo.wdot0)
     )
     return u_aux_matrix
Пример #10
0
def vec_mut_M(P):
    """Internal function. Needed for inertia parameters transformation

    Parameters
    ==========
    P : Matrix 3x1
        position vector

    Returns : Matrix 6x1
    """
    U = -tools.skew(P) * tools.skew(P)
    return Matrix([U[0, 0], U[0, 1], U[0, 2], U[1, 1], U[1, 2], U[2, 2]])
Пример #11
0
def vec_mut_M(P):
    """Internal function. Needed for inertia parameters transformation

    Parameters
    ==========
    P : Matrix 3x1
        position vector

    Returns : Matrix 6x1
    """
    U = -tools.skew(P)*tools.skew(P)
    return Matrix([U[0, 0], U[0, 1], U[0, 2], U[1, 1], U[1, 2], U[2, 2]])
Пример #12
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]
Пример #13
0
def compute_beta(robo, symo, j, w, beta_star):
    """Internal function. Computes link's wrench when
    the joint accelerations are zero

    Notes
    =====
    beta_star is the output parameter
    """
    E1 = symo.mat_replace(robo.J[j]*w[j], 'JW', j)
    E2 = symo.mat_replace(tools.skew(w[j])*E1, 'KW', j)
    E3 = tools.skew(w[j])*robo.MS[j]
    E4 = symo.mat_replace(tools.skew(w[j])*E3, 'SW', j)
    E5 = -robo.Nex[j] - E2
    E6 = -robo.Fex[j] - E4
    beta_star[j] = Matrix([E6, E5])
Пример #14
0
def vec_mut_MS(v, P):
    """Internal function. Needed for inertia parameters transformation

    Parameters
    ==========
    v : Matrix 3x1
        axis vector
    P : Matrix 3x1
        position vector

    Returns : Matrix 6x1
    """
    U = - tools.skew(v)*tools.skew(P)
    return Matrix([2*U[0, 0], U[0, 1] + U[1, 0], U[0, 2] + U[2, 0],
                   2*U[1, 1], U[1, 2] + U[2, 1], 2*U[2, 2]])
Пример #15
0
def compute_beta(robo, symo, j, w, beta_star):
    """Internal function. Computes link's wrench when
    the joint accelerations are zero

    Notes
    =====
    beta_star is the output parameter
    """
    E1 = symo.mat_replace(robo.J[j] * w[j], 'JW', j)
    E2 = symo.mat_replace(tools.skew(w[j]) * E1, 'KW', j)
    E3 = tools.skew(w[j]) * robo.MS[j]
    E4 = symo.mat_replace(tools.skew(w[j]) * E3, 'SW', j)
    E5 = -robo.Nex[j] - E2
    E6 = -robo.Fex[j] - E4
    beta_star[j] = Matrix([E6, E5])
Пример #16
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]
Пример #17
0
def compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, Fjnt, Njnt, F, N,
                         Fex, Nex):
    """Internal function. Computes wrench (torques and forces)
    of the joint j

    Notes
    =====
    Fjnt, Njnt, Fex, Nex are the output parameters
    """
    Fjnt[j] = symo.mat_replace(F[j] + Fex[j], 'E', j)
    Njnt[j] = N[j] + Nex[j] + tools.skew(robo.MS[j]) * vdot[j]
    symo.mat_replace(Njnt[j], 'N', j)
    f_ant = symo.mat_replace(antRj[j] * Fjnt[j], 'FDI', j)
    if robo.ant[j] != -1:
        Fex[robo.ant[j]] += f_ant
        Nex[robo.ant[j]] += antRj[j] * Njnt[j] + tools.skew(antPj[j]) * f_ant
Пример #18
0
def compute_Jplus(robo, symo, j, antRj, antPj, Jplus, MSplus, Mplus, AJE1):
    """Internal function. Computes inertia parameters of composed link

    Notes
    =====
    Jplus, MSplus, Mplus are the output parameters
    """
    hat_antPj = tools.skew(antPj[j])
    antMSj = symo.mat_replace(antRj[j] * MSplus[j], 'AS', j)
    E1 = symo.mat_replace(antRj[j] * Jplus[j], 'AJ', j)
    AJE1[j] = E1[:, 2]
    E2 = symo.mat_replace(E1 * antRj[j].T, 'AJA', j)
    E3 = symo.mat_replace(hat_antPj * tools.skew(antMSj), 'PAS', j)
    Jplus[robo.ant[j]] += E2 - (E3 + E3.T) + hat_antPj * hat_antPj.T * Mplus[j]
    MSplus[robo.ant[j]] += antMSj + antPj[j] * Mplus[j]
    Mplus[robo.ant[j]] += Mplus[j]
Пример #19
0
def compute_Jplus(robo, symo, j, antRj, antPj, Jplus, MSplus, Mplus, AJE1):
    """Internal function. Computes inertia parameters of composed link

    Notes
    =====
    Jplus, MSplus, Mplus are the output parameters
    """
    hat_antPj = tools.skew(antPj[j])
    antMSj = symo.mat_replace(antRj[j]*MSplus[j], 'AS', j)
    E1 = symo.mat_replace(antRj[j]*Jplus[j], 'AJ', j)
    AJE1[j] = E1[:, 2]
    E2 = symo.mat_replace(E1*antRj[j].T, 'AJA', j)
    E3 = symo.mat_replace(hat_antPj*tools.skew(antMSj), 'PAS', j)
    Jplus[robo.ant[j]] += E2 - (E3 + E3.T) + hat_antPj*hat_antPj.T*Mplus[j]
    MSplus[robo.ant[j]] += antMSj + antPj[j]*Mplus[j]
    Mplus[robo.ant[j]] += Mplus[j]
Пример #20
0
def compute_joint_wrench(robo, symo, j, antRj, antPj, vdot,
                         Fjnt, Njnt, F, N, Fex, Nex):
    """Internal function. Computes wrench (torques and forces)
    of the joint j

    Notes
    =====
    Fjnt, Njnt, Fex, Nex are the output parameters
    """
    Fjnt[j] = symo.mat_replace(F[j] + Fex[j], 'E', j)
    Njnt[j] = N[j] + Nex[j] + tools.skew(robo.MS[j])*vdot[j]
    symo.mat_replace(Njnt[j], 'N', j)
    f_ant = symo.mat_replace(antRj[j]*Fjnt[j], 'FDI', j)
    if robo.ant[j] != - 1:
        Fex[robo.ant[j]] += f_ant
        Nex[robo.ant[j]] += antRj[j]*Njnt[j] + tools.skew(antPj[j])*f_ant
Пример #21
0
 def setUp(self):
     link = 33
     # setup an instance of DynParams
     self.data = DynParams(link)
     # setup data to compare against
     self.link = link
     # inertia matrix terms
     self.xx = var('XX33')
     self.xy = var('XY33')
     self.xz = var('XZ33')
     self.yy = var('YY33')
     self.yz = var('YZ33')
     self.zz = var('ZZ33')
     # mass tensor terms
     self.msx = var('MX33')
     self.msy = var('MY33')
     self.msz = var('MZ33')
     # link mass
     self.mass = var('M33')
     # rotor inertia term
     self.ia = var('IA33')
     # coulomb friction parameter
     self.frc = var('FS33')
     # viscous friction parameter
     self.frv = var('FV33')
     # external forces and moments
     self.fx_ext = var('FX33')
     self.fy_ext = var('FY33')
     self.fz_ext = var('FZ33')
     self.mx_ext = var('CX33')
     self.my_ext = var('CY33')
     self.mz_ext = var('CZ33')
     # setup matrix terms to compare against
     self.inertia = Matrix([
         [self.xx, self.xy, self.xz],
         [self.xy, self.yy, self.yz],
         [self.xz, self.yz, self.zz]
     ])
     self.mass_tensor = Matrix([self.msx, self.msy, self.msz])
     m_eye = self.mass * eye(3)
     ms_skew = tools.skew(self.mass_tensor)
     self.spatial_inertia = screw6.Screw6(
         tl=m_eye, tr=ms_skew.transpose(),
         bl=ms_skew, br=self.inertia
     )
     self.wrench = screw.Screw(
         lin=Matrix([self.fx_ext, self.fy_ext, self.fz_ext]),
         ang=Matrix([self.mx_ext, self.my_ext, self.mz_ext])
     )
     # setup params to compare against
     self.param_xx = var('XX20')
     self.param_yy = var('YY20')
     self.param_zz = var('ZZ20')
     self.params = {
         'xx': self.param_xx,
         'yy': self.param_yy,
         'zz': self.param_zz
     }
     self.wrong_param = {'rand': 'some-value'}
Пример #22
0
 def spatial_inertia(self):
     """Get spatial inertia (6x6) matrix."""
     m_eye = self.mass * eye(3)
     ms_skew = tools.skew(self.mass_tensor)
     return Screw6(
         tl=m_eye, tr=ms_skew.transpose(),
         bl=ms_skew, br=self.inertia
     )
Пример #23
0
def vec_mut_MS(v, P):
    """Internal function. Needed for inertia parameters transformation

    Parameters
    ==========
    v : Matrix 3x1
        axis vector
    P : Matrix 3x1
        position vector

    Returns : Matrix 6x1
    """
    U = -tools.skew(v) * tools.skew(P)
    return Matrix([
        2 * U[0, 0], U[0, 1] + U[1, 0], U[0, 2] + U[2, 0], 2 * U[1, 1],
        U[1, 2] + U[2, 1], 2 * U[2, 2]
    ])
Пример #24
0
 def _compute_smat(self):
     """
     Compute the transformation matrix in Screw (6x6) matrix form.
     """
     self._smat = Screw6(
         tl=self.rot, tr=tools.skew(self.trans),
         bl=sympy.zeros(3, 3), br=self.rot
     )
Пример #25
0
 def spatial_inertia(self):
     """Get spatial inertia (6x6) matrix."""
     m_eye = self.mass * eye(3)
     ms_skew = tools.skew(self.mass_tensor)
     return Screw6(tl=m_eye,
                   tr=ms_skew.transpose(),
                   bl=ms_skew,
                   br=self.inertia)
Пример #26
0
 def _compute_smat(self):
     """
     Compute the transformation matrix in Screw (6x6) matrix form.
     """
     self._smat = Screw6(tl=self.rot,
                         tr=tools.skew(self.trans),
                         bl=sympy.zeros(3, 3),
                         br=self.rot)
Пример #27
0
def compute_link_acc(robo, symo, j, antRj, antPj, link_acc, w, wi):
    """Internal function. Computes link's accelerations when
    the joint accelerations are zero

    Notes
    =====
    link_acc is the output parameter
    """
    E1 = symo.mat_replace(tools.skew(wi[j])*Matrix([0, 0, robo.qdot[j]]),
                          'WQ', j)
    E2 = (1 - robo.sigma[j])*E1
    E3 = 2*robo.sigma[j]*E1
    E4 = tools.skew(w[robo.ant[j]])*antPj[j]
    E5 = tools.skew(w[robo.ant[j]])*E4
    E6 = antRj[j].T*E5
    E7 = symo.mat_replace(E6 + E3, 'LW', j)
    link_acc[j] = Matrix([E7, E2])
Пример #28
0
def compute_link_acc(robo, symo, j, antRj, antPj, link_acc, w, wi):
    """Internal function. Computes link's accelerations when
    the joint accelerations are zero

    Notes
    =====
    link_acc is the output parameter
    """
    E1 = symo.mat_replace(
        tools.skew(wi[j]) * Matrix([0, 0, robo.qdot[j]]), 'WQ', j)
    E2 = (1 - robo.sigma[j]) * E1
    E3 = 2 * robo.sigma[j] * E1
    E4 = tools.skew(w[robo.ant[j]]) * antPj[j]
    E5 = tools.skew(w[robo.ant[j]]) * E4
    E6 = antRj[j].T * E5
    E7 = symo.mat_replace(E6 + E3, 'LW', j)
    link_acc[j] = Matrix([E7, E2])
Пример #29
0
 def _compute_sinv(self):
     """
     Compute the inverse transformation matrix in Screw
     (6x6) matrix form.
     """
     self._sinv = Screw6(tl=self.inv_rot,
                         tr=-(self.inv_rot * tools.skew(self.trans)),
                         bl=sympy.zeros(3, 3),
                         br=self.inv_rot)
Пример #30
0
 def _compute_sinv(self):
     """
     Compute the inverse transformation matrix in Screw
     (6x6) matrix form.
     """
     self._sinv = Screw6(
         tl=self.inv_rot, tr=-(self.inv_rot * tools.skew(self.trans)),
         bl=sympy.zeros(3, 3), br=self.inv_rot
     )
Пример #31
0
 def setUp(self):
     link = 33
     # setup an instance of DynParams
     self.data = DynParams(link)
     # setup data to compare against
     self.link = link
     # inertia matrix terms
     self.xx = var('XX33')
     self.xy = var('XY33')
     self.xz = var('XZ33')
     self.yy = var('YY33')
     self.yz = var('YZ33')
     self.zz = var('ZZ33')
     # mass tensor terms
     self.msx = var('MX33')
     self.msy = var('MY33')
     self.msz = var('MZ33')
     # link mass
     self.mass = var('M33')
     # rotor inertia term
     self.ia = var('IA33')
     # coulomb friction parameter
     self.frc = var('FS33')
     # viscous friction parameter
     self.frv = var('FV33')
     # external forces and moments
     self.fx_ext = var('FX33')
     self.fy_ext = var('FY33')
     self.fz_ext = var('FZ33')
     self.mx_ext = var('CX33')
     self.my_ext = var('CY33')
     self.mz_ext = var('CZ33')
     # setup matrix terms to compare against
     self.inertia = Matrix([[self.xx, self.xy, self.xz],
                            [self.xy, self.yy, self.yz],
                            [self.xz, self.yz, self.zz]])
     self.mass_tensor = Matrix([self.msx, self.msy, self.msz])
     m_eye = self.mass * eye(3)
     ms_skew = tools.skew(self.mass_tensor)
     self.spatial_inertia = screw6.Screw6(tl=m_eye,
                                          tr=ms_skew.transpose(),
                                          bl=ms_skew,
                                          br=self.inertia)
     self.wrench = screw.Screw(
         lin=Matrix([self.fx_ext, self.fy_ext, self.fz_ext]),
         ang=Matrix([self.mx_ext, self.my_ext, self.mz_ext]))
     # setup params to compare against
     self.param_xx = var('XX20')
     self.param_yy = var('YY20')
     self.param_zz = var('ZZ20')
     self.params = {
         'xx': self.param_xx,
         'yy': self.param_yy,
         'zz': self.param_zz
     }
     self.wrong_param = {'rand': 'some-value'}
Пример #32
0
def compute_beta(robo, symo, j, w, beta):
    """
    Compute beta wrench which is a combination of coriolis forces,
    centrifugal forces and external forces (internal function).

    Note:
        beta is the output parameter
    """
    expr1 = robo.J[j] * w[j]
    expr1 = symo.mat_replace(expr1, 'JW', j)
    expr2 = tools.skew(w[j]) * expr1
    expr2 = symo.mat_replace(expr2, 'KW', j)
    expr3 = tools.skew(w[j]) * robo.MS[j]
    expr4 = tools.skew(w[j]) * expr3
    expr4 = symo.mat_replace(expr4, 'SW', j)
    expr5 = -robo.Nex[j] - expr2
    expr6 = -robo.Fex[j] - expr4
    beta[j] = Matrix([expr6, expr5])
    beta[j] = symo.mat_replace(beta[j], 'BETA', j)
Пример #33
0
def compute_beta(robo, symo, j, w, beta):
    """
    Compute beta wrench which is a combination of coriolis forces,
    centrifugal forces and external forces (internal function).

    Note:
        beta is the output parameter
    """
    expr1 = robo.J[j] * w[j]
    expr1 = symo.mat_replace(expr1, 'JW', j)
    expr2 = tools.skew(w[j]) * expr1
    expr2 = symo.mat_replace(expr2, 'KW', j)
    expr3 = tools.skew(w[j]) * robo.MS[j]
    expr4 = tools.skew(w[j]) * expr3
    expr4 = symo.mat_replace(expr4, 'SW', j)
    expr5 = -robo.Nex[j] - expr2
    expr6 = -robo.Fex[j] - expr4
    beta[j] = Matrix([expr6, expr5])
    beta[j] = symo.mat_replace(beta[j], 'BETA', j)
Пример #34
0
def compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma):
    """
    Compute gyroscopic acceleration (internal function).

    Note:
        gamma is the output parameter
    """
    i = robo.ant[j]
    expr1 = tools.skew(wi[j]) * Matrix([0, 0, robo.qdot[j]])
    expr1 = symo.mat_replace(expr1, 'WQ', j)
    expr2 = (1 - robo.sigma[j]) * expr1
    expr3 = 2 * robo.sigma[j] * expr1
    expr4 = tools.skew(w[i]) * antPj[j]
    expr5 = tools.skew(w[i]) * expr4
    expr6 = antRj[j].transpose() * expr5
    expr7 = expr6 + expr3
    expr7 = symo.mat_replace(expr7, 'LW', j)
    gamma[j] = Matrix([expr7, expr2])
    gamma[j] = symo.mat_replace(gamma[j], 'GYACC', j)
Пример #35
0
def compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma):
    """
    Compute gyroscopic acceleration (internal function).

    Note:
        gamma is the output parameter
    """
    i = robo.ant[j]
    expr1 = tools.skew(wi[j]) * Matrix([0, 0, robo.qdot[j]])
    expr1 = symo.mat_replace(expr1, 'WQ', j)
    expr2 = (1 - robo.sigma[j]) * expr1
    expr3 = 2 * robo.sigma[j] * expr1
    expr4 = tools.skew(w[i]) * antPj[j]
    expr5 = tools.skew(w[i]) * expr4
    expr6 = antRj[j].transpose() * expr5
    expr7 = expr6 + expr3
    expr7 = symo.mat_replace(expr7, 'LW', j)
    gamma[j] = Matrix([expr7, expr2])
    gamma[j] = symo.mat_replace(gamma[j], 'GYACC', j)
Пример #36
0
def compute_composite_inertia(
    robo, symo, j, antRj, antPj,
    comp_inertia3, comp_ms, comp_mass, composite_inertia
):
    """
    Compute composite inertia (internal function).

    Note:
        comp_inertia3, comp_ms, comp_mass, composite_inertia are the
        output parameters.
    """
    i = robo.ant[j]
    # update inertia3, ms, mass from inertia in order to have the
    # intermediate variables
    comp_inertia3[i] = composite_inertia[i][3:, 3:]
    comp_ms[i] = tools.skew2vec(composite_inertia[i][3:, 0:3])
    comp_mass[i] = composite_inertia[i][0, 0]
    comp_inertia3[j] = composite_inertia[j][3:, 3:]
    comp_ms[j] = tools.skew2vec(composite_inertia[j][3:, 0:3])
    comp_mass[j] = composite_inertia[j][0, 0]
    # actual computation
    i_ms_j_c = antRj[j] * comp_ms[j]
    i_ms_j_c = symo.mat_replace(i_ms_j_c, 'AS', j)
    expr1 = antRj[j] * comp_inertia3[j]
    expr1 = symo.mat_replace(expr1, 'AJ', j)
    expr2 = expr1 * antRj[j].transpose()
    expr2 = symo.mat_replace(expr2, 'AJA', j)
    expr3 = tools.skew(antPj[j]) * tools.skew(i_ms_j_c)
    expr3 = symo.mat_replace(expr3, 'PAS', j)
    i_comp_inertia3_j = expr2 - (expr3 + expr3.transpose()) + \
        (comp_mass[j] * tools.skew(antPj[j]) * \
        tools.skew(antPj[j]).transpose())
    i_comp_inertia3_j = symo.mat_replace(i_comp_inertia3_j, 'JJI', j)
    comp_inertia3[i] = comp_inertia3[i] + i_comp_inertia3_j
    i_comp_ms_j = i_ms_j_c + (antPj[j] * comp_mass[j])
    i_comp_ms_j = symo.mat_replace(i_comp_ms_j, 'MSJI', j)
    comp_ms[i] = comp_ms[i] + i_comp_ms_j
    i_comp_mass_j = symo.replace(comp_mass[j], 'MJI', j)
    comp_mass[i] = comp_mass[i] + i_comp_mass_j
    composite_inertia[i] = inertia_spatial(
        comp_inertia3[i], comp_ms[i], comp_mass[i]
    )
Пример #37
0
def compute_screw_transform(robo, symo, j, antRj, antPj, jTant):
    """Internal function. Computes the screw transformation matrix
    between ant[j] and j frames.

    Notes
    =====
    jTant is an output parameter
    """
    jRant = antRj[j].T
    ET = symo.mat_replace(-jRant * tools.skew(antPj[j]), 'JPR', j)
    jTant[j] = (Matrix([jRant.row_join(ET), zeros(3, 3).row_join(jRant)]))
Пример #38
0
def compute_screw_transform(robo, symo, j, antRj, antPj, jTant):
    """Internal function. Computes the screw transformation matrix
    between ant[j] and j frames.

    Notes
    =====
    jTant is an output parameter
    """
    jRant = antRj[j].T
    ET = symo.mat_replace(-jRant*tools.skew(antPj[j]), 'JPR', j)
    jTant[j] = (Matrix([jRant.row_join(ET),
                        zeros(3, 3).row_join(jRant)]))
Пример #39
0
def compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, F, N, Fjnt, Njnt,
                         Fex, Nex):
    """
    Compute reaction wrench (for default Newton-Euler) of joint j
    (internal function).

    Note:
        Fjnt, Njnt, Fex, Nex are the output parameters
    """
    forced = True if j == 0 else False
    i = robo.ant[j]
    Fjnt[j] = F[j] + Fex[j]
    Fjnt[j] = symo.mat_replace(Fjnt[j], 'E', j, forced=forced)
    Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j])
    Njnt[j] = symo.mat_replace(Njnt[j], 'N', j, forced=forced)
    f_ant = antRj[j] * Fjnt[j]
    f_ant = symo.mat_replace(f_ant, 'FDI', j)
    if i != -1:
        Fex[i] = Fex[i] + f_ant
        Nex[i] = Nex[i] + \
            (antRj[j] * Njnt[j]) + (tools.skew(antPj[j]) * f_ant)
Пример #40
0
def _compute_reaction_wrench(
    robo, symo, name, j, antRj, antPj, vdot, F, N, Fjnt, Njnt, Fex, Nex
):
    """
    Compute reaction wrench (for default Newton-Euler) of joint j
    (internal function).

    Note:
        Fjnt, Njnt, Fex, Nex are the output parameters
    """
    i = robo.ant[j]
    Fjnt[j] = F[j] + Fex[j]
    Fjnt[j] = vec_replace_wrapper(symo, Fjnt[j], 'E', name, j)
    Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j])
    Njnt[j] = vec_replace_wrapper(symo, Njnt[j], 'N', name, j)
    f_ant = antRj[j] * Fjnt[j]
    f_ant = vec_replace_wrapper(symo, f_ant, 'FDI', name, j)
    if i != -1:
        Fex[i] = Fex[i] + f_ant
        Nex[i] = Nex[i] + \
            (antRj[j] * Njnt[j]) + (tools.skew(antPj[j]) * f_ant)
Пример #41
0
def compute_dynamic_wrench(robo, symo, j, w, wdot, U, vdot, F, N):
    """
    Compute total wrench of link j (internal function).

    Note:
        F, N are the output parameters
    """
    F[j] = (robo.M[j] * vdot[j]) + (U[j] * robo.MS[j])
    F[j] = symo.mat_replace(F[j], 'F', j)
    Psi = robo.J[j] * w[j]
    Psi = symo.mat_replace(Psi, 'PSI', j)
    N[j] = (robo.J[j] * wdot[j]) + (tools.skew(w[j]) * Psi)
    N[j] = symo.mat_replace(N[j], 'No', j)
Пример #42
0
def compute_joint_wrench(
    robo, symo, j, antRj, antPj, vdot, F, N, Fjnt, Njnt, Fex, Nex
):
    """
    Compute reaction wrench (for default Newton-Euler) of joint j
    (internal function).

    Note:
        Fjnt, Njnt, Fex, Nex are the output parameters
    """
    forced = True if j == 0 else False
    i = robo.ant[j]
    Fjnt[j] = F[j] + Fex[j]
    Fjnt[j] = symo.mat_replace(Fjnt[j], 'E', j, forced=forced)
    Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j])
    Njnt[j] = symo.mat_replace(Njnt[j], 'N', j, forced=forced)
    f_ant = antRj[j] * Fjnt[j]
    f_ant = symo.mat_replace(f_ant, 'FDI', j)
    if i != -1:
        Fex[i] = Fex[i] + f_ant
        Nex[i] = Nex[i] + \
            (antRj[j] * Njnt[j]) + (tools.skew(antPj[j]) * f_ant)
Пример #43
0
def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N):
    """Internal function. Computes total wrench (torques and forces)
    of the link j

    Notes
    =====
    F, N are the output parameters
    """
    F[j] = robo.M[j] * vdot[j] + U[j] * robo.MS[j]
    symo.mat_replace(F[j], 'F', j)
    Psi = symo.mat_replace(robo.J[j] * w[j], 'PSI', j)
    N[j] = robo.J[j] * wdot[j] + tools.skew(w[j]) * Psi
    symo.mat_replace(N[j], 'No', j)
Пример #44
0
def compute_dynamic_wrench(robo, symo, j, w, wdot, U, vdot, F, N):
    """
    Compute total wrench of link j (internal function).

    Note:
        F, N are the output parameters
    """
    F[j] = (robo.M[j] * vdot[j]) + (U[j] * robo.MS[j])
    F[j] = symo.mat_replace(F[j], 'F', j)
    Psi = robo.J[j] * w[j]
    Psi = symo.mat_replace(Psi, 'PSI', j)
    N[j] = (robo.J[j] * wdot[j]) + (tools.skew(w[j]) * Psi)
    N[j] = symo.mat_replace(N[j], 'No', j)
Пример #45
0
def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N):
    """Internal function. Computes total wrench (torques and forces)
    of the link j

    Notes
    =====
    F, N are the output parameters
    """
    F[j] = robo.M[j]*vdot[j] + U[j]*robo.MS[j]
    symo.mat_replace(F[j], 'F', j)
    Psi = symo.mat_replace(robo.J[j]*w[j], 'PSI', j)
    N[j] = robo.J[j]*wdot[j] + tools.skew(w[j])*Psi
    symo.mat_replace(N[j], 'No', j)
Пример #46
0
def compute_composite_inertia(robo, symo, j, antRj, antPj, comp_inertia3,
                              comp_ms, comp_mass, composite_inertia):
    """
    Compute composite inertia (internal function).

    Note:
        comp_inertia3, comp_ms, comp_mass, composite_inertia are the
        output parameters.
    """
    i = robo.ant[j]
    # update inertia3, ms, mass from inertia in order to have the
    # intermediate variables
    comp_inertia3[i] = composite_inertia[i][3:, 3:]
    comp_ms[i] = tools.skew2vec(composite_inertia[i][3:, 0:3])
    comp_mass[i] = composite_inertia[i][0, 0]
    comp_inertia3[j] = composite_inertia[j][3:, 3:]
    comp_ms[j] = tools.skew2vec(composite_inertia[j][3:, 0:3])
    comp_mass[j] = composite_inertia[j][0, 0]
    # actual computation
    i_ms_j_c = antRj[j] * comp_ms[j]
    i_ms_j_c = symo.mat_replace(i_ms_j_c, 'AS', j)
    expr1 = antRj[j] * comp_inertia3[j]
    expr1 = symo.mat_replace(expr1, 'AJ', j)
    expr2 = expr1 * antRj[j].transpose()
    expr2 = symo.mat_replace(expr2, 'AJA', j)
    expr3 = tools.skew(antPj[j]) * tools.skew(i_ms_j_c)
    expr3 = symo.mat_replace(expr3, 'PAS', j)
    i_comp_inertia3_j = expr2 - (expr3 + expr3.transpose()) + \
        (comp_mass[j] * tools.skew(antPj[j]) * \
        tools.skew(antPj[j]).transpose())
    i_comp_inertia3_j = symo.mat_replace(i_comp_inertia3_j, 'JJI', j)
    comp_inertia3[i] = comp_inertia3[i] + i_comp_inertia3_j
    i_comp_ms_j = i_ms_j_c + (antPj[j] * comp_mass[j])
    i_comp_ms_j = symo.mat_replace(i_comp_ms_j, 'MSJI', j)
    comp_ms[i] = comp_ms[i] + i_comp_ms_j
    i_comp_mass_j = symo.replace(comp_mass[j], 'MJI', j)
    comp_mass[i] = comp_mass[i] + i_comp_mass_j
    composite_inertia[i] = inertia_spatial(comp_inertia3[i], comp_ms[i],
                                           comp_mass[i])
Пример #47
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
Пример #48
0
def compute_A_triangle(robo, symo, j, k, ka, antRj, antPj, f, n, A, AJE1):
    """Internal function. Computes elements below and above diagonal
    of the inertia matrix

    Notes
    =====
    f, n, A are the output parameters
    """
    f[ka] = antRj[k]*f[k]
    if k == j and robo.sigma[j] == 0:
        n[ka] = AJE1[k] + tools.skew(antPj[k])*f[k]
    else:
        n[ka] = antRj[k]*n[k] + tools.skew(antPj[k])*f[k]
    if ka == - 1:
        symo.mat_replace(f[ka], 'AV0')
        symo.mat_replace(n[ka], 'AW0')
    else:
        symo.mat_replace(f[ka], 'E' + chars[j], ka)
        symo.mat_replace(n[ka], 'N' + chars[j], ka)
        if robo.sigma[ka] == 0:
            A[j, ka] = n[ka][2]
        elif robo.sigma[ka] == 1:
            A[j, ka] = f[ka][2]
        A[ka, j] = A[j, ka]
Пример #49
0
def compute_A_triangle(robo, symo, j, k, ka, antRj, antPj, f, n, A, AJE1):
    """Internal function. Computes elements below and above diagonal
    of the inertia matrix

    Notes
    =====
    f, n, A are the output parameters
    """
    f[ka] = antRj[k] * f[k]
    if k == j and robo.sigma[j] == 0:
        n[ka] = AJE1[k] + tools.skew(antPj[k]) * f[k]
    else:
        n[ka] = antRj[k] * n[k] + tools.skew(antPj[k]) * f[k]
    if ka == -1:
        symo.mat_replace(f[ka], 'AV0')
        symo.mat_replace(n[ka], 'AW0')
    else:
        symo.mat_replace(f[ka], 'E' + chars[j], ka)
        symo.mat_replace(n[ka], 'N' + chars[j], ka)
        if robo.sigma[ka] == 0:
            A[j, ka] = n[ka][2]
        elif robo.sigma[ka] == 1:
            A[j, ka] = f[ka][2]
        A[ka, j] = A[j, ka]
Пример #50
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
Пример #51
0
def _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False):
    """
    Computes jacobian of frame n (with origin On in Oj) projected to frame i
    """
    #  symo.write_geom_param(robo, 'Jacobian')
    # TODO: Check projection frames, rewrite DGM call for higher efficiency
    J_col_list = []
    if chain is None:
        chain = robo.chain(n)
        chain.reverse()
    # chain_ext = chain + [robo.ant[min(chain)]]
    # if not i in chain_ext:
    #     i = min(chain_ext)
    # if not j in chain_ext:
    #     j = max(chain_ext)
    kTj_dict = dgm(robo, symo, chain[0], j, key='left', trig_subs=trig_subs)
    kTj_tmp = dgm(robo, symo, chain[-1], j, key='left', trig_subs=trig_subs)
    kTj_dict.update(kTj_tmp)
    iTk_dict = dgm(robo, symo, i, chain[0], key='right', trig_subs=trig_subs)
    iTk_tmp = dgm(robo, symo, i, chain[-1], key='right', trig_subs=trig_subs)
    iTk_dict.update(iTk_tmp)
    for k in chain:
        kTj = kTj_dict[k, j]
        iTk = iTk_dict[i, k]
        isk, ink, iak = Transform.sna(iTk)
        sigm = robo.sigma[k]
        if sigm == 1:
            dvdq = iak
            J_col = dvdq.col_join(Matrix([0, 0, 0]))
        elif sigm == 0:
            dvdq = kTj[0, 3] * ink - kTj[1, 3] * isk
            J_col = dvdq.col_join(iak)
        else:
            J_col = Matrix([0, 0, 0, 0, 0, 0])
        J_col_list.append(J_col.T)
    Jac = Matrix(J_col_list).T
    Jac = Jac.applyfunc(symo.simp)
    iRj = Transform.R(iTk_dict[i, j])
    jTn = dgm(robo, symo, j, n, fast_form=False, trig_subs=trig_subs)
    jPn = Transform.P(jTn)
    L = -tools.skew(iRj * jPn)
    L = L.applyfunc(trigsimp)
    if forced:
        symo.mat_replace(Jac, 'J', '', forced)
        L = symo.mat_replace(L, 'L', '', forced)
    return Jac, L
Пример #52
0
def _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False):
    """
    Computes jacobian of frame n (with origin On in Oj) projected to frame i
    """
    #  symo.write_geom_param(robo, 'Jacobian')
    # TODO: Check projection frames, rewrite DGM call for higher efficiency
    J_col_list = []
    if chain is None:
        chain = robo.chain(n)
        chain.reverse()
    # chain_ext = chain + [robo.ant[min(chain)]]
    # if not i in chain_ext:
    #     i = min(chain_ext)
    # if not j in chain_ext:
    #     j = max(chain_ext)
    kTj_dict = dgm(robo, symo, chain[0], j, key='left', trig_subs=trig_subs)
    kTj_tmp = dgm(robo, symo, chain[-1], j, key='left', trig_subs=trig_subs)
    kTj_dict.update(kTj_tmp)
    iTk_dict = dgm(robo, symo, i, chain[0], key='right', trig_subs=trig_subs)
    iTk_tmp = dgm(robo, symo, i, chain[-1], key='right', trig_subs=trig_subs)
    iTk_dict.update(iTk_tmp)
    for k in chain:
        kTj = kTj_dict[k, j]
        iTk = iTk_dict[i, k]
        isk, ink, iak = Transform.sna(iTk)
        sigm = robo.sigma[k]
        if sigm == 1:
            dvdq = iak
            J_col = dvdq.col_join(Matrix([0, 0, 0]))
        elif sigm == 0:
            dvdq = kTj[0, 3]*ink-kTj[1, 3]*isk
            J_col = dvdq.col_join(iak)
        else:
            J_col = Matrix([0, 0, 0, 0, 0, 0])
        J_col_list.append(J_col.T)
    Jac = Matrix(J_col_list).T
    Jac = Jac.applyfunc(symo.simp)
    iRj = Transform.R(iTk_dict[i, j])
    jTn = dgm(robo, symo, j, n, fast_form=False, trig_subs=trig_subs)
    jPn = Transform.P(jTn)
    L = -tools.skew(iRj*jPn)
    L = L.applyfunc(trigsimp)
    if forced:
        symo.mat_replace(Jac, 'J', '', forced)
        L = symo.mat_replace(L, 'L', '', forced)
    return Jac, L
Пример #53
0
def _compute_base_reaction_wrench(
    robo, symo, name, antRj, antPj, vdot, F, N, Fex, Nex, Fjnt, Njnt
):
    """
    Compute reaction wrench (for default Newton-Euler) on the base
    (internal function).

    Note:
        Fjnt, Njnt are the output parameters
    """
    j = 0
    Fjnt[j] = F[j] + Fex[j]
    Fjnt[j] = vec_replace_wrapper(
        symo, Fjnt[j], 'DE', name, j, forced=True
    )
    Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j])
    Njnt[j] = vec_replace_wrapper(
        symo, Njnt[j], 'DN', name, j, forced=True
    )
Пример #54
0
def inertia_spatial(J, MS, M):
    return Matrix([(M * sympy.eye(3)).row_join(tools.skew(MS).T),
                   tools.skew(MS).row_join(J)])
Пример #55
0
def _v_j(robo, j, antPj, jRant, v, w, qdj, forced=False):
    ant = robo.ant[j]
    v[j] = jRant * (tools.skew(w[ant]) * antPj[j] + v[ant])
    if robo.sigma[j] == 1:  # prismatic joint
        v[j] += qdj
    return v[j]
Пример #56
0
def _omega_dot_j(robo, j, jRant, w, wi, wdot, qdj, qddj):
    wdot[j] = jRant * wdot[robo.ant[j]]
    if robo.sigma[j] == 0:  # revolute joint
        wdot[j] += (qddj + tools.skew(wi) * qdj)
    return wdot[j]
Пример #57
0
def _v_j(robo, j, antPj, jRant, v, w, qdj, forced=False):
    ant = robo.ant[j]
    v[j] = jRant*(tools.skew(w[ant])*antPj[j] + v[ant])
    if robo.sigma[j] == 1:     # prismatic joint
        v[j] += qdj
    return v[j]