Commit 2a30f05d by Jakob Gabriel

misc.ss.setPointChange: now also calculates multiple polynomes at once.

parent b6c78d30
 ... ... @@ -6,42 +6,53 @@ function [f, a] = setPointChange(phi0, phi1, t0, t1, n, optArgs) % d^k / dt^k f(t)|_{t=t0,t1} = 0, k = 1, 2, ..., n % The name of the variable can be set with "var" as name value pair. arguments phi0 (1,1) double; phi1 (1,1) double; phi0 (:,1) double; phi1 (:,1) double; t0 (1,1) double; t1 (1,1) double; n (1,1) double; optArgs.var = sym( "t" ); end assert(numel(phi0) == numel(phi1), "phi0 and phi1 must have same number of elements") if ~isscalar(phi0) f = cell(size(phi0)); a = cell(size(phi0)); optArgsNameValue = misc.struct2namevaluepair(optArgs); for it = 1 : numel(phi0) [f{it}, a{it}] = misc.ss.setPointChange(phi0(it), phi1(it), t0, t1, n, optArgsNameValue{:}); end % for it = 1 : numel(phi0) f = cat(1, f{:}); a = cat(2, a{:}); else N = 2*n+2; N = 2*n+2; M0 = zeros(n+1, N); M1 = zeros(n+1, N); M0 = zeros(n+1, N); M1 = zeros(n+1, N); coefficient = @(k, i, t) factorial( i + k -1 ) / factorial( i - 1) * t^(i-1); coefficient = @(k, i, t) factorial( i + k -1 ) / factorial( i - 1) * t^(i-1); for k = 0:n for i = 1:( N - k ) M0(k+1, i + k) = coefficient(k, i, t0); M1(k+1, i + k) = coefficient(k, i, t1); for k = 0:n for i = 1:( N - k ) M0(k+1, i + k) = coefficient(k, i, t0); M1(k+1, i + k) = coefficient(k, i, t1); end end end a = [M0; M1] \ [phi0; zeros(n,1); phi1; zeros(n,1)]; if misc.issym( optArgs.var ) t = optArgs.var; else t = sym( optArgs.var ); end a = [M0; M1] \ [phi0; zeros(n,1); phi1; zeros(n,1)]; f = sym(0); if misc.issym( optArgs.var ) t = optArgs.var; else t = sym( optArgs.var ); end for i = 1 : N f = f + a(i) * t^(i-1); end f = sym(0); end for i = 1 : N f = f + a(i) * t^(i-1); end end % if ~isscalar(phi0) end % setPointChange() \ No newline at end of file
 ... ... @@ -48,8 +48,9 @@ function planPolynomialTrajectoryTest(tc) x0 = [10; 12]; x1 = [-5; -5]; [trj.u, trj.y, trj.x] = misc.ss.planPolynomialTrajectory(S, t, 'x0', x0, 'x1', x1); warning off [y, ~, x] = lsim(S, trj.u.on(), t.grid, x0); warning on tc.verifyEqual(trj.x.on(), x, "AbsTol", 2e-2); tc.verifyEqual(trj.y.on(), y, "AbsTol", 1e-2); ... ...
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!