六自由度机械臂仿真设计|包括UR5|Puma|victor5|运动学正逆解介绍|MATLAB机器人工具箱|Adams仿真

六自由度机械臂仿真设计|包括UR5|Puma|victor5|运动学正逆解介绍|MATLAB机器人工具箱|Adams仿真

%% Compiled by Ezekiel according to robot modeling and control
%% 2020/9/25
clear;
clc;        %清屏
clear L     %清变量
Len_tool=0; %虚拟关节长度，
%% DH矩阵，DH建模法建立六轴机器人模型
%%          th    d   a      alpha
L(1) = Link([ 0,     1,  0,        pi/2   ], 'qlim','[-pi/4 pi/4]');    %定义连杆1，限制关节活动角度
L(2) = Link([ 0,     0,  1.5,        0      ], 'qlim','[0 pi]');        %定义连杆2，限制关节活动角度
L(3) = Link([ 0,     0,  0,        pi/2   ], 'qlim','[-pi/2 pi/2]');    %定义连杆3，限制关节活动角度
L(4) = Link([ 0,     0.5,  0,      0   ], 'qlim','[-pi pi]' );          %定义连杆4，限制关节活动角度
% L(5) = Link([ 0,     0,  0,        pi/2   ], 'qlim','[-pi/2 pi/2]' ); %定义连杆5，限制关节活动角度
% L(6) = Link([ 0,     0.114,  0,        0      ], 'qlim','[-pi pi]');  %定义连杆6，限制关节活动角度
Teaching= SerialLink(L,'name','Robot');%连接连杆
  %% 定义抓取终点（可运动学反解出二连杆相应位姿角度）
  theta_plot=[0 pi/4 pi/2-pi/4 0]
  
  %% 定义输出点的分辨率
  t_end=20;t_begin=0;
  point_num=50;
  t_step=(t_end-t_begin)/(point_num-1);
  t=t_begin:t_step:t_end;
  q0=[-pi/2 -pi/4 pi/2+pi/4 0];%重新定义初始位置矩阵（强调）
  q1=[0 -pi/4 pi/2+pi/4 0];%旋转任务
  figure(1)
  Teaching.plot(q0);  %画图,机器人处于初始位置
  %% 简单直线插补
  q_step=(theta_plot-q1)/(point_num-1);%定义步长
  q_step2=(q1-q0)/(point_num-1);
  draw_point=zeros(point_num,4);      %初始化矩阵存储点阵
  draw_point2=zeros(point_num,4);      %初始化矩阵存储点阵
  for i=1:point_num
      draw_point(i,:)=q1+(i-1)*q_step;%简单的直线插补命令
      draw_point2(i,:)=q0+(i-1)*q_step2;      %初始化矩阵存储点阵
  end
 %%
  j=1;
  while j<2
   for i=1:1:point_num
            Teaching.animate(draw_point2(i,:));%.animate绘制图形
            drawnow                           %马上作图
  end
  for i=1:1:point_num
            Teaching.animate(draw_point(i,:));%.animate绘制图形
            drawnow                           %马上作图
  end
  j=j+1;
  end