56 lines
1.2 KiB
Matlab
56 lines
1.2 KiB
Matlab
% Implementation of a minimum jerk motion algorithm, based on Flash &
|
|
% Hogan (1985)
|
|
% 23.8.2018, Wayne Glettig
|
|
% Path from (x0, y0) to (xT, yT) with the minimum jerk:
|
|
% (x0, y0) : Start position in x-y plane
|
|
% (xT, yT) : End position in x-y plane
|
|
% The algorithm also takes in account start and end velocities v0 & vT
|
|
% And plots the result in (x,t), (v,t) and (a,t) plots.
|
|
|
|
|
|
%% INPUT PARAMETERS:
|
|
x0=0; %Start position in x
|
|
y0=0; %Start position in y
|
|
|
|
xT=-10; %Target position in x
|
|
yT=-10; %Target position in y
|
|
|
|
t0=0; %Start time
|
|
dt=0.01; %time step
|
|
tT=1; %End time
|
|
%% CALCULATE PROFILE: STEP THROUGH PROFILE WITH GIVEN SAMPLE RATE:
|
|
%Vectors for results
|
|
T = t0:dt:tT;
|
|
X = zeros(length(T),1);
|
|
Y = zeros(length(T),1);
|
|
|
|
for (i = 1:length(T))
|
|
t = T(i);
|
|
|
|
x = x0 + (x0-xT)*(15*t^4-6*t^5-10*t^3);
|
|
y = y0 + (y0-yT)*(15*t^4-6*t^5-10*t^3);
|
|
|
|
%Write to vectors
|
|
X(i)=x;
|
|
Y(i)=y;
|
|
end
|
|
|
|
|
|
%% PLOT CURVES:
|
|
plot (X,Y,'r+');
|
|
% subplot (3,1,1);
|
|
% plot(T, X,'r+-');
|
|
% hold on;
|
|
% ylabel('x(t)[m]');
|
|
%
|
|
% subplot (3,1,2);
|
|
% plot(T, V,'g+-');
|
|
% hold on;
|
|
% ylabel('v(t)[m/s]');
|
|
%
|
|
% subplot (3,1,3);
|
|
% plot(T, A,'b+-');
|
|
% hold on;
|
|
% ylabel('a(t)[m/s^2]');
|
|
% xlabel('t[s]');
|