《第2章 位置与姿态描述》代码

最近在学习《视觉伺服/机器人学、机器视觉与控制》，发现书中的代码运行不通顺，原因可能是matlab升级后，部分函数的参数变化了。所以需要记录错误的代码和正确的代码。

第一处：

为了使上述推导更形象具体，下面我们将使用MATLAB工具箱展示一些具体数值化的例子。首先用函数se2创建一个齐次变换：

错误代码

T1=se2(1,2,30*pi/180)

报错提示：

错误使用 matlabshared.spatialmath.internal.SE2Base
Invalid number of arguments. To create an se2, specify 2 or fewer arguments.

出错 se2 (第 69 行)
            [email protected](varargin{:});

解决办法：

rotation = [cosd(30), -sind(30); sind(30), cosd(30)]; % 旋转矩阵 (30°)
translation = [1, 2]; % 平移向量 (1, 2)
T1 = se2(rotation, translation); % 创建 SE(2) 变换

第二处：

错误代码：

axis([0 5 0 5]); 
trplot2(T1,'frame','1','color','b')

错误提示：

Unable to perform assignment because value of type 'se2' is not convertible to 'double'.

出错 transl (第 84 行)
            t1(1:3,4,:) = x';

出错 trplot2 (第 96 行)
        if all(size(T) == [3 3]) || norm(transl(T)) < eps

原因:
    无法从 se2 转换为 double。

 SE2无法绘图，还是手动创建矩阵才行。

 正确代码：

%创建第1个旋转坐标系，旋转30度，x轴平移1，y轴平移2
rotation1 = [cosd(30), -sind(30); sind(30), cosd(30)]; % 旋转矩阵 (30°)
translation1 = [1; 2]; % 列向量形式
T1_matrix = [rotation1, translation1; 0 0 1]; % 手动创建 3×3 变换矩阵
trplot2(T1_matrix, 'frame', '1', 'color', 'b'); % 现在 T1_matrix 是 double 类型

%创建第2个旋转坐标系，旋转0度，x轴平移2，y轴平移1
hold on;
rotation2 = [cosd(0), -sind(0); sind(0), cosd(0)]; % 旋转矩阵 (0°)
translation2 = [2; 1]; % 列向量形式
T2_matrix = [rotation2, translation2; 0 0 1]; % 手动创建 3×3 变换矩阵
trplot2(T2_matrix, 'frame', '2', 'color', 'r'); % 现在 T2_matrix 是 double 类型

%创建第3个旋转坐标系
T3_matrix= T1_matrix*T2_matrix;
trplot2(T3_matrix, 'frame', '3', 'color', 'g'); % 现在 T3_matrix 是 double 类型

%创建第4个旋转坐标系
T4_matrix= T2_matrix*T1_matrix;
trplot2(T4_matrix, 'frame', '4', 'color', 'c'); % 现在 T4_matrix 是 double 类型

P=[3;2];
P1=inv(T1_matrix)*[P;1];
P2=h2e(inv(T1_matrix)*e2h(P));
P3=homtrans(inv(T1_matrix),P);
P4=homtrans(inv(T2_matrix),P);
plot_point(P,'*');
%P1好像不能绘制，P2和P3重合了
%   plot_point(P1,'*');
plot_point(P2,'*');
plot_point(P3,'*');
plot_point(P4,'*');
axis equal;  % 使得横纵坐标轴的单位长度相等
axis([-2 5 -2 5]);