看《机器人导论》中关于齐次变换的内容中，发现饶轴旋转的时候，分为两种情况。一、饶经过原点的轴进行旋转；二、饶不经过原点的旋转轴进行旋转。其实这两种情况下，采用旋转向量 和 旋转矩阵的相互转换的几种方法 这里面的方法都是可以求到旋转矩阵的，但是这两种旋转方式中的变换矩阵中的位置矩阵是不一样的。饶经过原点的轴进行旋转的变换矩阵，由于原点的位置没有发生改变，所以变换矩阵中的位置矩阵是零，而第二种方法中的由于旋转轴不在原点，由此变换矩阵中的位置矩阵不为零。
可以参考这里的描述[绕任意轴旋转]，这里求出的是4*4的变换矩阵(https://www.cnblogs.com/graphics/archive/2012/08/10/2627458.html)
[绕任意轴旋转]，这里求出的是3*3的变换矩阵(https://blog.csdn.net/chao56789/article/details/48138519)
function testtest()
%% 起始位姿矩阵
T1= [ -0.0008 , -0.0005 , 0.0004 , 0.2113,
0.0004, -0.0009 , -0.0002 , 0.0202,
0.0004 , -0.0000 , 0.0009 , 1.2244,
0 , 0 , 0 , 0.0010];
T= 1.0e+03 *T1;
pi=3.1415926;
qq_1= - pi/6;
p1=[0.8;0.6;0.5];
p2=[1;2;3];
PP=p2-p1;
PP=PP/sqrt( PP(1)^2 + PP(2)^2 + PP(3)^2 );
w_1=PP; %每个关节转轴在全局坐标系中的方向
r_1=p2; %旋量中一点在全局坐标系中的位置
c_1=[cross(r_1,w_1);w_1];
R_1 = C_poe(c_1,qq_1);
R_3=zeros(3,3);
R_3(1,1)=PP(1)^2 *(1 - cos(qq_1)) + cos(qq_1);
R_3(1,2)=PP(1)*PP(2) *(1 - cos(qq_1)) - PP(3)*sin(qq_1);
R_3(1,3)=PP(1)*PP(3) *(1 - cos(qq_1)) + PP(2)*sin(qq_1);
R_3(2,1)=PP(1)*PP(2) *(1 - cos(qq_1)) + PP(3)*sin(qq_1);
R_3(2,2)=PP(2)^2 *(1 - cos(qq_1)) + cos(qq_1);
R_3(2,3)=PP(2)*PP(3) *(1 - cos(qq_1)) - PP(1)*sin(qq_1);
R_3(3,1)=PP(1)*PP(3) *(1 - cos(qq_1)) - PP(2)*sin(qq_1);
R_3(3,2)=PP(2)*PP(3) *(1 - cos(qq_1)) + PP(1)*sin(qq_1);
R_3(3,3)=PP(3)^2 *(1 - cos(qq_1)) + cos(qq_1);
T_2_1=[eye(3),p2; 0 0 0 1];
T_2=[R_3,zeros(3,1);0 0 0 1];
T_2_2=[eye(3),-p2; 0 0 0 1];
T_2=T_2_1*T_2*T_2_2;
%% 基于旋量求得的变换矩阵
disp(R_1)
%% 基于《机器人导论》中介绍的方法求得的变换矩阵
disp(T_2)
disp(R_1*T)
disp(T_2*T)
%% 从下面结果可以看出两者是一致的
0.8667 0.4397 -0.2356 -0.0394
-0.4306 0.8979 0.0917 0.3600
0.2518 0.0220 0.9675 -0.1984
0 0 0 1.0000
0.8667 0.4397 -0.2356 -0.0394
-0.4306 0.8979 0.0917 0.3600
0.2518 0.0220 0.9675 -0.1984
0 0 0 1.0000
1.0e+03 *
-0.0006 -0.0008 0.0000 -0.0965
0.0007 -0.0006 -0.0003 0.0397
0.0002 -0.0001 0.0010 1.2381
0 0 0 0.0010
1.0e+03 *
-0.0006 -0.0008 0.0000 -0.0965
0.0007 -0.0006 -0.0003 0.0397
0.0002 -0.0001 0.0010 1.2381
0 0 0 0.0010