-
Notifications
You must be signed in to change notification settings - Fork 13
/
SplineGenerationMultipleQs.asv
44 lines (40 loc) · 1.59 KB
/
SplineGenerationMultipleQs.asv
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
clc
clear all
%% Setting up the spline
init_time = [0,2,7,12]'; % Initial time vector
q1 = [0, 2, 12, 5]'; % Initial points vector (Joint 1)
q2 = [0, 3, 15, 13]'; % Initial points vector (Joint 2)
q3 = [0, 5, 6, 1]'; % Initial points vector (Joint 3)
q = [q1 q2, q3]; % Initial points matrix
qdot_b1 = [0, 0]'; % Spline boundary velocities (Joint 1)
qdot_b2 = [0, 0]'; % Spline boundary velocities (Joint 2)
qdot_b3 = [0, 0]'; % Spline boundary velocities (Joint 3)
qdot_b = [qdot_b1, qdot_b2, qdot_b3]; % Spline boundary velocities
qdot_max = [5, 7, 3]'; % Maximum velocity that can be reached
h0 = diff(init_time); % Knots vector
w = max(abs(q(2:end,:) - q(1:end-1,:))./(repmat(qdot_max', [length(q)-1 1])))';
%% Setting up the spline knots optimization
options = optimoptions(@fmincon,'Algorithm','sqp');
[h,fval] = fmincon(@objfun, h0, [], [], [], [], w, h0,...
@(h)confuneqMultipleQ(h, q, qdot_b, qdot_max), options);
TC = [0,2,7,12]';
% h = diff(TC);
% q = runge(TC);
q = [0;2;12;5];
qdot_b = [0, 1]';
[mLower, mUpper, s0, s1, s2, s3] = cubic_spline(h, q, qdot_b);
plot_points = 1000;
% tt = linspace(-7,7,plot_points);
% qq = runge(tt);
% plot(tt,qq,'g');
hold on;
TC = zeros(length(h)+1, 1);
TC(1) = 0;
for i=2:length(h)+1
TC(i) =TC(i-1) + h(i-1);
end
plot(TC, q, 'o');
hold on
TC
plot_cubic_spline(TC,s0,s1,s2,s3);
% You can see it in action bq running the following at the Matlab prompt