Question
please convert this coding into flowchart % This is for planning the linear trajectory with parabolic blends for a % RRR planar robotic arm %
please convert this coding into flowchart
% This is for planning the linear trajectory with parabolic blends for a
% RRR planar robotic arm
% Inputs:
% - Goal matrix TQ (via points with designated time of arrival)
% - parabolic time interval tp (default 0.5s for all)
% - Cartesian or Joint? CJ (usually plan trajectory in cartesian)
% - Link Info l1 l2 l3
%% Settings
addpath(genpath(pwd));
% Goal matrix
% Link length
l1 = 5;
l2 = 3;
l3 = 1;
% Goal matrix
% t x y theta
TQ = [ 0 -4 0 120 ;
2 -5 5 45 ;
4 2 3 30 ;
9 5 -3 0 ];
tp = 0.5; %parabolic blends time (all 0.5 in this case)
CJ = 0; %Cartesian or Joint? 0: Cart; 1:Joint
%end of INPUTS
% LineWidth and pOintWidth
LW = 4;
OW = 2;
Tint = 0.05; %time interval for each frame
P = tp*ones(size(TQ,1),1); %parabolic blends time (all 0.5 in this case)
G = [TQ P]; %overall GOAL matrix
%% video and animation setting
%animation?
ani = 1;
%video recording
videoSave = 0;
videoSave = videoSave*ani;
if videoSave == 1
videoName = 'TEST';
AVI1 =VideoWriter(videoName,'MPEG-4');
open(AVI1);
end
%% STEP 0: If joint space, perform Inverse Kinematics first
if CJ == 1
JA = InverseKin(TQ(:,2),TQ(:,3),TQ(:,4),l1,l2);
G(:,2:4) = JA;
end
%% STEP 1: find out the velocities and acc. then get all the points in between
[TT, QQ] = genParaPoints(G,Tint); %will display the velocities and accelerations of each section
N = size(QQ,1); %number of points
if CJ == 1 %if joint space
QQ(:,1:end-1) = QQ(:,1:end-1)*180/pi; %to degrees
G(:,2:end-1) = G(:,2:end-1)*180/pi; %to degrees
end
%% STEP 2: plot the trajectories of each DOF
TS = DOFpointPlot(TT,QQ,G);
%Cartesian Space Labels
if CJ == 0
subplot(1,3,1);ylabel('X(t)');
subplot(1,3,2);ylabel('Y(t)');
subplot(1,3,3);ylabel('\theta(t)');
else
subplot(1,3,1);ylabel('\theta_1(t)');
subplot(1,3,2);ylabel('\theta_2(t)');
subplot(1,3,3);ylabel('\theta_3(t)');
end
%% STEP 3: plot the trajectories on Cartesian Space
figure(2);
%set(2, 'Position', [635 200 560 420]);
plot(0,0,'ko','MarkerSize',10);hold on;
axis equal;grid on;grid minor;
xlim([-(l1+l2) (l1+l2)]);ylim([-(l1+l2) (l1+l2)]);
xlabel('X');ylabel('Y');
set(gca,'fontsize',25);
%Plot the Workspace
d = linspace(0,2*pi);
plot( (l1-l2)*cos(d), (l1-l2)*sin(d), 'm');
plot( (l1+l2)*cos(d), (l1+l2)*sin(d), 'm');
color = 0;
TJ = zeros(N,2);
if CJ == 0
TJ = QQ(:,1:2);
else
TJ(:,1) = l1*cos(QQ(:,1)*pi/180)+l2*cos((QQ(:,1) + QQ(:,2))*pi/180);
TJ(:,2) = l1*sin(QQ(:,1)*pi/180)+l2*sin((QQ(:,1) + QQ(:,2))*pi/180);
end
%% Plot the trajectory: Green for parabolic region, Blue for linear region
for j = 1:length(TS)-1
if color == 0
plot(TJ(TS(j):TS(j+1),1),TJ(TS(j):TS(j+1),2),'g','LineWidth',LW);hold on;
else
plot(TJ(TS(j):TS(j+1),1),TJ(TS(j):TS(j+1),2),'color',[0 0 0.7],'LineWidth',LW);hold on;
end
color = 1-color;
end
%Plot the via points
plot(TQ(:,2),TQ(:,3),'r.','MarkerSize',25);
clear color d;
%% Plot the LINKS by finding all angles of each axes using inverse kinematics
if CJ == 0
JA = InverseKin(QQ(:,1),QQ(:,2),QQ(:,3),l1,l2);
else
JA = QQ(:,1:3)*pi/180;
end
J1 = JA(:,1);
J2 = JA(:,1)+JA(:,2);
J3 = JA(:,1)+JA(:,2)+JA(:,3);
% the links positions
XX = [ zeros(N,1) l1*cos(J1) l1*cos(J1)+l2*cos(J2) l1*cos(J1)+l2*cos(J2)+l3*cos(J3) ];
YY = [ zeros(N,1) l1*sin(J1) l1*sin(J1)+l2*sin(J2) l1*sin(J1)+l2*sin(J2)+l3*sin(J3) ];
%% Animation
if ani == 0
plot(XX(1,:),YY(1,:),'k','linewidth',LW);
else
for i = 1:N
p1 = plot(XX(i,:),YY(i,:),'k','linewidth',LW);
if videoSave == 1
frame = getframe(fig);
writeVideo(AVI1,frame);
end
pause(0.1);
if i
delete(p1);
end
end
end
if videoSave==1
close(AVI1);
end
hold off;
Step by Step Solution
There are 3 Steps involved in it
Step: 1
Get Instant Access to Expert-Tailored Solutions
See step-by-step solutions with expert insights and AI powered tools for academic success
Step: 2
Step: 3
Ace Your Homework with AI
Get the answers you need in no time with our AI-driven, step-by-step assistance
Get Started