Program Listing for File ClothoidSplineG2.m¶
↰ Return to documentation for file (ClothoidSplineG2.m)
%>
%> Construct a piecewise clothoids \f$ \G(s) \f$ composed by
%> n clothoids that solve the G2 problem
%>
%> **match points**
%>
%> \f[ G(s_k) = \mathbf{p}_k \f]
%> \f[ \lim_{s\to s_k^+} G'(s) = \lim_{s\to s_k^-} G'(s) \f]
%> \f[ \lim_{s\to s_k^+} G''(s) = \lim_{s\to s_k^-} G''(s) \f]
%>
%> **Reference**
%>
%> The solution algorithm is described in
%>
%> - **E.Bertolazzi, M.Frego**, Interpolating clothoid splines with curvature continuity
%> Mathematica Methods in the Applied Sciences, vol 41, N.4, pp. 1723-1737, 2018
%>
%> \rst
%>
%> .. image:: ../../images/G2problem3arc.jpg
%> :width: 80%
%> :align: center
%>
%> \endrst
%>
classdef ClothoidSplineG2 < handle
%% MATLAB class wrapper for the underlying C++ class
properties (SetAccess = private, Hidden = true)
objectHandle; % Handle to the underlying C++ class instance
use_Ipopt;
iter_opt;
ipopt_check_gradient;
isOctave;
end
methods (Hidden = true)
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [ceq,jaceq] = nlsys( self, theta )
ceq = ClothoidSplineG2MexWrapper( 'constraints', self.objectHandle, theta );
jaceq = ClothoidSplineG2MexWrapper( 'jacobian', self.objectHandle, theta );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [c,ceq,jac,jaceq] = con( self, theta )
c = zeros(0,0);
ceq = ClothoidSplineG2MexWrapper( 'constraints', self.objectHandle, theta );
jac = sparse(0,0);
jaceq = ClothoidSplineG2MexWrapper( 'jacobian', self.objectHandle, theta ).';
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [o,g] = obj( self, theta )
o = ClothoidSplineG2MexWrapper( 'objective', self.objectHandle, theta );
g = ClothoidSplineG2MexWrapper( 'gradient', self.objectHandle, theta );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%>
%> Call C++ setup for the problem solver.
%> Check that consecutive points are distinct
%>
function build( self, x, y )
chk = diff(x).^2+diff(y).^2;
[mi,idx1] = min(chk);
[ma,idx2] = max(chk);
if mi == 0 || mi < 1e-10*ma
error( ...
[ 'ClothoidSplineG2, build failed: minimum distance between ', ...
'two consecutive points [min dist=%g, max dist=%g] is 0 or too small!\n', ...
'index of points at minimum distance is %d\n'], mi, ma, idx1 ...
);
end
ClothoidSplineG2MexWrapper( 'build', self.objectHandle, x, y );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function clots = build_internal( self, x, y, varargin )
%
% Compute guess angles
%
self.build( x, y );
[ theta_guess, theta_min, theta_max ] = self.guess();
[~,nc] = self.dims();
if self.use_Ipopt
options = {};
options.ub = theta_max;
options.lb = theta_min;
% The constraint functions are bounded to zero
options.cl = zeros(nc,1); % constraints
options.cu = zeros(nc,1);
% Set the IPOPT options.
options.ipopt.jac_d_constant = 'no';
options.ipopt.hessian_constant = 'no';
options.ipopt.mu_strategy = 'adaptive';
options.ipopt.max_iter = 400;
options.ipopt.tol = 1e-10;
options.ipopt.derivative_test_tol = 1e-5;
if self.ipopt_check_gradient
options.ipopt.derivative_test = 'first-order';
else
options.ipopt.derivative_test = 'none';
end
%options.ipopt.derivative_test_perturbation = 1e-8;
% The callback functions.
funcs.objective = @(theta) ClothoidSplineG2MexWrapper( 'objective', self.objectHandle, theta );
funcs.constraints = @(theta) ClothoidSplineG2MexWrapper( 'constraints', self.objectHandle, theta );
funcs.gradient = @(theta) ClothoidSplineG2MexWrapper( 'gradient', self.objectHandle, theta );
funcs.jacobian = @(theta) ClothoidSplineG2MexWrapper( 'jacobian', self.objectHandle, theta );
funcs.jacobianstructure = @() ClothoidSplineG2MexWrapper( 'jacobian_pattern', self.objectHandle );
%options.ipopt.jacobian_approximation = 'finite-difference-values';
options.ipopt.hessian_approximation = 'limited-memory';
%options.ipopt.limited_memory_update_type = 'bfgs'; % {bfgs}, sr1 = 6; % {6}
%options.ipopt.limited_memory_update_type = 'sr1';
options.ipopt.limited_memory_update_type = 'bfgs'; % {bfgs}, sr1 = 6; % {6}
tic
[theta, info] = ipopt(theta_guess,funcs,options);
stats.elapsed = toc;
info;
else
% 'interior-point'
if self.isOctave
options.TolX = 1e-10;
options.TolFun = 1e-20;
else
options = optimoptions(...
'fmincon','Display',self.iter_opt, ...
'CheckGradients',false, ...
'FiniteDifferenceType','central', ...
'Algorithm','sqp',...
'SpecifyConstraintGradient',true,...
'SpecifyObjectiveGradient',true,...
'OptimalityTolerance',1e-20,...
'ConstraintTolerance',1e-10 ...
);
end
obj = @(theta) self.obj(theta);
con = @(theta) self.con(theta);
theta = fmincon(obj,theta_guess,[],[],[],[],theta_min,theta_max,con,options);
%options = optimset(varargin{:});
%[theta,resnorm,~,~,output,~,~] = lsqnonlin( @target, theta, [], [], options );
end
%
% Compute spline parameters
%
clots = ClothoidList();
N = length(theta);
clots.reserve(N-1);
for j=1:N-1
clots.push_back_G1( x(j), y(j), theta(j), x(j+1), y(j+1), theta(j+1) );
end
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function clots = build_internal2( self, x, y, varargin )
%
% Compute guess angles
%
self.build( x, y );
[ theta_guess, ~, ~ ] = self.guess();
% 'interior-point'
if self.isOctave
options.TolX = 1e-20;
else
options = optimoptions( ...
'fsolve', 'Display', self.iter_opt, ...
'CheckGradients', false, ...
'FiniteDifferenceType', 'central', ...
'Algorithm', 'levenberg-marquardt',...
'SpecifyObjectiveGradient', true,...
'OptimalityTolerance', 1e-20 ...
);
end
obj = @(theta) self.nlsys(theta);
[theta,~,exitflag,~] = fsolve(obj,theta_guess,options);
if exitflag <= 0
error('ClothoidSplineG2, fsolve failed exitflag = %d\n',exitflag);
end
%
% Compute spline parameters
%
clots = ClothoidList();
N = length(theta);
clots.reserve(N-1);
for j=1:N-1
clots.push_back_G1( x(j), y(j), theta(j), x(j+1), y(j+1), theta(j+1) );
end
end
end
methods
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function self = ClothoidSplineG2()
self.objectHandle = ClothoidSplineG2MexWrapper( 'new' );
self.use_Ipopt = false;
self.iter_opt = 'iter';
self.ipopt_check_gradient = false;
self.isOctave = exist('OCTAVE_VERSION', 'builtin') ~= 0;
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function delete( self )
ClothoidSplineG2MexWrapper( 'delete', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function verbose( self, yes )
if yes
self.iter_opt = 'iter';
else
self.iter_opt = 'none';
end
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function ipopt( self, yesno )
self.use_Ipopt = yesno;
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function ipopt_check( self, yesno )
self.ipopt_check_gradient = yesno;
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [n,nc] = dims( self )
[n,nc] = ClothoidSplineG2MexWrapper( 'dims', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
function [ theta, theta_min, theta_max ] = guess( self )
[ theta, theta_min, theta_max ] = ...
ClothoidSplineG2MexWrapper( 'guess', self.objectHandle );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%>
%> Compute the clothoid spline passing to the points (x(i),y(i))
%> with initial angle theta0 (radiants) and final angle theta1 (radiants)
%>
function clots = buildP1( self, x, y, theta0, theta1 )
ClothoidSplineG2MexWrapper( ...
'target', self.objectHandle, 'P1', theta0, theta1 ...
);
clots = self.build_internal2( x, y );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%>
%> Compute the clothoid spline passing to the points (x(i),y(i))
%> with cyclic condition (tangent and curvature meet)
%>
%> - theta(1) = theta(end) mod 2*pi
%> - kappa(1) = kappa(end)
%>
function clots = buildP2( self, x, y )
ClothoidSplineG2MexWrapper( 'target', self.objectHandle, 'P2' );
clots = self.build_internal2( x, y );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%>
%> Compute the clothoid spline passing to the points (x(i),y(i))
%> with assignede initial angle and survature
%> NB: this target is not reccomended (its unstable), used only for debugging
%>
function clots = buildP3( self, x, y, theta0, kappa0 )
ClothoidSplineG2MexWrapper( 'target', self.objectHandle, 'P3' );
%
% Compute spline parameters
%
clots = ClothoidList();
N = length(x);
clots.reserve(N-1);
c = ClothoidCurve();
ok = c.build_forward( x(1), y(1), theta0, kappa0, x(2), y(2) );
if ok
clots.push_back( c );
else
warning('buildP3 failed');
end
for j=3:N
theta0 = c.thetaEnd();
kappa0 = c.kappaEnd();
ok = c.build_forward( x(j-1), y(j-1), theta0, kappa0, x(j), y(j) );
if ok
clots.push_back( c );
else
warning('buildP3 failed');
end
end
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%>
%> Compute the clothoid spline passing to the points \f$ (x_i,y_i)\f$
%> and minimizing derivative of the curvature al the extrema
%>
%> minimize \f$ k'(0)^2 + k'(L)^2 \f$
%>
function clots = buildP4( self, x, y )
ClothoidSplineG2MexWrapper( 'target', self.objectHandle, 'P4' );
clots = self.build_internal( x, y );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%>
%> Compute the clothoid spline passing to the points \f$ (x_i,y_i)\f$
%> and minimizing the length of the first and last segment
%>
%> minimize \f$ (s_1-s_0)+(s_n - s_{n-1}) \f$
%>
function clots = buildP5( self, x, y )
ClothoidSplineG2MexWrapper( 'target', self.objectHandle, 'P5' );
clots = self.build_internal( x, y );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%>
%> Compute the clothoid spline passing to the points \f$ (x_i,y_i)\f$
%> and minimizing the total length of the spline
%>
%> \f$ L = s_n-s_0 \f$
%>
function clots = buildP6( self, x, y )
ClothoidSplineG2MexWrapper( 'target', self.objectHandle, 'P6' );
clots = self.build_internal( x, y );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%>
%> Compute the clothoid spline passing to the points \f$ (x_i,y_i)\f$
%> and minimizing the integral of the square of the curvature:
%>
%> minimize: \f$ \displaystyle \int_0^L k(s)^2 \mathrm{d}s \f$
%>
function clots = buildP7( self, x, y )
ClothoidSplineG2MexWrapper( 'target', self.objectHandle, 'P7' );
clots = self.build_internal( x, y );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%>
%> Compute the clothoid spline passing to the points \f$ (x_i,y_i)\f$
%> and minimizing the integral of the square of the curvature derivative:
%>
%> minimize: \f$ \displaystyle \int_0^L k'(s)^2 \mathrm{d}s \f$
%>
function clots = buildP8( self, x, y )
ClothoidSplineG2MexWrapper( 'target', self.objectHandle, 'P8' );
clots = self.build_internal( x, y );
end
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%>
%> Compute the clothoid spline passing to the points \f$ (x_i,y_i)\f$
%> and minimizing the integral of the jerk squared
%>
function clots = buildP9( self, x, y )
ClothoidSplineG2MexWrapper( 'target', self.objectHandle, 'P9' );
clots = self.build_internal( x, y );
end
%
end
end
