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
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)