Изменение точки представления относительно 2 разных баз
Я хотел бы сделать программу на Python, которая будет принимать координаты точки (XYZ-ABC), например: POINT = X 100, Y 200, Z 120, A -90, B 0, CO относительно базиса:B = X 0, Y 200, Z 0, A 0, B 0, C 0 и найти координаты той же точки относительно другого базиса:A = X 100, Y 200, Z 0, A 0, B 0, C 0 Я нашел много информации о преобразованиях в 3D, но я не знаю, с чего начать. У меня также есть библиотека translation.py. Мне нужно несколько советов о том, как это сделать, какие шаги я должен буду выполнить в математических терминах.
3 ответа
Учитывая вектор происхождения O=(X, Y, Z) и матрица вращения R что вы можете вычислить из углов Эйлера (осторожно, есть много вариантов), абсолютные координаты точки с относительными координатами p=(x, y, z) даны
P = R p + O.
Со вторым кадром
P = R'p'+ O',
давая уравнения от локальных координат в первом кадре ко второму
p' = R'*(P - O') = R'*(R p + O - O')
где * обозначает транспонирование (которое также является обратным для матрицы вращения).
Я пришел к этому. Найти преобразование точки Pfa относительно кадра A относительно кадра B. Pfb?? В этом примере полезно преобразовать позицию или точку из одного кадра в другой в промышленном роботе Kuka. Кроме того, это будет полезно, для любого аффинного преобразования базиса или фрейма, мы должны только принять во внимание порядок вращения для однородной матрицы преобразования.
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)
Я использовал углы Tait-Bryan ZYX для матрицы вращения, углы Эйлера - Википедия. Это мой код Python:
# -*- 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()
Эти точки являются декартовыми координатами положения робота. Эйлеровы углы XYZ(перевод) и ABC(вращение Rz,Ry,Rx) относительно основы или рамы. Мне нужно (я думаю) найти матрицу векторов единиц этой позиции. Это то, что я сделал до сих пор:
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]
Я тоже прочитал этот вопрос [ Координаты 3D-камеры с мировыми координатами (изменение основы?), Но я не понимаю, что мне нужно делать. На данный момент я делаю некоторые расчеты в листе Excel, пытаясь понять, что делать. Также я должен сказать, что это положение относительно кадра, который в свою очередь имеет координаты относительно мировой системы координат. В этом случае значения этого кадра:
Fa= [X 571.16217, Y -1168.71704, Z 372.404694000000, A -179.72329, B -0.2066, C 0.8562]
Теперь, если у меня есть второй кадр Fb:
Fb= [X 0, Y -1168.71704, Z 372.404694000000, A -179.72329, B -0.2066, C 0.8562]
Я знаю, что моя точка P относительно Fb должна быть:
Pfb =[X -1106.036,Y -822.9583, Z 1039.342,A -165.2141, B -3.169379,C -178.6721]
Я знаю этот результат, потому что я использовал программу, которая выполняет этот расчет автоматически, но я не знаю, как он это делает.