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
Related
I am trying to generate a diagonal matrix using a linear regression coefficient. First I generated an empty matrix. Then I extract the coefficient from the regression model. Here's my code:
P = np.zeros((ncol, ncol), dtype = int)
intercep = np.zeros((1, ncol), dtype = int)
my_pls = PLSRegression(n_components = ncomp, scale=False)
model = my_pls.fit(x, y)
#extract pls coeffeicient:
coef = model.coef_
intercep = model.y_mean_ - (model.x_mean_.dot(coef))
P[(i-k):(i+k), i-k] = np.diag(coef[0:ncol])
But I got zero matrices after running the code. Can anyone please help me out with how to get the diagonal matrix from the regression coefficient?
Not sure why you need to declare P.
You can get diagonal matrix with zeros directly from the 1D list/vector using numpy.diag
x=[3,5,6,7]
numpy.diag(x)
Output:
array([[3, 0, 0, 0],
[0, 5, 0, 0],
[0, 0, 6, 0],
[0, 0, 0, 7]])
For your case, try P=np.diag(coef)
I am getting a very strange error and I can't really understand why.
I can successfully import a module named "filterpy", but when I run the code I get the error: module 'filterpy' has no attribute 'kalman'
Fun fact, the spyder editor tells me that such module exist, in fact it allows me to use even self-completion.
Am I missing something?
Thanks,
Gabriele
The code is below, the library can be installed with pip
import numpy as np
import filterpy as fp
def fx(x, dt):
# state transition function - predict next state based
# on constant velocity model x = vt + x_0
F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]], dtype=float)
return np.dot(F, x)
def hx(x):
# measurement function - convert state into a measurement
# where measurements are [x_pos, y_pos]
return np.array([x[0], x[2]])
dt = 0.1
# create sigma points to use in the filter. This is standard for Gaussian processes
points = fp.kalman.MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1)
kf = fp.kalman.UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([-1., 1., -1., 1]) # initial state
kf.P *= 0.2 # initial uncertainty
z_std = 0.1
kf.R = np.diag([z_std**2, z_std**2]) # 1 standard
kf.Q = fp.common.Q_discrete_white_noise(dim=2, dt=dt, var=0.01**2, block_size=2)
zs = [[i+np.random.randn()*z_std, i+np.random.randn()*z_std] for i in range(50)] # measurements
for z in zs:
kf.predict()
kf.update(z)
print(kf.x, 'log-likelihood', kf.log_likelihood)
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
I am trying to find the minimum distance from a point (x0,y0,z0) to a line joined by (x1,y1,z1) and (x2,y2,z2) using numpy or anything in python. Unfortunately, all i can find on the net is related to 2d spaces and i am fairly new to python. Any help will be appreciated. Thanks in advance!
StackOverflow doesn't support Latex, so I'm going to gloss over some of the math. One solution comes from the idea that if your line spans the points p and q, then every point on that line can be represented as t*(p-q)+q for some real-valued t. You then want to minimize the distance between your given point r and any point on that line, and distance is conveniently a function of the single variable t, so standard calculus tricks work fine. Consider the following example, which calculates the minimum distance between r and the line spanned by p and q. By hand, we know the answer should be 1.
import numpy as np
p = np.array([0, 0, 0])
q = np.array([0, 0, 1])
r = np.array([0, 1, 1])
def t(p, q, r):
x = p-q
return np.dot(r-q, x)/np.dot(x, x)
def d(p, q, r):
return np.linalg.norm(t(p, q, r)*(p-q)+q-r)
print(d(p, q, r))
# Prints 1.0
This works fine in any number of dimensions, including 2, 3, and a billion. The only real constraint is that p and q have to be distinct points so that there is a unique line between them.
I broke the code down in the above example in order to show the two distinct steps arising from the way I thought about it mathematically (finding t and then computing the distance). That isn't necessarily the most efficient approach, and it certainly isn't if you want to know the minimum distance for a wide variety of points and the same line -- doubly so if the number of dimensions is small. For a more efficient approach, consider the following:
import numpy as np
p = np.array([0, 0, 0]) # p and q can have shape (n,) for any
q = np.array([0, 0, 1]) # n>0, and rs can have shape (m,n)
rs = np.array([ # for any m,n>0.
[0, 1, 1],
[1, 0, 1],
[1, 1, 1],
[0, 2, 1],
])
def d(p, q, rs):
x = p-q
return np.linalg.norm(
np.outer(np.dot(rs-q, x)/np.dot(x, x), x)+q-rs,
axis=1)
print(d(p, q, rs))
# Prints array([1. , 1. , 1.41421356, 2. ])
There may well be some simplifications I'm missing or other things that could speed that up, but it should be a good start at least.
This duplicates #Hans Musgrave solution, but imagine we know nothing of
'standard calculus tricks' that 'work fine' and also very bad at linear algebra.
All we know is:
how to calculate a distance between two points
a point on line can be represented as a function of two points and a paramater
we know find a function minimum
(lists are not friends with code blocks)
def distance(a, b):
"""Calculate a distance between two points."""
return np.linalg.norm(a-b)
def line_to_point_distance(p, q, r):
"""Calculate a distance between point r and a line crossing p and q."""
def foo(t: float):
# x is point on line, depends on t
x = t * (p-q) + q
# we return a distance, which also depends on t
return distance(x, r)
# which t minimizes distance?
t0 = sci.optimize.minimize(foo, 0.1).x[0]
return foo(t0)
# in this example the distance is 5
p = np.array([0, 0, 0])
q = np.array([2, 0, 0])
r = np.array([1, 5, 0])
assert abs(line_to_point_distance(p, q, r) - 5) < 0.00001
You should not use this method for real calculations, because it uses
approximations wher eyou have a closed form solution, but maybe it helpful
to reveal some logic begind the neighbouring answer.
For the Code below, I am wondering how to make a circular kernel instead of a rectangular one. I am currently looking at something circular, and I want to find the BGR average values for it. By adjusting my kernel, my data will be more accurate.
for center in c_1:
b = img2[center[0]-4: center[0]+5, center[1]-4: center[1]+5, 0]
g = img2[center[0]-4: center[0]+5, center[1]-4: center[1]+5, 1]
r = img2[center[0]-4: center[0]+5, center[1]-4: center[1]+5, 2]
From: https://docs.opencv.org/3.0-beta/doc/py_tutorials/py_imgproc/py_morphological_ops/py_morphological_ops.html
We manually created a structuring elements in the previous examples with help of Numpy. It is rectangular shape. But in some cases, you may need elliptical/circular shaped kernels. So for this purpose, OpenCV has a function, cv2.getStructuringElement(). You just pass the shape and size of the kernel, you get the desired kernel.
# Elliptical Kernel
>>> cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(5,5))
array([[0, 0, 1, 0, 0],
[1, 1, 1, 1, 1],
[1, 1, 1, 1, 1],
[1, 1, 1, 1, 1],
[0, 0, 1, 0, 0]], dtype=uint8)
Get the circle region when given the center, you could try the following function:
def circleAverage(center, r = 4):
"""
"""
for i in range(center[0]-r, center[0]+r):
for j in range(center[1]-r, center[1] + r):
if (center[0] - i) ** 2 + (center[1] - j) ** 2 <= r**2:
// do your computation here.
Hope this helps you.
Came here to find how to make a circular (symmetric) kernel. Ended up with my own implementation.
import numpy as np
def get_circular_kernel(diameter):
mid = (diameter - 1) / 2
distances = np.indices((diameter, diameter)) - np.array([mid, mid])[:, None, None]
kernel = ((np.linalg.norm(distances, axis=0) - mid) <= 0).astype(int)
return kernel
Note that for low diameters, behavior is perhaps unexpected. Variable mid when used for the second time can for example be replaced by diameter / 2.
I've implemented it in a following way:
r = 16
kernel = np.fromfunction(lambda x, y: ((x-r)**2 + (y-r)**2 <= r**2)*1, (2*r+1, 2*r+1), dtype=int).astype(np.uint8)
Extra type conversion is needed to avoid overflow