% 逆运动学+六自由度机械臂五次多项式轨迹规划clear; clc; close all;%% 1.docxVIP

%逆运动学+六自由度机械臂五次多项式轨迹规划

clear;clc;closeall;

%%1.D-H建模

L1=Link([0,0.15,0,-pi/2]);

L2=Link([0,0,0.3,0]);

L3=Link([0,0,0.25,0]);

L4=Link([0,0.1,0,-pi/2]);

L5=Link([0,0,0,pi/2]);

L6=Link([0,0.08,0,0]);

arm=SerialLink([L1L2L3L4L5L6],name,6DOF机械臂);

%%2.设定末端笛卡尔起点、终点位姿

T0=transl(0.3,0.2,0.25)*trotx(pi/2);

Tf=transl(0.45,-0.1,0.35)*troty(pi/3);

%%3.逆运动学求解（核心）

q0=arm.ikine(T0,mask,[111111]);

qf=arm.ikine(Tf,mask,[111111]);

%%4.五次多项式轨迹规划jtraj内部就是五次多项式

t_total=5;

dt=0.05;

t=0:dt:t_total;

[q,qd,qdd]=jtraj(q0,qf,t);

%%5.正运