trplot()的使用

trplot_3D.m，trplot()用法

%% 补充内容，在python里有类似的功能

% Matplotlib.pyplot.triplot()
%% 以下是trplot2()和trplot()的用法
%https://zhuanlan.zhihu.com/p/163266891
clear all;
clc;
hold on;
%% 2D下的test
%T_2d = SE2(1,2,30*pi/180)
%T_2d = SE2(3,4,pi/3)
%T_2d = SE2(3,4,60,'deg')
%T_2d = transl2(3,4)
%R = rot2(pi/3);
%R = rot2(0)
%% 3D下把Base显示出来
% Base为默认{1,0,0;0,1,0;0,0,1};实际没显示出来，可以不显示出来
% 以下只是把base显示出来
R_3d_Base_z = rotz(0);%rotz(θ)，θ的单位为弧度
R_3d_Base_y = roty(0);
R_3d_Base_x = rotx(0); 
%以ZYX或者XYZ显示出来，有没有区别？？？
R_3d_Base = R_3d_Base_z*R_3d_Base_y*R_3d_Base_x;
T_3d_Base = [0,0,0];
% HTM = Homogeneous transformation matrix
HTM_Base = SE3(R_3d_Base,T_3d_Base);
%trplot(HTM_Base) %方式一
%trplot(HTM_Base,'frame','Base Frame') %方式二
%trplot(HTM_Base,'frame','Base Frame','rgb') %方式三，更改坐标轴颜色
%trplot(HTM_Base,'frame','Base Frame','arrow') %方式四
%trplot(HTM_Base,'frame','Base Frame','arrow','rgb') %方式五
%trplot(HTM_Base,'labels','XYZ','arrow','rgb') %'labels'与'frame'一起用冲突
%trplot(HTM_Base,'arrow','rgb','frame','Base Frame','labels','XYZ') %错误用法，先'frame'后'labels'
%trplot(HTM_Base,'labels','XYZ','arrow','rgb','frame','Base Frame','rgb') %错误使用；结合方拾三，最后的'rgb'无效；
%{
trplot(HTM_Base,'frame','Base Frame','arrow','rgb') 错误使用
trplot(HTM_Base,'labels','XYZ','arrow','rgb')
trplot(HTM_Base,'frame','Base Frame','rgb')
%}
%trplot(HTM_Base,'labels','XYZ','frame','Base Frame','thick',5) %坐标轴线粗细
%trplot(HTM_Base, 'frame','Base
%Frame','text_opts',{'FontSize',10,'FontWeight','bold'}) %text_opts轴标签，FontSize字体大小，字体粗细
%trplot(HTM_Base,'rviz','length',100) %'length',需要这样用，不能搭配，'frame'
%trplot(HTM_Base,'rviz','length',100,'axis',[-700 700 -700 700 700 700],'anaglyph','rc','framelabel','Base','labels','XYZ')
trplot(HTM_Base,'labels','XYZ','arrow','rgb','frame','Base Frame') %'labels'与'frame'有先后顺序，先'labels'后'frame'，否则'frame'产生一个轴标签，'labels'再产生
% 以上，可以直接使用trplot()来代替
% trplot()生成的就是origin在[0,0,0]的orthonomal coordinate frame{1,0,0;0,1,0;0,0,1}
%% 把Base的Rotation Matrix转化为ZYX的Euler，没用途，只是演示Rotation Matrix转化为ZYX的原理
%旋转矩阵转欧拉角
%tr2rpy('ZYX',R_3d_Base) 错误
BaseEuler_x = atan2(R_3d_Base(3,2),R_3d_Base(3,3));
BaseEuler_y = atan2(-R_3d_Base(3,1),sqrt(R_3d_Base(3,2)^2 R_3d_Base(3,3)^2));
BaseEuler_z = atan2(R_3d_Base(2,1),R_3d_Base(1,1));
%% 把FlangeTool显示出来
R_3d_FlangeTool_z = rotz(-pi/2);
R_3d_FlangeTool_y = roty(0);
R_3d_FlangeTool_x = rotx(-pi);
R_3d_FlangeTool = R_3d_FlangeTool_z*R_3d_FlangeTool_y*R_3d_FlangeTool_x;
tr2rpy(R_3d_FlangeTool)
T_3d_FlangeTool = [T_3d_Base(1) 2,T_3d_Base(2) 2,T_3d_Base(3) 2];
% HTM = Homogeneous transformation matrix
HTM_FlangeTool = SE3(R_3d_FlangeTool,T_3d_FlangeTool);
%trplot(HTM_FlangeTool,'color','r')
trplot(HTM_FlangeTool,'frame','Tool Frame','color','r')
%% 结束部分
axis equal;
hold off;