I would like to make a program in python, which will take a point coordinates(XYZ-ABC), for example: POINT = X 100, Y 200, Z 120, A -90, B 0, C O with respect to basis: B = X 0, Y 200, Z 0, A 0, B 0, C 0 and find the coordinates of the same point with respect to another basis: A = X 100, Y 200, Z 0, A 0, B 0, C 0. I have found a lot of information about transformations in 3D, but I don't know where to start. I also have the transformation.py library. I would need some hints on how to go about this, which steps I will have to follow in mathematical terms.
Asked
Active
Viewed 490 times
-1
-
I'm afraid your information does not make sense - what are `X Y Z A B C`? They don't look at all like any standard ways of specifying a basis. – meowgoesthedog Jul 17 '17 at 14:19
-
Maybe,It is not the proper mathematical way of representing a Basis or Frame. But in many industrial robots it is correct. Actually, it is taken literally from a KUKA robot. – Xabier Fernandez Jul 27 '17 at 03:08
3 Answers
1
Given the origin vector O=(X, Y, Z) and the rotation matrix R that you can compute from the Euler angles (caution, there are many variants), the absolute coordinates of a point with relative coordinates p=(x, y, z) are given by
P = R p + O.
With a second frame
P = R'p'+ O',
giving the equations from local coordinates in the first frame to the second
p' = R'*(P - O') = R'*(R p + O - O')
where * denotes the transpose (which is also the inverse for a rotation matrix).
0
This points are the Cartesian coordinates of a robot position. XYZ(translation) and ABC(Rz,Ry,Rx rotation) euler angles, with respect to a basis or frame. I need (I think) to find the Unit Vector Matrix of this position. This is what I have done so far:
C(b)C(a) S(c)S(b)C(a)-C(c)S(a) C(c)S(b)C(a)+S(c)S(a) x
C(b)S(a) C(c)C(a)+S(c)S(b)S(a) C(c)S(b)S(a)-S(c)C(a) y
-S(b) S(c)C(b) C(c)C(b) z
0 0 0 1
//For example point P= [X -534.884033,Y -825.747070, Z 1037.32373,
A -165.214142,B -3.16937923,C -178.672119]
I have read this question too [3D Camera coordinates to world coordinates (change of basis?), but I don't understand what I have to do. At the moment I am doing some calculation in an Excel sheet, trying to figure out what to do. Also I have to say this position is with respect to a Frame wich in turn has coordinates with respect to World coordinate system. In this case, the values of this Frame are:
Fa= [X 571.16217, Y -1168.71704, Z 372.404694000000, A -179.72329, B -0.2066, C 0.8562]
Now, if I have a second Frame Fb:
Fb= [X 0, Y -1168.71704, Z 372.404694000000, A -179.72329, B -0.2066, C 0.8562]
I know that my point P with respect to Fb should be:
Pfb =[X -1106.036,Y -822.9583, Z 1039.342,A -165.2141, B -3.169379,C -178.6721]
I know this result because I have use a program that does this calculation automatically, but I don't know how it does it.
Xabier Fernandez
- 31
- 5
0
I have come to this. Find transformation of point Pfa, that is relative to Frame A, with respect to Frame B. Pfb?? This example it is useful to transform a position or point, from one frame to another in Kuka industrial robot. Also, it will be useful, for any kind of affine transformation of basis or frame, we only have to take in consideration the order of rotation for homogeneous transformation matrix.
A = Rz
B = Ry
C = Rx
Fa_mat --> Homogeneous transformation matrix(HTM) of Frame A, relative to World CS(coordinate system).
Fb_mat --> HTM of Frame B, relative to World CS.
Pfa_mat --> HTM of point A in Frame A.
Pfb_mat --> HTM of point B in Frame B.
Pwa_mat --> HTM of point A in World CS.
Pwb_mat --> HTM of point B in World CS.
If Pwa == Pwb then:
Pwa = Fa_mat · Pfa_mat
Pwb = Fb_mat · Pfb_mat
Fa_mat · Pfa_mat = Fb_mat · Pfb_mat
Pfb_mat = Pwa · Fb_mat' (Fb_mat' is the inverse)
I have used Tait-Bryan ZYX angles for the rotation matrix, euler angles - Wikipedia. This is my python code:
# -*- coding: utf-8 -*-
"""
Created on Tue Jul 18 08:54:16 2017
@author: xabier fernandez
"""
import math
import numpy as np
def point_rotation(point_mat):
decpl = 7
sy = math.sqrt(math.pow(point_mat[0,0],2) + math.pow(point_mat[1,0],2))
singularity = sy < 1e-6
if not singularity :
A = math.atan2(point_mat[1,0], point_mat[0,0])
B = math.atan2(-point_mat[2,0], sy)
C = math.atan2(point_mat[2,1] , point_mat[2,2])
else :
A = 0
B = math.atan2(-point_mat[2,0], sy)
C = math.atan2(-point_mat[1,2], point_mat[1,1])
A = round(math.degrees(A),decpl)
B = round(math.degrees(B),decpl)
C = round(math.degrees(C),decpl)
return np.array([A,B,C])
def point_translation(point_mat):
decpl = 5
X = round(point_mat[0,3],decpl)
Y = round(point_mat[1,3],decpl)
Z = round(point_mat[2,3],decpl)
return np.array([X,Y,Z])
def point_to_mat(posX,posY,posZ,degA,degB,degC):
t=np.zeros((4,4))
radA=math.radians(degA)
radB=math.radians(degB)
radC=math.radians(degC)
cos_a=math.cos(radA)
sin_a=math.sin(radA)
cos_b=math.cos(radB)
sin_b=math.sin(radB)
cos_c=math.cos(radC)
sin_c=math.sin(radC)
t[0,0] = cos_a*cos_b
t[0,1] = -sin_a*cos_c + cos_a*sin_b*sin_c
t[0,2] = sin_a*sin_c + cos_a*sin_b*cos_c
t[1,0] = sin_a*cos_b
t[1,1] = cos_a*cos_c + sin_a*sin_b*sin_c
t[1,2] = -cos_a*sin_c + sin_a*sin_b*cos_c
t[2,0] = -sin_b
t[2,1] = cos_b*sin_c
t[2,2] = cos_b*cos_c
t[0,3] = posX
t[1,3] = posY
t[2,3] = posZ
t[3,0] = 0
t[3,1] = 0
t[3,2] = 0
t[3,3] = 1
return t
def test1():
"""
-----------------------------------
Rotational matrix 'zyx'
-----------------------------------
Fa--> Frame A relative to world c.s
Fb--> Frame B relative to world c.s
-----------------------------------
Pwa--> Point A in world c.s
Pwb--> Point B in world c.s
-----------------------------------
Pfa--> Point in frame A c.s
Pfb--> Point in frame B c.s
-----------------------------------
Pwa == Pwb
Pw = Fa x Pfa
Pw = Fb x Pfb
Pfb = Fb' x Pw
-----------------------------------
"""
frameA_mat = point_to_mat(571.162170,-1168.71704,372.404694,-179.723297,-0.206600,0.856200)
frameB_mat = point_to_mat(1493.90100, 209.460, 735.007, 179.572, -0.0880000, 0.130000)
Pfa_mat = point_to_mat(-534.884033, -825.747070,1037.32373, -165.214142, -3.16937923, -178.672119)
inverse_frameB_mat = np.linalg.inv(frameB_mat)
#--------------------------------------------------------------------------
#Point A in World coordinate system
Pwa_mat = np.dot(frameA_mat,Pfa_mat)
Pwa_Trans = point_translation(Pwa_mat)
Pwa_Rot = point_rotation(Pwa_mat)
print('\n')
print('Point A in World C.S.: ')
print(('Translation--> X = {0} , Y = {1} , Z = {2} ').format(Pwa_Trans[0],Pwa_Trans[1],Pwa_Trans[2]))
print(('Rotation(Euler angles)--> : A = {0} , B = {1} , C = {2} ').format(Pwa_Rot[0],Pwa_Rot[1],Pwa_Rot[2]))
print('\n')
#--------------------------------------------------------------------------
#Point A affine transformation
#Point A in Frame B coordinate system
Pfb_mat= np.dot(inverse_frameB_mat,Pwa_mat)
Pfb_Trans = point_translation(Pfb_mat)
Pfb_Rot = point_rotation(Pfb_mat)
print('Point A in Frame B C.S.: ')
print(('Translation--> X = {0} , Y = {1} , Z = {2} ').format(Pfb_Trans[0],Pfb_Trans[1],Pfb_Trans[2]))
print(('Rotation(Euler angles)--> : A = {0} , B = {1} , C = {2} ').format(Pfb_Rot[0],Pfb_Rot[1],Pfb_Rot[2]))
#--------------------------------------------------------------------------
#Point B in World coordinate system
Pwb_mat = np.dot(frameB_mat,Pfb_mat)
Pwb_Trans = point_translation(Pwb_mat)
Pwb_Rot = point_rotation(Pwb_mat)
print('\n')
print('Point B in World C.S.: ')
print(('Translation--> X = {0} , Y = {1} , Z = {2} ').format(Pwb_Trans[0],Pwb_Trans[1],Pwb_Trans[2]))
print(('Rotation(Euler angles)--> : A = {0} , B = {1} , C = {2} ').format(Pwb_Rot[0],Pwb_Rot[1],Pwb_Rot[2]))
print('\n')
if __name__ == "__main__":
test1()
Xabier Fernandez
- 31
- 5