三维坐标变换习题

最近在做《先进机器人控制》作业，题目如下

 笔者答案以及python3计算如下

#方法一

import numpy as np 

F=np.mat([[0,-1,0,10],[1,0,0,20],[0,0,1,1],[0,0,0,1]])
u=np.mat([[3],[2],[2],[1]])

v=F*u
print(v[0:3])

Ry90_homo=np.mat([[0,0,1,0],[0,1,0,0],[-1,0,0,0],[0,0,0,1]])
Tx20_homo=np.mat([[1,0,0,20],[0,1,0,0],[0,0,1,0],[0,0,0,1]])
Ry90=Ry90_homo[0:3,0:3]
Tx20=Tx20_homo[0:3,3]

F1=Tx20_homo*(Ry90_homo*F)
print(F1)

R1=F1[0:3,0:3]
p1=F1[0:3,3]
p1_inverse=-R1.transpose()*p1
temp1_inverse=np.hstack((R1.transpose(),p1_inverse))
c=np.mat([[0,0,0,1]])
F1_inverse=np.vstack((temp1_inverse,c))

u1=F1_inverse*v
print(u1[0:3])

#方法二

import numpy as np 

R=np.mat([[0,-1,0],[1,0,0],[0,0,1]])
p=np.mat([[10],[20],[1]])
u=np.mat([[3],[2],[2]])

v=R*u+p
print(v)

Ry90=np.mat([[0,0,1],[0,1,0],[-1,0,0]])
Tx20=np.mat([[20],[0],[0]])

R1=Ry90*R
p1=Ry90*p+Tx20
temp=np.hstack((R1,p1))
c=np.mat([[0,0,0,1]])
F1=np.vstack((temp,c))
print(F1)

u1=R1.transpose()*(v-p1)
print(u1)