I don't have much experience with Python, so please don't laugh at my coding skills. I used Fusion 360 to figure out the correct answers python needs to give me. In Fusion360, I used revolute joints to define the motion. Then, I could drive joints at specific angles, and measure where the tcp ended up. Also, you can apply joints to the tcp, and then measure the resulting angles of axes to get the answers for the inverse case. I'd really like to talk to someone who codes Fushion, b/c they have all this stuff figured out very, very well. I'm amazed that they let people use Fusion 360 for free. There's a whole lot you can do with it.
Just a quick note. This robot has a Euler wrist, which simplifies the math a great deal. It means axes 4,5 and 6 intersect at one point. I've called this point the Euler point in my code. Also, you need to pick points on the individual axes. Don't just pick any points. If you're clever, you'll notice that, in the mastering position, or when all axes are zero, you will find that points can be chosen such that the points for the machine origin, a1, a2, a3 and the Euler point are all co-planer. This simplifies the inverse a fair amount.
This code is very much under construction. I'm not sure I need to be using numpy arrays, perhaps the regular lists will work. One thing I haven't dealt with is the status and turn selection using bit maps. That's getting a little bit outside of my current mission. If you'd like help with that, I'll help as much as I can.
So, the forward function is the easy case and is straight forward. Call the forward function, and input your angles, and the position from the machine origin to the tcp is the output. I guess I should add more output to describe the orientation of the end effector.
For the inverse, there are 3 inputs. The target x,y,z in relation to the machine origin, and the orientation of the end effector. To specify an orientation, input a unit vectors (actually, I don't think they need to be unit vectors) of the target x and y axes.
So, this is where I am. I have lots of the math, but getting LinuxCNC to understand it seems to be very difficult. The limitation seems to be the G-code interpreter. It needs to pass more to the kinematics module than just xyz. I also need to send xyz for the target x and y orientations. Or, for 5 axis machining, just the one of those vectors. I imagine a kinematics module can be written somewhat similar to the python below. Even in that case, a linear move won't be correct. The kinematic module will need to change the speeds of all the motors to hold the tcp on a straight line path during a G01 move.
Code:
from pyquaternion import Quaternion
import numpy as np
#units are mm and rads
a1p=np.array([0, 0, 225.60], dtype=np.double) #x,y,z from machine origin to any point on the A1 Axis
a2p=np.array([350.00, 0, 449.40], dtype=np.double) #x,y,z a1p to any point on the A2 Axis
a3p=np.array([0, 0, 1150.00], dtype=np.double) #x,y,z a2p to any point on the A3 Axis
a4p=np.array([1200,0,-41], dtype=np.double) #x,y,z a3p to any point on the A4 Axis
a5p=np.array([0, 0, 0], dtype=np.double) #x,y,z a4p to any point on the A5 Axis
a6p=np.array([0, 0, 0], dtype=np.double) #x,y,z a5p to any point on the A6 Axis
eulerp=np.array([1200,0,-41]) #from a3p to euler point
tcp=np.array([688,0,0]) #from eulerp to tcp
#axes of rotation when all are zero
a1a=[0,0,1]
a2a=[0,-1,0]
a3a=[0,1,0]
a4a=[1,0,0]
a5a=[0,-1,0]
a6a=[1,0,0]
def forward(a1,a2,a3,a4,a5,a6):
#Quaternions
a1q=Quaternion(axis=a1a, angle=a1)
a2q=a1q*Quaternion(axis=a2a, angle=a2)
a3q=a2q*Quaternion(axis=a3a, angle=a3)
a4q=a3q*Quaternion(axis=a4a, angle=a4)
a5q=a4q*Quaternion(axis=a5a, angle=a5)
a6q=a5q*Quaternion(axis=a6a, angle=a6)
#Rotations
a2pr=a1q.rotate(a2p)
a3pr=a2q.rotate(a3p)
eulerpr=a3q.rotate(eulerp)
#a5pr=a4q.rotate(a5p)
#a6pr=a5q.rotate(a6p)
tcpr=a6q.rotate(tcp)
#position of end effector
pos=a1p+a2pr+a3pr+eulerpr+tcpr
return(pos)
def inv(target,targetx,targety):
#Find frame 6 quat
#1st quat
angle=ang((1,0,0),targetx)
axis=np.cross((1,0,0),targetx)
quat61=Quaternion(axis=axis, angle=angle)
#print(angle)
#print(axis)
#2nd quat
yrot=quat61.rotate((0,1,0))
angle=ang(yrot,targety)
axis=quat61.rotate((1,0,0))
quat62=Quaternion(axis=axis, angle=angle)
#total quat
quat6=quat62*quat61
#Find Euler point
tcpr=quat6.rotate(tcp)
eulerloc=target-tcpr
#print(eulerloc)
a1=ang((1,0,0),(eulerloc[0],eulerloc[1],0))
#Find a2
a1q=Quaternion(axis=a1a, angle=a1)
a2pr=a1q.rotate(a2p)
a,b,c=sss(a3p,eulerp,eulerloc-a1p-a2pr)
a2=b-ang((0,0,1),(eulerloc-a1p-a2pr))
#find a3
a3=np.pi/2+np.arctan(eulerp[2]/eulerp[0])-c
#find a4
a1q=Quaternion(axis=a1a, angle=a1)
a2q=a1q*Quaternion(axis=a2a, angle=a2)
a3q=a2q*Quaternion(axis=a3a, angle=a3)
a4ar=a3q.rotate(a4a)
v1=a3q.rotate((0,0,-1))
a4=projang(v1,tcpr,a4ar)
#find a5
a4q=a3q*Quaternion(axis=a4a, angle=a4)
a5ar=a4q.rotate(a5a)
v1=a4q.rotate((1,0,0))
a5=projang(v1,tcpr,a5ar)
#find a6
a5q=a4q*Quaternion(axis=a5a, angle=a5)
a6ar=a5q.rotate(a6a)
v1=a5q.rotate((0,1,0))
a6=projang(v1,targety,a6ar)
return(a1,a2,a3,a4,a5,a6)
def proj(v1,v2):
return(v1-np.dot(v1,v2)/np.linalg.norm(v2)**2*np.array(v2))
def ang(v1,v2): #angle between vectors
return(np.arccos(np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2))))
def projang(v1,v2,v3): #project v1,v2 on to plane v3, then measure angle between
proj1=proj(v1,v3)
proj2=proj(v2,v3)
angle=ang(proj1,proj2)
sign=np.sign(np.dot(np.cross(proj1,proj2),v3))
return(sign*angle)
def sss(p1,p2,p3):
a=np.linalg.norm(p1)
b=np.linalg.norm(p2)
c=np.linalg.norm(p3)
ang1=np.arccos(
(-a**2+b**2+c**2)/
(2*b*c)
)
ang2=np.arccos(
(a**2-b**2+c**2)/
(2*a*c)
)
ang3=np.arccos(
(a**2+b**2-c**2)/
(2*a*b)
)
return(ang1,ang2,ang3)
