Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- 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)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement