-
Notifications
You must be signed in to change notification settings - Fork 0
/
minjerk.m
22 lines (19 loc) · 1011 Bytes
/
minjerk.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
function [T,P,Pd,Pdd] = minjerk(P1,P2,t,n)
% Given initial position P1=(x1,y1), final position P2=(x2,y2) and movement duration t,
% and the total number of desired sampled points n,
% Returns the path P over time T that satisfies minimum-jerk.
% Also returns derivatives Pd and Pdd. From the paper:
% Flash T. & Hogan N. (1985) "The coordination of arm
% movements: an experimentally confirmed mathematical model."
% Journal of Neuroscience 5(7): 1688-1703.
T = linspace(0,t,n)';
P = zeros(n,2);
Pd = zeros(n,2);
Pdd = zeros(n,2);
P(:,1) = P1(1) + ((P1(1)-P2(1))*(15*((T/t).^4) - (6*(T/t).^5) - (10*(T/t).^3)));
P(:,2) = P1(2) + ((P1(2)-P2(2))*(15*((T/t).^4) - (6*(T/t).^5) - (10*(T/t).^3)));
Pd(:,1) = (P1(1) - P2(1))*(-30*(T.^4/t^5) + 60*(T.^3/t^4) - 30*(T.^2/t^3));
Pd(:,2) = (P1(2) - P2(2))*(-30*(T.^4/t^5) + 60*(T.^3/t^4) - 30*(T.^2/t^3));
Pdd(:,1) = (P1(1) - P2(1))*(-120*(T.^3/t^5) + 180*(T.^2/t^4) - 60*(T/t^3));
Pdd(:,2) = (P1(2) - P2(2))*(-120*(T.^3/t^5) + 180*(T.^2/t^4) - 60*(T/t^3));
end