Matlab机器人仿真（七）：机器人的基本命令操作

包含机器人：① 机器人的示教；②机器人的正解（已知各关节角度→位姿矩阵）；③机器人的逆解（通过多方法获取位姿矩阵→各关节角度）；

%%利用标准D-H法建立多轴机器人
clear;
close all;
clc;
L1 = Link('d', 5, 'a', 5, 'alpha', -pi/2,'offset',0);    %Link 类函数;offset建立初始的偏转角
L2 = Link('d', 0, 'a', 20, 'alpha', 0,'offset',0);
L3 = Link('d', 0, 'a', 5, 'alpha', -pi/2,'offset',0);
L4 = Link('d', 20, 'a', 0, 'alpha', pi/2,'offset',0);
L5 = Link('d', 0, 'a', 0, 'alpha', -pi/2,'offset',0);
L6 = Link('d', 10, 'a', 0, 'alpha', 0,   'offset',0);

L1.qlim = [-pi,pi];%利用qlim设置每个关节的旋转角度范围
robot=SerialLink([L1,L2,L3,L4,L5,L6],'name','S725');   %SerialLink 类函数

%% 一、普通机器人的示教展示

% robot.display();%展示出机器人的信息
% teach(robot);%调出示教滑块


%% 二、展示当六个关节角为000000时对应的姿态

% theta=[0 0 0 0 0 0];
% robot.plot(theta);   
% p_1=robot.fkine(theta);

%% 三、机器人的正解函数

% theta1=[pi/4,-pi/3,pi/6,pi/4,-pi/3,pi/6];
% robot.plot(theta1);
% p0=robot.fkine(theta);
% p1=robot.fkine(theta1);

%% 四、机器人的逆解
%1、已知位姿矩阵p
% Pos_x=30;Pos_y=0;Pos_z=-25;
%  p = [1 0 0 Pos_x;
%       0 1 0 Pos_y;
%       0 0 1 Pos_z;
%       0 0 0 1];%已知空间中的位姿q
% mask = [1 1 1 0 0 0];
% q=ikine(robot,p,'mask',mask);   %ikine逆解函数，根据末端位姿p，求解出关节角q
% robot.plot(q);%输出机器人模型，后面的三个角为输出时的theta姿态
% disp(q);
%%
% 2、位置描述得出位姿矩阵p
p = transl(30,0,-25)*rpy2tr(0,pi,pi/3);%得出位姿矩阵
q = robot.ikine(p);%得出关节角theta
robot.plot(q);
disp(q);
%% 五、机器人多解情况(ikine6s函数)
% Pos_x=30;Pos_y=0;Pos_z=-25;
%  p = [1 0 0 Pos_x;
%       0 1 0 Pos_y;
%       0 0 1 Pos_z;
%       0 0 0 1];%已知空间中的位姿q
% q = robot.ikine6s(p,'ru');%'l'臂向左(默认),'r'臂向右,'u'肘向上(默认),' d '肘部,'n'手腕没有翻转(默认),'f'腕翻转(旋转180度)
% robot.plot(q_0);