Exemplo n.º 1
0
    def execute(self, arff_data):
        attributes = [attribute[0] for attribute in arff_data['attributes']]
        dataset = arff_data['data']

        X = (arff_data['X'].T.tolist() if 'X' in arff_data else [])

        base_height = np.array(dataset[:, attributes.index('baseHeight')],
                               dtype='float64')
        target_height = np.array(dataset[:,
                                         attributes.index('targetHeight')],
                                 dtype='float64')
        base_width = np.array(dataset[:, attributes.index('baseWidth')],
                              dtype='float64')
        target_width = np.array(dataset[:, attributes.index('targetWidth')],
                                dtype='float64')
        X.append(
            np.minimum(base_height * base_width, target_height * target_width))

        base_x = np.array(dataset[:, attributes.index('baseX')],
                          dtype='float64')
        target_x = np.array(dataset[:, attributes.index('targetX')],
                            dtype='float64')
        base_y = np.array(dataset[:, attributes.index('baseY')],
                          dtype='float64')
        target_y = np.array(dataset[:, attributes.index('targetY')],
                            dtype='float64')
        X.append(
            np.sqrt(
                np.power(np.abs(base_x - target_x), 2) +
                np.power(np.abs(base_y - target_y), 2)))

        X.append(
            np.abs((base_height * base_width) -
                   (target_height * target_width)) / np.maximum(
                       np.minimum(base_height * base_width, target_height *
                                  target_width), np.ones(len(base_height))))

        X.append(dataset[:, attributes.index('chiSquared')])

        arff_data['X'] = np.array(X, dtype='float64').T
        prev_features = (arff_data['features']
                         if 'features' in arff_data else [])
        arff_data['features'] = prev_features + [
            'area', 'displacement', 'sdr', 'chisquared'
        ]
        arff_data['y'] = np.array(
            arff_data['data'][:, attributes.index(self._class_attr)],
            dtype='int16')
        return arff_data
def whiteningTransform(spectrogram):

    covarianceMatrix = np.cov(spectrogram)
    [eigVal, eigVect] = np.linalg.eigh(covarianceMatrix)
    correctIndices = np.argsort(-eigVal)
    eigVal = eigVal[correctIndices]
    eigVect = eigVect[:, correctIndices]
    Lsqrtinv = np.linalg.inv(np.power(np.diag(eigVal), 0.5))
    return Lsqrtinv @ (eigVect.T)
def project_along_vector(x1, y1, z1, x2, y2, z2, L):
    '''
	 Solve for the point px, py, pz, that is
	 a vector with magnitude L away in the direction between point 2 and point 1,
	 starting at point 1
	 Parameters
	 ----------
	 x1, y1, z1 : scalars
		 point 1
	 x2, y2, z2 : scalars
		 point 2
	 L : scalar
		 Magnitude of vector?
	Returns
	-------
	out : array_like
		projected point on the vector
	'''

    # vector from point 1 to point 2
    vx = x2 - x1
    vy = y2 - y1
    vz = z2 - z1
    v = np.sqrt(np.power(vx, 2) + np.power(vy, 2) + np.power(vz, 2))

    ux = vx / v
    uy = vy / v
    uz = vz / v

    # Need to always project along radius
    # Project backwards
    px = x1 + L * ux
    py = y1 + L * uy
    pz = z1 + L * uz

    return np.array([px, py, pz])
def angle_from_dot_product(a, b):

    ax = a[0]
    ay = a[1]
    az = a[2]

    bx = b[0]
    by = b[1]
    bz = b[2]

    a_mag = np.sqrt(np.power(ax, 2) + np.power(ay, 2) + np.power(az, 2))
    b_mag = np.sqrt(np.power(bx, 2) + np.power(by, 2) + np.power(bz, 2))

    theta = np.arccos((1 / (a_mag * b_mag)) * (ax * bx + ay * by + az * bz))

    return theta
