机械臂直线绘制 机械臂平滑运动

%%
%%逆运动学验证
clear;
clc;
%建立机器人模型
% theta d a alpha offset
L1=link（[0 0 0 0 0 ]‘modified‘）; %连杆的D-H参数
L2=link（[0 149.09 0 -pi/2 0 ]‘modified‘）;
L3=link（[0 0 431.8 0 0 ]‘modified‘）;
L4=link（[0 433.07 20.32 -pi/2 0 ]‘modified‘）;
L5=link（[0 0 0 pi/2 0 ]‘modified‘）;
L6=link（[0 0 0 -pi/2 0 ]‘modified‘）;
robot=Seriallink（[L1 L2 L3 L4 L5 L6]‘name‘‘puma560‘‘base‘  ...
transl（0 0 0.62）* trotz（0））; %连接连杆，机器人取名puma560
% robot.plot（[0pi/2000pi/2]）;%输出机器人模型，后面的六个角为输出时的theta姿态
robot.plot（[000000]）
figure（1）
% robot.teach（） 
robot.display（）; %显示D-H表
hold on
T1 = transl（5000-400）;	%起点
T2 = transl（-300-800-100）;	%终点
%ctraj 利用匀加速匀减速规划轨迹%
T = ctraj（T1T250）;
Tj = transl（T）;
%输出末端轨迹%
plot3（Tj（:1）Tj（:2）Tj（:3））;
grid on;

T=Tj;
G = [];  %用来存储新的障碍节点，用于栅格地图的更新
% N = ndims（T）;
N = size（T1）;
Q = []; %可行路径上的最终各点的关节角
c = [0 0 0 0 0 0 0 0]; %不可行点判别数组  % A为路径可行判别数，可行为1，不可行为0
t0=[0 0 0 0 0 0];
e0 = [];
temp0=0;
sum = [];
for i =1:N    %分别对各点进行碰撞检测
    t = T（i:）;
    q = transl（t）;
    TH = niyundx_change（q）;
    for n=1:8
        for m=1:6
            if TH（nm）>pi
                TH（nm）=TH（nm）-2*pi;
            elseif TH（nm）<-pi