Related
I have a square 2D numpy array, A, and an array of zeros, B, with the same shape.
For every index (i, j) in A, other than the first and last rows and columns, I want to assign to B[i, j] the value of np.sum(A[i - 1:i + 2, j - 1:j + 2].
Example:
A =
array([[0, 0, 0, 0, 0],
[0, 1, 0, 1, 0],
[0, 1, 1, 0, 0],
[0, 1, 0, 1, 0],
[0, 0, 0, 0, 0])
B =
array([[0, 0, 0, 0, 0],
[0, 3, 4, 2, 0],
[0, 4, 6, 3, 0],
[0, 3, 4, 2, 0],
[0, 0, 0, 0, 0])
Is there an efficient way to do this? Or should I simply use a for loop?
There is a clever (read "borderline smartass") way to do this with np.lib.stride_tricks.as_strided. as_strided allows you to create views into your buffer that simulate windows by adding another dimension to the view. For example, if you had a 1D array like
>>> x = np.arange(10)
>>> np.lib.stride_tricks.as_strided(x, shape=(3, x.shape[0] - 2), strides=x.strides * 2)
array([[0, 1, 2, 3, 4, 5, 6, 7],
[1, 2, 3, 4, 5, 6, 7, 8],
[2, 3, 4, 5, 6, 7, 8, 9]])
Hopefully it is clear that you can just sum along axis=0 to get the sum of each size 3 window. There is no reason you couldn't extrend that to two or more dimensions. I've written the shape and index of the previous example in a way that suggests a solution:
A = np.array([[0, 0, 0, 0, 0],
[0, 1, 0, 1, 0],
[0, 1, 1, 0, 0],
[0, 1, 0, 1, 0],
[0, 0, 0, 0, 0]])
view = np.lib.stride_tricks.as_strided(A,
shape=(3, 3, A.shape[0] - 2, A.shape[1] - 2),
strides=A.strides * 2
)
B[1:-1, 1:-1] = view.sum(axis=(0, 1))
Summing along multiple axes simultaneously has been supported in np.sum since v1.7.0. For older versions of numpy, just sum repeatedly (twice) along axis=0.
Filling in the edges of B is left as an exercise for the reader (since it's not really part of the question).
As an aside, the solution here is a one-liner if you want it to be. Personally, I think anything with as_strided is already illegible enough, and doesn't need any further obfuscation. I'm not sure if a for loop is going to be bad enough performance-wise to justify this method in fact.
For future reference, here is a generic window-making function that can be used to solve this sort of problem:
def window_view(a, window=3):
"""
Create a (read-only) view into `a` that defines window dimensions.
The first ``a.ndim`` dimensions of the returned view will be sized according to `window`.
The remaining ``a.ndim`` dimensions will be the original dimensions of `a`, truncated by `window - 1`.
The result can be post-precessed by reducing the leading dimensions. For example, a multi-dimensional moving average could look something like ::
window_view(a, window).sum(axis=tuple(range(a.ndim))) / window**a.ndim
If the window size were different for each dimension (`window` were a sequence rather than a scalar), the normalization would be ``np.prod(window)`` instead of ``window**a.ndim``.
Parameters
-----------
a : array-like
The array to window into. Due to numpy dimension constraints, can not have > 16 dims.
window :
Either a scalar indicating the window size for all dimensions, or a sequence of length `a.ndim` providing one size for each dimension.
Return
------
view : numpy.ndarray
A read-only view into `a` whose leading dimensions represent the requested windows into `a`.
``view.ndim == 2 * a.ndim``.
"""
a = np.array(a, copy=False, subok=True)
window = np.array(window, copy=False, subok=False, dtype=np.int)
if window.size == 1:
window = np.full(a.ndim, window)
elif window.size == a.ndim:
window = window.ravel()
else:
raise ValueError('Number of window sizes must match number of array dimensions')
shape = np.concatenate((window, a.shape))
shape[a.ndim:] -= window - 1
strides = a.strides * 2
return np.lib.stride_tricks.as_strided(a, shake=shape, strides=strides)
I have found no 'simple' ways of doing this. But here are two ways:
Still involves a for loop
# Basically, get the sum for each location and then pad the result with 0's
B = [[np.sum(A[j-1:j+2,i-1:i+2]) for i in range(1,len(A)-1)] for j in range(1,len(A[0])-1)]
B = np.pad(B, ((1,1)), "constant", constant_values=(0))
Is longer but no for loops (this will be a lot more efficient on big arrays):
# Roll basically slides the array in the desired direction
A_right = np.roll(A, -1, 1)
A_left = np.roll(A, 1, 1)
A_top = np.roll(A, 1, 0)
A_bottom = np.roll(A, -1, 0)
A_bot_right = np.roll(A_bottom, -1, 1)
A_bot_left = np.roll(A_bottom, 1, 1)
A_top_right = np.roll(A_top, -1, 1)
A_top_left = np.roll(A_top, 1, 1)
# After doing that, you can just add all those arrays and these operations
# are handled better directly by numpy compared to when you use for loops
B = A_right + A_left + A_top + A_bottom + A_top_left + A_top_right + A_bot_left + A_bot_right + A
# You can then return the edges to 0 or whatever you like
B[0:len(B),0] = 0
B[0:len(B),len(B[0])-1] = 0
B[0,0:len(B)] = 0
B[len(B[0])-1,0:len(B)] = 0
You can just sum the 9 arrays that make up a block, each one being shifted by 1 w.r.t. the previous in either dimension. Using slice notation this can be done for the whole array A at once:
B = np.zeros_like(A)
B[1:-1, 1:-1] = sum(A[i:A.shape[0]-2+i, j:A.shape[1]-2+j]
for i in range(0, 3) for j in range(0, 3))
General version for arbitrary rectangular windows
def sliding_window_sum(a, size):
"""Compute the sum of elements of a rectangular sliding window over the input array.
Parameters
----------
a : array_like
Two-dimensional input array.
size : int or tuple of int
The size of the window in row and column dimension; if int then a quadratic window is used.
Returns
-------
array
Shape is ``(a.shape[0] - size[0] + 1, a.shape[1] - size[1] + 1)``.
"""
if isinstance(size, int):
size = (size, size)
m = a.shape[0] - size[0] + 1
n = a.shape[1] - size[1] + 1
return sum(A[i:m+i, j:n+j] for i in range(0, size[0]) for j in range(0, size[1]))
I'm attempting to convert a double summation formula into code, but can't figure out the correct matrix/vector representation of it.
The first summation is i to n, and the second is over j > i to n.
I'm guessing there is a much more efficient & pythonic way of writing this?
I resorted to nested for loops to just get it working but, as expected, it runs very slowly with a large dataset:
def wapc_denom(weights, vols):
x = []
y = []
for i, wi in enumerate(weights):
for j, wj in enumerate(weights):
if j > i:
x.append(wi * wj * vols[i] * vols[j])
y.append(np.sum(x))
return np.sum(y)
Edit:
Using guidance from smci's answer I think I have a potential solution:
def wapc_denom2(weights, vols):
return np.sum(np.tril(np.outer(weights, vols.T)**2, k=-1))
Assuming you want to count every term only once (for that you have to move the x = [] into the outer loop) one cheap way of computing the sum would be
Create mock data
weights = np.random.random(10)
vols = np.random.random(10)
Do the calculation
wv = weights * vols
result = (wv.sum()**2 - wv#wv) / 2
Check that it's the same
def wapc_denom(weights, vols):
y = []
for i, wi in enumerate(weights):
x = []
for j, wj in enumerate(weights):
if j > i:
x.append(wi * wj * vols[i] * vols[j])
y.append(np.sum(x))
return np.sum(y)
assert np.allclose(result, wapc_denom(weights, vols))
Why does it work?
What we are doing is compute the sum of the full matrix, subtract the diagonal and divide by two. This is cheap because it is easy to verify that the sum of an outer product is just the product of the summed factors.
wi * wj * vols[i] * vols[j] is a telltale. vols is another vector, so first you want to compute the vector wv = w * vols
then (wj * vols[j]) * (wi * vols[i]) = wv^T * wv is your (matrix outer product) expression; that's a column vector * a row vector. But actually you only want the sum. So I don't see a need to construct a vector y.append(np.sum(x)), you're only going to sum it anyway np.sum(y)
also the if j > i part means you only want the sum of the Lower Triangular part, and exclude the diagonal.
EDIT: the result is fully determined just from wv, I didn't think we needed the matrix to get the sum, and we didn't need the diagonal; #PaulPanzer found the most compact expression.
You can use triangulations in numpy, check np.triu and np.meshgrid. Do:
np.product(np.triu(np.meshgrid(weights,weights), 1) * np.triu(np.meshgrid(vols,vols), 1),0).sum(1).cumsum().sum()
Example:
w = np.arange(4) +1
v = np.array([1,3,2,2])
print(np.triu(np.meshgrid(w,w), k=1))
>>array([[[0, 2, 3, 4],
[0, 0, 3, 4],
[0, 0, 0, 4],
[0, 0, 0, 0]],
[[0, 1, 1, 1],
[0, 0, 2, 2],
[0, 0, 0, 3],
[0, 0, 0, 0]]])
# example of product + triu + meshgrid (your x values):
print(np.product(np.triu(np.meshgrid(w,w), 1) * np.triu(np.meshgrid(v,v), 1),0))
>>array([[ 0, 6, 6, 8],
[ 0, 0, 36, 48],
[ 0, 0, 0, 48],
[ 0, 0, 0, 0]])
print(np.product(np.triu(np.meshgrid(w,w), 1) * np.triu(np.meshgrid(v,v), 1),0).sum(1).cumsum().sum())
>> 428
print(wapc_denom(w, v))
>> 428
Currently learning about computer vision and machine learning through the free online course by stanford CS131. Came across some heavy math formulas and was wondering if anyone could explain to me how one would go on about in implementing a naive 4 nested for loops for the convolution algorithm using only knowing the image height, width and kernel height and width. I was able to come up with this solution by researching online.
image_padded = np.zeros((image.shape[0] + 2, image.shape[1] + 2))
image_padded[1:-1, 1:-1] = image
for x in range(image.shape[1]): # Loop over every pixel of the image
for y in range(image.shape[0]):
# element-wise multiplication of the kernel and the image
out[y, x] = (kernel * image_padded[y:y + 3, x:x + 3]).sum()
I was able to understand this based on some website examples using this type of algorithm however, I can't seem to grasp how a 4 nested for loops would do it. And if you could, break down the formula into something more digestible then the given mathematical equation found online.
Edit:
Just to clarify while the code snippet I left works to a certain degree I'm trying to come up with a solution that's a bit less optimized and a bit more beginner friendly such as what this code is asking:
def conv_nested(image, kernel):
"""A naive implementation of convolution filter.
This is a naive implementation of convolution using 4 nested for-loops.
This function computes convolution of an image with a kernel and outputs
the result that has the same shape as the input image.
Args:
image: numpy array of shape (Hi, Wi)
kernel: numpy array of shape (Hk, Wk)
Returns:
out: numpy array of shape (Hi, Wi)
"""
Hi, Wi = image.shape
Hk, Wk = kernel.shape
out = np.zeros((Hi, Wi))
### YOUR CODE HERE
### END YOUR CODE
return out
For this task scipy.signal.correlate2d is your friend.
Demo
I wrapped your code in a function named naive_correlation:
import numpy as np
def naive_correlation(image, kernel):
image_padded = np.zeros((image.shape[0] + 2, image.shape[1] + 2))
image_padded[1:-1, 1:-1] = image
out = np.zeros_like(image)
for x in range(image.shape[1]):image
for y in range(image.shape[0]):
out[y, x] = (kernel * image_padded[y:y + 3, x:x + 3]).sum()
return out
Notice that your snippet throws an error because out is not initialized.
In [67]: from scipy.signal import correlate2d
In [68]: img = np.array([[3, 9, 5, 9],
...: [1, 7, 4, 3],
...: [2, 1, 6, 5]])
...:
In [69]: kernel = np.array([[0, 1, 0],
...: [0, 0, 0],
...: [0, -1, 0]])
...:
In [70]: res1 = correlate2d(img, kernel, mode='same')
In [71]: res1
Out[71]:
array([[-1, -7, -4, -3],
[ 1, 8, -1, 4],
[ 1, 7, 4, 3]])
In [72]: res2 = naive_correlation(img, kernel)
In [73]: np.array_equal(res1, res2)
Out[73]: True
If you wish to perform convolution rather than correlation you could use convolve2d.
Edit
Is this what you are looking for?
def explicit_correlation(image, kernel):
hi, wi= image.shape
hk, wk = kernel.shape
image_padded = np.zeros(shape=(hi + hk - 1, wi + wk - 1))
image_padded[hk//2:-hk//2, wk//2:-wk//2] = image
out = np.zeros(shape=image.shape)
for row in range(hi):
for col in range(wi):
for i in range(hk):
for j in range(wk):
out[row, col] += image_padded[row + i, col + j]*kernel[i, j]
return out
I found a code snippet for making a circular filter using scipy and I'd like to understand how it works. I know there's a better one in skimage, but I'm interested in what's going on in this one.
from scipy.ndimage.filters import generic_filter as gf
# Define physical shape of filter mask
def circular_filter(image_data, radius):
kernel = np.zeros((2*radius+1, 2*radius+1))
y, x = np.ogrid[-radius:radius+1, -radius:radius+1]
mask = x**2 + y**2 <= radius**2
kernel[mask] = 1
filtered_image = gf(image_data, np.median, footprint = kernel)
return filtered_image
But I'm not sure I understand perfectly what's going on. In particular, what exactly do the lines
y, x = np.ogrid[-radius:radius+1, -radius:radius+1]
mask = x**2 + y**2 <= radius**2
kernel[mask] = 1
do?
I posted this as an answer to one of my previous questions, but it wasn't replied to, so I'm posting it as a new question.
Looking at your code in detail:
kernel = np.zeros((2*radius+1, 2*radius+1))
y, x = np.ogrid[-radius:radius+1, -radius:radius+1]
mask = x**2 + y**2 <= radius**2
kernel[mask] = 1
The first line:
kernel = np.zeros((2*radius+1, 2*radius+1))
creates a 2-d array of zeros, with a center point and "radius" points on either side. For radius = 2, you would get:
# __r__ +1 __r__
[ 0, 0, 0, 0, 0, ] #\
[ 0, 0, 0, 0, 0, ] #_} r
[ 0, 0, 0, 0, 0, ] # +1
[ 0, 0, 0, 0, 0, ] #\
[ 0, 0, 0, 0, 0, ] #_} r
Next, you get two arrays from the open mesh grid created by numpy.ogrid. Mesh grids are a "trick" in numpy that involves storing a "parallel" array or matrix that holds the x or y coordinate of a particular cell at the location of that cell.
For example, a y-mesh grid might look like this:
[ 0, 0, 0, 0, 0, ]
[ 1, 1, 1, 1, 1, ]
[ 2, 2, 2, 2, 2, ]
[ 3, 3, 3, 3, 3, ]
[ 4, 4, 4, 4, 4, ]
And an x-mesh grid might look like this:
[ 0, 1, 2, 3, 4, ]
[ 0, 1, 2, 3, 4, ]
[ 0, 1, 2, 3, 4, ]
[ 0, 1, 2, 3, 4, ]
[ 0, 1, 2, 3, 4, ]
If you look at them, you'll realize that Y_grid[x][y] == y and X_grid[x][y] == x which is so often useful that it has more than one numpy function to support it. ;-)
An open mesh grid is similar to a closed one, except that it only has "one dimension." That is, instead of a pair of (for example) 5x5 arrays, you get a 1x5 array and a 5x1 array. That's what ogrid does - it returns two open grids. The values are from -radius to radius+1, according to python rules (meaning the radius+1 is left out):
y, x = np.ogrid[-radius:radius+1, -radius:radius+1]
So y is a numpy array storing from e.g., -2..2 (inclusive), and x is an array from -2..2 inclusive. The next step is to build a boolean mask - that is, an array full of boolean values. As you know, when you operate on a numpy array, you get another numpy array. So involving two arrays in an expression with a constant produces another array:
mask = x**2 + y**2 <= radius**2
The value of mask is going to be a 2-color bitmap, where one color is "True" and the other color is "False." The bitmap will describe a solid circle or disk. (Because of the <= relation. Remember that x and y contain -2..2, not 0..4.)
Finally, you convert from type Boolean to int by using the masking array as an overlay on the kernel array (of zeroes), setting the zeroes to ones whenever the mask is "True":
kernel[mask] = 1
At this point, kernel looks like:
# __r__ +1 __r__
[ 0, 0, 1, 0, 0, ] #\
[ 0, 1, 1, 1, 0, ] #_} r
[ 1, 1, 1, 1, 1, ] # +1
[ 0, 1, 1, 1, 0, ] #\
[ 0, 0, 1, 0, 0, ] #_} r
I'm not familiar with SciPy but I'll give it a shot trying to explain the basic concepts.
This entire function's purpose is to alter the original image by applying a filter. This filter could do a lot of things, from changing the contrast of the image, or adding special effects, etc.
Let's go through the different lines:
kernel = np.zeros((2*radius+1, 2*radius+1))
In this line, a copy of the image data is being created, but with all the data being zeros (hence the zeros function is being used). This is so the mask can be applied later onto it.
y, x = np.ogrid[-radius:radius+1, -radius:radius+1]
This is creating what is known as a "meshgrid" or a multi-dimensional grid. This is to create the circular "mask". Just like how on a graph, x and y axes have evenly spaced scaling, the same is necessary here in the meshgrid.
The x and y variables in this case store evenly spaced values that serve as the axes' scaling.
mask = x**2 + y**2 <= radius**2
Here, a "mask" is being created. A mask will serve as the region in the image to be protected from the filter, so as to not alter any original data. Notice how x and y variables are used here in a Pythagorean inequality (important to see that it's not just a circle but a disk), just like how they would be in a mathematical sense. This will create a disk with the given radius that is now considered the mask. The mask variable now contains all coordinates (x,y) where the original data values should not be altered.
kernel[mask] = 1
This is where the mask is now applied to the copy of the image that was created earlier. Now, there is a perfect copy of the image (i.e. same dimensions) but with a disk-like "mask" that "protects" the original data from being altered. This is why all the points covered by the disk is set to 1. Also, notice how the dimensions of kernel and mask match. Both are multi-dimensional. The rest of values in the image copy are still set to zero, as was done in the first line.
filtered_image = gf(image_data, np.median, footprint = kernel)
This is final part where everything is pieced together. There is the original data stored in image_data and there is the kernel, which is the image copy with the mask applied on it indicating where the data should not be altered. Both of them are passed as parameters into the actual filter function gf (stands for generic filter) and the output is a new filtered image.
This is a core concept in image filtering and if you want to learn more about it, I suggest starting out by learning basic signal processing concepts. Signal processing courses cover the mathematics of how these concepts work, but are usually explained in really abstract mathematics because this concept can be applied to numerous different examples.
I have built a simple robotic arm using 3 RC Servos and an Arduino.
I just want to play around with it and learn something about robotics.
Currently, I am trying to compute the position of the tip of the robotic arm using the three angular positions of the servos.
"Forward kinematics" I think is the technical term for this.
Btw the tip of the arm is a pen, I thought I might try to draw something with it later on.
In the movement range of the arm I set up a Cartesian coordinate system and recorded 24 (angle => position) samples.
pastebin.com/ESqWzJJB
Now, I am trying to model this data, but I am a bit out of my depth here.
Here is my approach so far:
I use the Denavit–Hartenberg equations found on Wikipedia en.wikipedia.org/wiki/Denavit–Hartenberg_parameters.
I then try to determine the parameters using least squares optimization.
minimize(sum(norm(f(x,P)-y)^2))
I also added linear terms to the input and output of the model to compensate for possible distortions (e.g. phase-shift in the servo angle):
y = f(ax+b)*c+d
My Python code: pastebin.com/gQF72mQn
from numpy import *
from scipy.optimize import minimize
# Denavit-Hartenberg Matrix as found on Wikipedia "Denavit-Hartenberg parameters"
def DenHarMat(theta, alpha, a, d):
cos_theta = cos(theta)
sin_theta = sin(theta)
cos_alpha = cos(alpha)
sin_alpha = sin(alpha)
return array([
[cos_theta, -sin_theta*cos_alpha, sin_theta*sin_alpha, a*cos_theta],
[sin_theta, cos_theta*cos_alpha, -cos_theta*sin_alpha, a*sin_theta],
[0, sin_alpha, cos_alpha, d],
[0, 0, 0, 1],
])
def model_function(parameters, x):
# split parameter vector
scale_input, parameters = split(parameters,[3])
translate_input, parameters = split(parameters,[3])
scale_output, parameters = split(parameters,[3])
translate_output, parameters = split(parameters,[3])
p_T1, parameters = split(parameters,[3])
p_T2, parameters = split(parameters,[3])
p_T3, parameters = split(parameters,[3])
# compute linear input distortions
theta = x * scale_input + translate_input
# load Denavit-Hartenberg Matricies
T1 = DenHarMat(theta[0], p_T1[0], p_T1[1], p_T1[2])
T2 = DenHarMat(theta[1], p_T2[0], p_T2[1], p_T2[2])
T3 = DenHarMat(theta[2], p_T3[0], p_T3[1], p_T3[2])
# compute joint transformations
# y = T1 * T2 * T3 * [0 0 0 1]
y = dot(T1,dot(T2,dot(T3,array([0,0,0,1]))))
# compute linear output distortions
return y[0:3] * scale_output + translate_output
# least squares cost function
def cost_function(parameters, X, Y):
return sum(sum(square(model_function(parameters, X[i]) - Y[i])) for i in range(X.shape[0])) / X.shape[0]
# ========== main script start ===========
# load data
data = genfromtxt('data.txt', delimiter=',', dtype='float32')
X = data[:,0:3]
Y = data[:,3:6]
cost = 9999999
#try:
# parameters = genfromtxt('parameters.txt', delimiter=',', dtype='float32')
# cost = cost_function(parameters, X, Y)
#except IOError:
# pass
# random init
for i in range(100):
tmpParams = (random.rand(7*3)*2-1)*8
tmpCost = cost_function(tmpParams, X, Y)
if tmpCost < cost:
cost = tmpCost
parameters = tmpParams
print('Random Cost: ' + str(cost))
savetxt('parameters.txt', parameters, delimiter=',')
# optimization
continueOptimization = True
while continueOptimization:
res = minimize(cost_function, parameters, args=(X,Y), method='nelder-mead', options={'maxiter':100,'xtol': 1e-5})
parameters = res.x
print(res.fun)
savetxt('parameters.txt', parameters, delimiter=',')
continueOptimization = not res.success
print(res)
But it just won't work, none of my attempts have converged on a good solution.
I also tried a simple 3x4 matrix multiplication, which does not make much sense as a model, but oddly it didn't do worse than the more sophisticated model above.
I hope there is someone out there who can help.
If I understood you correctly, you are trying to solve the inverse kinematics (IK) of your robot arm. Forward kinematics (FK) is about figuring out where your end-effector is located given the joint angles. You want to find the angles that makes the end-effector reach the desired position.
In order to solve the IK problem you have to figure out the forward-kinematics of your arm.
If you are unsure about your current FK, you could use the following script to get the symbolic FK matrices for each joint (including end-effector). It also generates the Jacobian.
import numpy as np
from sympy import *
def pos(matrix):
list = [0,0,0]
list[0] = matrix[0,3]
list[1] = matrix[1,3]
list[2] = matrix[2,3]
return np.array(list).astype(float).tolist()
class KinematicChain:
def __init__(self):
self.i = 1
self.syms = []
self.types = []
self.matrices = []
self.fk = []
def add(self, type, relPos):
"""
Parameters:
type - the type of joint
relpos - the position of the joint relative to the previos one
"""
mat = self.transMatrix(type, relPos);
self.matrices.append(mat)
self.types.append(type)
if len(self.fk) == 0:
self.fk.append(eye(4)*mat)
else:
self.fk.append(simplify(self.fk[-1]*mat))
def jacobian(self):
fk = self.fk[-1]
px = fk[0,3]
py = fk[1,3]
pz = fk[2,3]
f = Matrix([px, py, pz])
if (len(self.syms) < 1):
return eye(4)
else:
x = Matrix(self.syms)
ans = f.jacobian(x)
return ans
def transMatrix(self, type, p):
if (type != "FIXED"):
s1 = "a" + str(self.i)
self.i += 1
a = symbols(s1)
self.syms.append(a)
if (type == "FIXED"):
return Matrix([
[1, 0, 0, p[0]],
[0, 1, 0, p[1]],
[0, 0, 1, p[2]],
[0, 0, 0, 1]])
elif (type == "RX"):
return Matrix([
[1, 0, 0, p[0]],
[0, cos(a), -sin(a), p[1]],
[0, sin(a), cos(a), p[2]],
[0, 0, 0, 1]])
elif (type == "RY"):
return Matrix([
[cos(a), 0, sin(a), p[0]],
[0, 1, 0, p[1]],
[-sin(a), 0, cos(a), p[2]],
[0, 0, 0, 1]])
elif (type == "RZ"):
return Matrix([
[cos(a), -sin(a), 0, p[0]],
[sin(a), cos(a), 0, p[1]],
[0, 0, 1, p[2]],
[0, 0, 0, 1]])
elif (type == "PX"):
return Matrix([
[1, 0, 0, p[0] + a],
[0, 1, 0, p[1]],
[0, 0, 1, p[2]],
[0, 0, 0, 1]])
elif (type == "PY"):
return Matrix([
[1, 0, 0, p[0]],
[0, 1, 0, p[1] + a],
[0, 0, 1, p[2]],
[0, 0, 0, 1]])
elif (type == "PZ"):
return Matrix([
[1, 0, 0, p[0]],
[0, 1, 0, p[1]],
[0, 0, 1, p[2] + a],
[0, 0, 0, 1]])
else:
return eye(4)
There are many ways of solving the IK. A good one is the Damped Least Squared method.
See: http://math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf
A simpler method is Cyclic Coordinate Decent which is quite manageable to get working on a arduino with limited matrix support. See: http://www.cs.cmu.edu/~15464-s13/assignments/assignment2/jlander_gamedev_nov98.pdf
I think what you are trying to do is some kind of "kinematics calibration": identifying the robot parameters from a set of measurement data. There are numerous classic text books discussing this topic if you really want to dig deeper, for example [Mooring et al.] "Fundamentals of manipulator calibration".
Back to your question, a lot of things can cause your parameter identification to fail converging, so mind you this is not a cookbook answer. ;)
One possible case is you have two (or more) joints with parallel axes. It is quite common to have this kind of configuration in simpler robots, for instance in SCARA, or PUMA-like mechanisms. In this case, with DH convention there are infinitely many way to choose the axis lines.
There are different approaches to cope with this, but YMMV. One thing that you can try is to use Hayati-modified DH model. This model adds one more parameter "beta" to the basic DH, to cope with singularity in parallel-axis case.
Or you can try creating your own "custom" transformation matrices to model your mechanism. For example, you can use roll-pitch-yaw (or Euler angles) to represent the rotation between joint axes, then add one length parameter to reach the next joint, etc.
Another thing that caught my attention is the _scale_output_. I think this means you can have multiple "arm length" solutions for a given data set. As an illustration, both [scale_output=1, arm_length=100] and [scale_output=100, arm_length=1] will give the same position with same joint angles. Try removing the scale_output from the model, and see if that helps.
Also you might want to try other optimization/minimization routines. I successfully used scipy.optimize.leastsq() for kinematics calibration in the past.
Hope this helps!
Seeing that your goal is to learn more about robotics, building strong fundamentals first will greatly assist you in the long run. You'll most likely want to dive into the world of transformation matrices first so you'll have something to build off of when you get to more complex topics like DH-tables and inverse kinematics.
Here are some videos that may help:
https://www.youtube.com/watch?v=xYQpeKYCfGs&list=PLJtm2YNbaY4_rQApwht0ia5r_sx3vaSxv