def fabrik(l1, l2, l3, x_prev, y_prev, z_prev, x_command, y_command, z_command,
           tol_limit, max_iterations):

    # Base rotation is simply based on angle made within the x-y plane

    q1_prev = np.arctan2(y_prev[3], x_prev[3])
    q1 = np.arctan2(y_command, x_command)

    base_rotation = q1 - q1_prev  # this is the rotation the base must make to get from initial position to the commanded position

    # Base rotation matrix about z
    R_z = np.array([[np.cos(base_rotation), -np.sin(base_rotation), 0.0],
                    [np.sin(base_rotation),
                     np.cos(base_rotation), 0.0], [0.0, 0.0, 1.0]])

    # Rotate the location of each joint by the base rotation
    # This will force the FABRIK algorithim to only solve
    # in two dimensions, else each joint will move as if it has
    # a 3 DOF range of motion
    # print 'inside the fabrik method and x_joints is'
    # print x_joints
    p4 = np.dot(R_z, [x_prev[3], y_prev[3], z_prev[3]])
    p3 = np.dot(R_z, [x_prev[2], y_prev[2], z_prev[2]])
    p2 = np.dot(R_z, [x_prev[1], y_prev[1], z_prev[1]])
    p1 = np.dot(R_z, [x_prev[0], y_prev[0], z_prev[0]])

    # Store the (x,y,z) position of each joint

    p4x = p4[0]
    p4y = p4[1]
    p4z = p4[2]

    p3x = p3[0]
    p3y = p3[1]
    p3z = p3[2]

    p2x = p2[0]
    p2y = p2[1]
    p2z = p2[2]

    p1x = p1[0]
    p1y = p1[1]
    p1z = p1[2]

    # Starting point of each joint
    p1x_o = p1x
    p1y_o = p1y
    p1z_o = p1z

    iterations = 0
    for j in range(1, max_iterations + 1):

        if np.sqrt(
                np.power(x_command, 2) + np.power(y_command, 2) +
                np.power(z_command, 2)) > (l1 + l2 + l3):
            print(' desired point is likely out of reach')

        [p3x, p3y, p3z] = project_along_vector(x_command, y_command, z_command,
                                               p3x, p3y, p3z, l3)
        [p2x, p2y, p2z] = project_along_vector(p3x, p3y, p3z, p2x, p2y, p2z,
                                               l2)
        [p1x, p1y, p1z] = project_along_vector(p2x, p2y, p2z, p1x, p1y, p1z,
                                               l1)

        [p2x, p2y, p2z] = project_along_vector(p1x_o, p1y_o, p1z_o, p2x, p2y,
                                               p2z, l1)
        [p3x, p3y, p3z] = project_along_vector(p2x, p2y, p2z, p3x, p3y, p3z,
                                               l2)
        [p4x, p4y, p4z] = project_along_vector(p3x, p3y, p3z, x_command,
                                               y_command, z_command, l3)

        # check how close FABRIK position is to command position
        tolx = p4x - x_command
        toly = p4y - y_command
        tolz = p4z - z_command

        tol = np.sqrt(
            np.power(tolx, 2) + np.power(toly, 2) + np.power(tolz, 2))
        iterations = iterations + 1

        # Check if tolerance is within the specefied limit

        # Re-organize points into a big matrix for plotting elsewhere
        p_joints = np.array([[p1x, p2x, p3x, p4x], [p1y, p2y, p3y, p4y],
                             [p1z, p2z, p3z, p4z]])

        v21 = np.array([p2x - p1x, p2y - p1y, p2z - p1z])
        v32 = np.array([p3x - p2x, p3y - p2y, p3z - p2z])
        v43 = np.array([p4x - p3x, p4y - p3y, p4z - p3z])

        q2 = np.arctan2(
            (p2z - p1z),
            np.sqrt(np.power(p2x - p1x, 2) + np.power(p2y - p1y, 2)))

        q3 = -1 * angle_from_dot_product(v21, v32)
        q4 = -1 * angle_from_dot_product(v32, v43)

        q_joints = np.array([q1, q2, q3, q4])

        return q_joints
Exemplo n.º 6
0
 def __init__(self, array):
     self.states = int(len(array)**(1.0 / NEIGH_KERNEL.size))
     self.size = Rule.get_array_size(self.states)
     self.array = np.array(array)
     self.kernel = np.power(self.states, NEIGH_KERNEL)