最近在做《先进机器人控制》作业,题目如下
笔者答案以及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)