import numpy as np
import sympy as sp
from sympy import *
init_printing()
uVars = list(symbols(', '.join([f'u{n}' for n in range(1, 3 + 1)])))
aVars = list(symbols(', '.join([f'a{n}' for n in range(1, 3 + 1)])))
lambda1, mu = symbols('lambda, mu')
U = np.array([ [0, -uVars[2], uVars[1]], [uVars[2], 0, -uVars[0]], [-uVars[1], uVars[0], 0] ])
a = np.array([ [aVars[0], 0, 0], [0, aVars[1], 0], [0, 0, aVars[2]] ])
I = np.eye(3)
L = a*lambda1 + U
preCharPoly = L - mu*I
preCharPoly_sym = sp.Matrix(preCharPoly)
factor(preCharPoly_sym.det())
The above code outputs the following polynomial:
However, I require the polynomial to be factored with respect to the variables lambda and mu as shown here:
I have been examining the documentation at https://docs.sympy.org/latest/modules/simplify/simplify.html but cannot figure out how to do what is desired. How do I specify factor() or simplify() to perform their tasks with respect to lambda and mu?
im currently working on some molecular simulations, but cant figure out why one part of my rotation matrix is nor working properly.
Here my code:
import numpy as np
import math as math
alpha = 0
beta = 0
gamma = 0
R_x_a = np.array([[1, 0, 0],[0, np.cos(alpha), -np.sin(alpha)],[0, np.sin(alpha), np.cos(alpha)]])
R_y_b = np.array([[np.cos(beta), 0, np.sin(beta)], [0, 1, 0], [-np.sin(beta), 0, np.cos(beta)]])
R_z_g = np.array([[np.cos(gamma), -np.sin(gamma), 0],[np.sin(gamma), np.cos(gamma), 0],[0, 0, 1]])
#Rot_matrix = np.dot(R_x_a, R_y_b, R_z_g)
print(R_x_a)
print(R_y_b)
print(R_z_g)
Whenever i try this code and change the alpha value, the first x-rotation matrix is working properly, but it influences the z-rotation matrix in some way.
Whenever i change the gamma value it changes it in some curious way and i dont know how to fix this problem.
I hope someone can help me.
You did not show any code of how you are applying these rotations. Possibly make sure you are using
np.matmul(rotation, vector)
otherwise your code seemed to work for me when I ran it.
Well, i dont get it.
I tried this:
import numpy as np
import math as math
pi = np.pi
steps = [0, pi/2, pi, 3*pi/2, 2*pi]
def rotation_matrix(alpha, beta, gamma):
global R_x_a
global R_y_b
global R_z_g
R_x_a = np.array([[1, 0, 0],[0, np.cos(alpha), -np.sin(alpha)],[0, np.sin(alpha), np.cos(alpha)]])
R_y_b = np.array([[np.cos(beta), 0, np.sin(beta)], [0, 1, 0], [-np.sin(beta), 0, np.cos(beta)]])
R_z_g = np.array([[np.cos(gamma), -np.sin(gamma), 0],[np.sin(gamma), np.cos(gamma), 0],[0, 0, 1]])
Rot_matrix = np.matmul(R_z_g, R_y_b, R_x_a)
for i in steps:
print(i)
rotation_matrix(i, 0, 0)
print(R_x_a[2,2], "\n")
I did this to try whether my code is doing something or not.
The list is just to iterate some fractions of pi.
Still, the code is doing nothing.
Output:
0
1.0
1.5707963267948966
1.0
3.141592653589793
1.0
4.71238898038469
1.0
6.283185307179586
1.0
Your actual code for the rotations is correct just your use of global variables resets the R_x_a to the first result of calculating it after the function is run each time try this instead.
import numpy as np
pi = np.pi
steps = [0, pi/2, pi, 3*pi/2, 2*pi]
def rotation_matrix(alpha):
return np.array([[1, 0, 0],[0, np.cos(alpha), -np.sin(alpha)],[0, np.sin(alpha), np.cos(alpha)]])
for i in steps:
print(i)
R_x_a = rotation_matrix(i)
print(R_x_a[2,2], "\n")
I have written a code that plots random walks. There are traj different random walks generated and each consists of n steps. I would like to animate their moves. How can I do that?
My code below:
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def random_walk_2D(n, traj = 1):
for i in range(traj):
skoki = np.array([[0, 1], [1, 0], [-1, 0], [0, -1]])
losy = np.random.randint(4, size = n)
temp = skoki[losy, :]
x = np.array([[0, 0]])
temp1 = np.concatenate((x, temp), axis = 0)
traj = np.cumsum(temp1, axis = 0)
plt.plot(traj[:, 0], traj[:, 1])
plt.plot(traj[-1][0], traj[-1][1], 'ro') #the last point
plt.show()
As it stands now, you generate traj in one shot. I mean that traj in traj = np.cumsum(temp1, axis = 0) already contains all the "story" from the beginning to the end. If you want to create an animation that is in "real time", you should not generate traj in one shot, but iteratively, plotting new steps as they come. What about doing:
import numpy as np
import matplotlib.pyplot as plt
def real_time_random_walk_2D_NT(
nb_steps, nb_trajs, with_dots=False, save_trajs=False, tpause=.01
):
"""
Parameters
----------
nb_steps : integer
number of steps
nb_trajs : integer
number of trajectories
save_trajs : boolean (optional)
If True, entire trajectories are saved rather than
saving only the last steps needed for plotting.
False by default.
with_dots : boolean (optional)
If True, dots representative of random-walking entities
are displayed. Has precedence over `save_trajs`.
False by default.
tpause : float (optional)
Pausing time between 2 steps. .01 secondes by default.
"""
skoki = np.array([[0, 1], [1, 0], [-1, 0], [0, -1]])
trajs = np.zeros((nb_trajs, 1, 2))
for i in range(nb_steps):
_steps = []
for j in range(nb_trajs):
traj = trajs[j,:,:]
losy = np.random.randint(4, size = 1)
temp = skoki[losy, :]
traj = np.concatenate((traj, temp), axis = 0)
traj[-1,:] += traj[-2,:]
_steps.append(traj)
if save_trajs or with_dots:
trajs = np.array(_steps)
if with_dots:
plt.cla()
plt.plot(trajs[:,i, 0].T, trajs[:,i, 1].T, 'ro') ## There are leeway in avoiding these costly transpositions
plt.plot(trajs[:,:i+1, 0].T, trajs[:,:i+1, 1].T)
else:
plt.plot(trajs[:,-1+i:i+1, 0].T, trajs[:,-1+i:i+1, 1].T)
else:
trajs = np.array(_steps)[:,-2:,:]
plt.plot(trajs[:,:, 0].T, trajs[:,:, 1].T)
plt.pause(tpause)
real_time_random_walk_2D_NT(50, 6, with_dots=True)
real_time_random_walk_2D_NT(50, 6)
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