Пример #1
0
def calc_inv_pos(angles, target_pos, target_ori, epsilon, right=True):
    p = np.array([0, 0, 0, 1])
    angs = np.array([a for a in angles])
    sum_old = 100000
    while True:
        pos, ori, j = fk.calc_fk_and_jacob(angs, jacob=True, right=right)
        J = _calc_invJ(j)
        delta_pos = np.matrix((target_pos - pos)[0:3]).transpose()
        delta_ori = np.matrix((target_ori - ori)[0:3]).transpose()

        delta = np.vstack((delta_pos, delta_ori))

        v_w = (J * delta).transpose()
        angs = np.squeeze(np.asarray(v_w)) + angs

        sum = 0

        for d in delta:
            sum = sum + math.fabs(d)

        #sum = np.sum(delta_pos)
        if sum < epsilon:
            break

        if sum > sum_old:
            print '# set_position error : Distance can not converged.'
            return None
        sum_old = sum
    return angs
Пример #2
0
def left_arm_get_position(angles):
    """
    Just calculate the position when joints on the pepper's left arm is in given positions

    Args:
      angles : Angles of left arm joints (list of 5 double values. unit is radian)
    
    Returns:
      A tuple of two arrays (position, orientation). orientation is presented as Matrix. Unit = meter.
      
      (position, orientation) = (np.array([position_x, position_y, position_z]), np.array([[R00, R01, R02], [R10, R11, R12], [R20, R21, R22]]))
    """
    return fk.calc_fk_and_jacob(angles, jacob=False, right=False)
def left_arm_get_position(angles):
    """
    Just calculate the position when joints on the pepper's left arm is in given positions

    Args:
      angles : Angles of left arm joints (list of 5 double values. unit is radian)
    
    Returns:
      A tuple of two arrays (position, orientation). orientation is presented as Matrix. Unit = meter.
      
      (position, orientation) = (np.array([position_x, position_y, position_z]), np.array([[R00, R01, R02], [R10, R11, R12], [R20, R21, R22]]))
    """
    return fk.calc_fk_and_jacob(angles, jacob=False, right=False)
def calc_inv_pos(angles, target_pos, target_ori, epsilon, right=True):
    p  = np.array([0,0,0,1])
    angs = np.array([a for a in angles])
    sum_old = 100000
    while True:
        pos, ori, j = fk.calc_fk_and_jacob(angs, jacob=True, right=right)
        J = _calc_invJ(j)
        delta_pos = np.matrix((target_pos-pos)[0:3]).transpose()
        v = (J * delta_pos).transpose()
        angs = np.squeeze(np.asarray(v)) + angs
        
        sum = 0
        for d in delta_pos:
            sum = sum + math.fabs(d)
        #sum = np.sum(delta_pos)
        if sum < epsilon:
            break
        if sum > sum_old:
            print '# set_position error : Distance can not converged.'
            return None
        sum_old = sum
    return angs