POE指数积公式的简单matlab实现

参考现代机器人学
及机器人工程师进阶之路（八）指数积（PoE）建立机械臂模型及正运动
1.使用Robotics在matlab中建立一个简单的机械臂，方便验证后面的代码写的对不对

%Robotics 工具箱介绍： https://blog.csdn.net/weixin_43365751/article/details/100101827
%安装： https://petercorke.com/toolboxes/robotics-toolbox/
% clc
% clear 
%% 定义连杆
%L=Link([theta,d,a,alpha],standard/modified)%
%  theta        kinematic: joint angle  绕Z轴旋转角度
%  d            kinematic: link offset  绕Z轴平移距离
%  a            kinematic: link length  绕X轴平移距离
%  alpha        kinematic: link twist   绕X轴旋转角度
L1 = Link([0 0 0.1  pi/2]);  %关节2相对于关节1的参数
L2 = Link([0 0 0.2 0]);
L3 = Link([0 0 0.3 0]);
b=isrevolute(L1);  %Link 类函数，转动关节变量
robot=SerialLink([L1,L2,L3],'name','VIPER7','comment','LL');   %SerialLink 类函数
%robot.name='VIPER7';
%robot.comment='LL';

figure(2)
robot.display();  %Link 类函数，显示建立的机器人DH参数
%通过手动输入各个连杆转角，模型会自动运动到相应位置
theta=[0 0 0];
robot.plot(theta);   %展示
robot.teach(theta);       
pos=robot.fkine(theta)		%计算机器人正运动学，括号内为theta值

2.建立POE指数积的代码
步骤：
一、当机器人处于初始位置时，末端位形M
二、当机器人处于初始位置时，相对基坐标系的螺旋轴S 1 , ⋯ S n ，对应各个关节的旋转轴w和运动旋量v
v的计算公式为v=-w_h*q q为选取旋转轴上的一点的坐标 w_h为w的三维矩阵形式
三、用POE公式进行计算

L1 = 0.1; L2 = 0.2 ; L3 = 0.3;
%各轴角度
theta = [0,0,0];

%初始坐标  式(4.6)
M=[1 ,0 ,0, L1+L2+L3;
   0 ,1 ,0, 0;
   0 ,0 ,1, 0;
   0 ,0 ,0, 1];

%旋转轴 3*1
w1 = [0 ; 0; 1];
w2 = [0 ; 1; 0];
w3 = [0 ; 1; 0];

%转换成旋转矩阵的形式
w1_h = slove_m(w1);
w2_h = slove_m(w2);
w3_h = slove_m(w3);

%选取关节上的点 3*1
p1 = [0 ;0 ;0];
p2 = [L1;0 ;0];
p3 = [L1+L2;0 ;0];

%求线速度 85页【117】 v=-w*q
v1 = -w1_h*p1;
v2 = -w2_h*p2;
v3 = -w3_h*p3;

%式(3.85)
s1 = [w1_h,p1;0 ,0,0,0];
s2 = [w2_h,p2;0 ,0,0,0];
s3 = [w3_h,p3;0 ,0,0,0];

pos = esc(w1_h,v1,theta(1))*esc(w2_h,v2,theta(2))*esc(w3_h,v3,theta(3)) *M

%% 式(3.85)
function w_h = slove_m(w)
    w_h=[0, -w(3), w(2);
        w(3), 0, -w(1);
        -w(2), w(1), 0];
end

%% 命题3.25
function T = esc(w,v,theta)

    if NearZero(norm(w)) %式(3.89) 如果w=0
        T = [eye(3), v*theta; 0, 0, 0, 1];
    else  %式(3.88)
        T = [eye(3) + sin(theta) * w + (1 - cos(theta)) * w * w , ...
             (eye(3) * theta + (1 - cos(theta)) * w ...
              + (theta - sin(theta)) * w * w)* v;
             0, 0, 0, 1];
    end
end

%% 
function judge = NearZero(near)
    judge = norm(near) < 1e-6;
end

3.与Robotics 的计算结果进行对比
当 theta = [pi/4,pi/4,0] 时
POE的结果如下

pos =

    0.5000   -0.7071    0.5000    0.3207
    0.5000    0.7071    0.5000    0.3207
   -0.7071         0    0.7071   -0.3536
         0         0         0    1.0000

Robotics 的结果如下

x y的坐标差不多，不知道为啥z方向坐标相反了。