├── .gitignore ├── LICENSE ├── README.md ├── src ├── @scopt2 │ ├── adaptTrustRegion.m │ ├── computeAugmentedObjectiveFunctionTerms.m │ ├── computeAugmentedObjectiveFunctions.m │ ├── computeConvergenceHistory.m │ ├── computeDirectionalDerivative.m │ ├── computeMeritFunction.m │ ├── computeNewPoint.m │ ├── computeObjectiveFunctions.m │ ├── computeObjectiveFunctionsNormal.m │ ├── computePenaltiesVirtualControls.m │ ├── computeRegularisation.m │ ├── evaluateSufficientDecrease.m │ ├── generateFigures.m │ ├── plotAdaptiveTrustRegionMonitor.m │ ├── plotConvergenceHistory.m │ ├── plotNonZeroEntries.m │ ├── plotNonZeroEntriesAB.m │ ├── plotRealObjective.m │ ├── plotSolutionMatrices.m │ ├── plotTime.m │ ├── plotTrustRegionErrorHistory.m │ ├── plotTrustRegionHistory.m │ ├── plotVerificationDefectConstraints.m │ ├── plotVirtualBuffersErrorHistory.m │ ├── plotVirtualBuffersHistory.m │ ├── plotVirtualControlErrorHistory.m │ ├── plotVirtualControlHistory.m │ ├── problemTranscription.m │ ├── scaleControlsToNormalized.m │ ├── scaleControlsToReal.m │ ├── scaleStatesToNormalized.m │ ├── scaleStatesToReal.m │ ├── scaleTimeToNormalized.m │ ├── scaleTimeToReal.m │ ├── scopt2.m │ ├── scoptSave2pdf.m │ ├── setUpSCOPT_level1.m │ ├── setUpSCOPT_level2.m │ ├── solveSCOPTProblem.m │ ├── solveSOCPProblem.m │ └── terminationVariableConvergence.m └── toolbox │ └── goldenSection │ ├── costFun_convex1D.m │ ├── goldenSection.m │ └── goldenSectionTest.m └── test ├── .gitignore ├── quadRotor3D ├── controlMatrixQuadrotor3D.m ├── noFlyZone.m ├── stateDerivativeQuadrotor3D.m ├── stateMatrixQuadrotor3D.m └── unitTest_quadrotor3D.m ├── rocketLanding ├── bodyMap │ ├── aerodynamicModel │ │ └── aerodynamicModel.m │ ├── atmosphereModel │ │ ├── USSA1976Until130kmASTOS_HD.txt │ │ ├── atmosphereModel.m │ │ ├── atmosphereModelValidation.m │ │ └── atmosusa76.m │ ├── bodyMap.m │ ├── gravityModel │ │ └── gravityModel.m │ ├── massModel │ │ └── massModel.m │ ├── orientationModel │ │ └── orientationModel.m │ └── thrustModel │ │ └── thrustModel.m ├── dynamics │ ├── controlMatrixRocketLandingFull.m │ ├── controlMatrixSolutionRocketLandingFull.m │ ├── stateDerivativeRocketLanding.m │ ├── stateDerivativeSolutionRocketLanding.m │ ├── stateMatrixRocketLandingFull.m │ ├── stateMatrixSolutionRocketLandingFull.m │ └── timeDerivativeSolutionRocketLanding.m ├── unitTest_rocketLanding_3dofAcikmeseAlternative.m ├── unitTest_rocketLanding_3dofAcikmeseDetailedEnvironment.m ├── unitTest_rocketLanding_3dofAcikmeseFixTimeVerification.m └── unitTest_rocketLanding_3dofSzmuk.m └── spacecraftLanding ├── controlMatrixSpacecraft.m ├── stateDerivativeSpacecraft.m ├── stateMatrixSpacecraft.m └── unitTest_spacecraftLanding.m /.gitignore: -------------------------------------------------------------------------------- 1 | *.asv 2 | *.fig 3 | *.jpg 4 | *.pdf 5 | 6 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # scopt2 2 | Successive Convexification OPtimal conTrol 2.0 3 | Optimal control tool to solve single phase autonomous problems 4 | using successive convexification. Can also solve basic SOCP/linear 5 | optimal control problems with one iteration. 6 | Copyright (C) 2020 Guillermo J. Dominguez Calabuig 7 | 8 | This program is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU General Public License as published by 10 | the Free Software Foundation, either version 3 of the License, or 11 | (at your option) any later version. 12 | 13 | This program is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU General Public License 19 | along with this program. If not, see . 20 | 21 | 22 | ## Installation 23 | 24 | scopt2 requires matlab R2018a or latest. 25 | 26 | ### Dependencies 27 | 1. Ecos Solver 28 | 2. SDPT3 Solver 29 | 3. OR CVX Installation 30 | 31 | 32 | ## User Manual 33 | A full Description is available in a Master Thesis from the TU Delft Repository: 34 | 'Optimum On-board Abort Guidance based on Successive Convexification 35 | for Atmospheric Re-Entry Vehicles', Master Thesis, Guillermo 36 | Joaquin Dominguez Calabuig. 37 | -------------------------------------------------------------------------------- /src/@scopt2/adaptTrustRegion.m: -------------------------------------------------------------------------------- 1 | function [radius,lambda,rejected] = adaptTrustRegion(obj,radius,lambda,rho,numericalIssues) 2 | % Adapts trust region based on convexification errors 3 | if nargin<5 4 | numericalIssues = 0; 5 | end 6 | radius_l = obj.algorithm.sequential.trustRegion.radius_l; 7 | radius_u = obj.algorithm.sequential.trustRegion.radius_u; 8 | lambda_l = obj.algorithm.sequential.trustRegion.lambda_l; 9 | lambda_u = obj.algorithm.sequential.trustRegion.lambda_u; 10 | rho0 = obj.algorithm.sequential.trustRegion.rho0; 11 | rho1 = obj.algorithm.sequential.trustRegion.rho1; 12 | rho2 = obj.algorithm.sequential.trustRegion.rho2; 13 | alpha = obj.algorithm.sequential.trustRegion.alpha; 14 | beta = obj.algorithm.sequential.trustRegion.beta; 15 | rejected = 0; 16 | if strcmp(obj.algorithm.sequential.type,'trust-region') && obj.algorithm.sequential.trustRegion.adaptive 17 | % Step 3 18 | if (rho <= rho0) || numericalIssues 19 | % Reject this step, contract trust region and go back to step 1 20 | radius = radius / alpha; 21 | radius = max(radius,radius_l); 22 | % Reject this step, increase trust region penalisation and go back to step 1 23 | lambda = lambda * alpha; 24 | lambda = min(lambda,lambda_u); 25 | 26 | if (radius>radius_l) || (lambda= rho2 50 | % Expand Trust Region 51 | radius = radius * beta; 52 | % Reduce Trust Region Penalisation 53 | lambda = lambda / beta; 54 | 55 | if obj.algorithm.sequential.trustRegion.phases.timeFinal.adaptive && obj.problem.phases(1).freeTimeFinal 56 | % Reject this step, contract trust region and go back to step 1 57 | obj.algorithm.sequential.trustRegion.phases(1).timeFinal.component_n = min(obj.algorithm.sequential.trustRegion.phases(1).timeFinal.component_n * beta,radius_u); 58 | % Reject this step, increase trust region penalisation and go back to step 1 59 | obj.algorithm.sequential.trustRegion.phases(1).timeFinal.lambda = max(obj.algorithm.sequential.trustRegion.phases(1).timeFinal.lambda / beta,lambda_l); 60 | end 61 | end 62 | radius = min(max(radius,radius_l),radius_u); 63 | lambda = min(max(lambda,lambda_l),lambda_u); 64 | end 65 | end 66 | end 67 | 68 | -------------------------------------------------------------------------------- /src/@scopt2/computeAugmentedObjectiveFunctionTerms.m: -------------------------------------------------------------------------------- 1 | function [penaltyDefectsNonLinear,penaltyBuffersNonLinear,penaltyTrustRegion,penaltyTime,penaltyDefectsLinear,penaltyBuffersLinear] = computeAugmentedObjectiveFunctionTerms(obj,time_n,X_n,U_n,time_n_p,X_n_p,U_n_p,linearToo) 2 | % Computes Augmeted Objective Function terms to be used to 3 | % measure the convexification errors 4 | timeFinal_n = time_n(end); 5 | timeInitial_n= time_n(1); 6 | X = obj.scaleStatesToReal(1,X_n); 7 | U = obj.scaleControlsToReal(1,U_n); 8 | timeFinal = obj.scaleTimeToReal(1,timeFinal_n); 9 | timeInitial = obj.scaleTimeToReal(1,timeInitial_n); 10 | 11 | timeDisc = (timeFinal - timeInitial)/(obj.problem.phases.Nodes-1); 12 | time = (timeInitial:timeDisc:timeFinal); 13 | 14 | if nargin<8 15 | linearToo = 1; 16 | end 17 | timeFinal_n_p = time_n_p(end); 18 | timeInitial_n_p= time_n_p(1); 19 | X_p = obj.scaleStatesToReal(1,X_n_p); 20 | U_p = obj.scaleControlsToReal(1,U_n_p); 21 | timeFinal_p = obj.scaleTimeToReal(1,timeFinal_n_p); 22 | timeInitial_p = obj.scaleTimeToReal(1,timeInitial_n_p); 23 | 24 | timeDisc_p = (timeFinal_p - timeInitial_p)/(obj.problem.phases.Nodes-1); 25 | time_p = (timeInitial_p:timeDisc_p:timeFinal_p); 26 | % 27 | index = obj.problem.phases(1).index; 28 | Nodes = obj.problem.phases(1).Nodes; 29 | n = obj.problem.phases(1).n; 30 | %% Virtual Control Dynamics Error Measure 31 | if strcmp(obj.algorithm.sequential.type,'trust-region') %&& obj.algorithm.sequential.trustRegion.adaptive % IF SEQUENTIAL BETTER. THERE IS ALWAYS A LINEARIZATION ERROR 32 | stateDiff_n = X_n(:,2:end)-X_n(:,1:end-1); 33 | 34 | % ScalingVirtual = ones(n.states,1); 35 | % ScalingVirtual(obj.algorithm.virtualControl.phases(1).rowE) = obj.algorithm.virtualControl.phases(1).scale; 36 | ScalingVirtual = obj.algorithm.virtualControl.phases(1).scale; 37 | switch obj.algorithm.collocationMethod 38 | case 'euler' 39 | dX_n_dt_ii = repmat((1./obj.problem.phases(1).scale.states(:)),1,Nodes) .* obj.problem.phases(1).dynamics.stateDerivativeFunction(time,X,U,obj.problem.phases(1).BodyMap,index.nodes); 40 | errorDefectsNonLinear_aux0 = (stateDiff_n - timeDisc*dX_n_dt_ii(:,1:end-1))./ScalingVirtual; 41 | case 'trapezoidal' 42 | dX_n_dt_ii = repmat((1./obj.problem.phases(1).scale.states(:)),1,Nodes) .* obj.problem.phases(1).dynamics.stateDerivativeFunction(time,X,U,obj.problem.phases(1).BodyMap,index.nodes); 43 | errorDefectsNonLinear_aux0 = (stateDiff_n - timeDisc/2*(dX_n_dt_ii(:,1:Nodes-1)+dX_n_dt_ii(:,2:Nodes)))./ScalingVirtual; 44 | case 'exact' 45 | f_n_ii = repmat((1./obj.problem.phases(1).scale.states(:)),1,Nodes-1) .* obj.problem.phases(1).dynamics.stateDerivativeFunction(time,X,U,obj.problem.phases(1).BodyMap,index.nodes); 46 | errorDefectsNonLinear_aux0 = (stateDiff_n - f_n_ii)./ScalingVirtual; 47 | otherwise 48 | error('SCOPT Error: Collocation Method does not exist') 49 | end 50 | 51 | errorDefectsLinear_aux0 = reshape(-(obj.problem.phases(1).transcription.dynamics.MEQnoVirtual*obj.solution.Zopt-obj.problem.phases(1).transcription.dynamics.pEQ),obj.problem.phases(1).n.states,obj.problem.phases(1).Nodes-1)./ScalingVirtual; 52 | 53 | switch obj.algorithm.sequential.trustRegion.defectsCompErrorPenalty 54 | case 0 55 | errorDefectsNonLinear_aux1 = sum(errorDefectsNonLinear_aux0.*errorDefectsNonLinear_aux0,1); 56 | if linearToo,errorDefectsLinear_aux1 = sum(errorDefectsLinear_aux0.*errorDefectsLinear_aux0,1);end 57 | case {1,2,inf} 58 | errorDefectsNonLinear_aux1 = vecnorm(errorDefectsNonLinear_aux0,obj.algorithm.sequential.trustRegion.defectsCompErrorPenalty,1); 59 | if linearToo,errorDefectsLinear_aux1 = vecnorm(errorDefectsLinear_aux0,obj.algorithm.sequential.trustRegion.defectsCompErrorPenalty,1);end 60 | otherwise 61 | error('Error, not defined for adaptive trust region dynamic state penalty of type %0.0 \n',obj.algorithm.sequential.trustRegion.defectsCompErrorPenalty) 62 | end 63 | 64 | switch obj.algorithm.sequential.trustRegion.defectsNodeErrorPenalty 65 | case {1,2,inf} 66 | errorDefectsNonLinear = vecnorm(errorDefectsNonLinear_aux1,obj.algorithm.sequential.trustRegion.defectsNodeErrorPenalty,2); 67 | if linearToo,errorDefectsLinear = vecnorm(errorDefectsLinear_aux1,obj.algorithm.sequential.trustRegion.defectsNodeErrorPenalty,2);end 68 | otherwise 69 | error('Error, not defined for adaptive trust region dynamics penalty of type %0.0 \n',obj.algorithm.sequential.trustRegion.defectsNodeErrorPenalty) 70 | end 71 | penaltyDefectsNonLinear = obj.algorithm.virtualControl.phases(1).lambda*errorDefectsNonLinear; 72 | if linearToo,penaltyDefectsLinear = obj.algorithm.virtualControl.phases(1).lambda*errorDefectsLinear;else,penaltyDefectsLinear = 0;end 73 | else 74 | penaltyDefectsNonLinear = 0; 75 | penaltyDefectsLinear = 0; 76 | end 77 | %% Virtual Buffer Zone Error Measure 78 | if strcmp(obj.algorithm.sequential.type,'trust-region') && obj.algorithm.sequential.trustRegion.include.pathAndEventsErrors && obj.algorithm.sequential.trustRegion.adaptive 79 | penaltyBufferNonLinearPath_ii = zeros(1,obj.problem.phases.n.path); 80 | penaltyBufferLinearPath_ii = zeros(1,obj.problem.phases.n.path); 81 | 82 | for ii = 0:obj.problem.phases.n.path-1 83 | scalePath = obj.problem.phases(1).path(ii+obj.MIOFF).scale; 84 | limit = obj.problem.phases(1).path(ii+obj.MIOFF).limit; 85 | switch obj.problem.phases(1).path(ii+obj.MIOFF).funType 86 | case 'linear' 87 | if ~strcmp(obj.problem.phases(1).path(ii+obj.MIOFF).type,'equal') 88 | if obj.problem.phases(1).path(ii+obj.MIOFF).integral && ~obj.problem.phases(1).path(ii+obj.MIOFF).derivative 89 | switch obj.problem.phases(1).path(ii+obj.MIOFF).type 90 | case 'lower' 91 | typeSign = -1; 92 | case 'upper' 93 | typeSign = 1; 94 | otherwise 95 | error('SCOPT Error: type of path constraint does not exist') 96 | end 97 | penaltyBufferNonLinearPath_ii(ii+obj.MIOFF) = obj.problem.phases(1).path(ii+obj.MIOFF).buffer.lambda*max(0,typeSign*(timeDisc*sum(sum((obj.problem.phases(1).path(ii+obj.MIOFF).states.*X + obj.problem.phases(1).path(ii+obj.MIOFF).controls.*U),1),2) - limit)/scalePath); 98 | if linearToo,penaltyBufferLinearPath_ii(ii+obj.MIOFF) = obj.problem.phases(1).path(ii+obj.MIOFF).buffer.lambda*max(0,typeSign*(tauDisc/2*(timeFinal_p-timeInitial_p)*sum(sum((obj.problem.phases(1).path(ii+obj.MIOFF).states.*X + obj.problem.phases(1).path(ii+obj.MIOFF).controls.*U),1),2) + tauDisc/2*(timeFinal - timeFinal_p)*sum(sum((obj.problem.phases(1).path(ii+obj.MIOFF).states.*X_p + obj.problem.phases(1).path(ii+obj.MIOFF).controls.*U_p),1),2) - limit)/scalePath);end 99 | else 100 | penaltyBufferNonLinearPath_ii(ii+obj.MIOFF) = 0; 101 | penaltyBufferLinearPath_ii(ii+obj.MIOFF) = 0; 102 | end 103 | end 104 | case 'non-linear' 105 | switch obj.problem.phases(1).path(ii+obj.MIOFF).type 106 | case 'lower' 107 | typeSign = -1; 108 | case 'upper' 109 | typeSign = 1; 110 | otherwise 111 | error('SCOPT Error, Path Constraints of constraint type %s not implemented. Use inequality constraints instead \n Aborting',Path(ii+obj.MIOFF).type) 112 | 113 | end 114 | f = obj.problem.phases(1).path(ii+obj.MIOFF).function(time,X,U,obj.problem.phases(1).BodyMap,index.nodes); 115 | if linearToo 116 | f_ii = obj.problem.phases(1).path(ii+obj.MIOFF).function(time_p,X_p,U_p,obj.problem.phases(1).BodyMap_prev,index.nodes); 117 | dG_dX_ii = obj.problem.phases(1).path(ii+obj.MIOFF).jacobian.states(time_p,X_p,U_p,obj.problem.phases(1).BodyMap_prev,index.nodes); 118 | dG_dU_ii = obj.problem.phases(1).path(ii+obj.MIOFF).jacobian.controls(time_p,X_p,U_p,obj.problem.phases(1).BodyMap_prev,index.nodes); 119 | end 120 | if obj.problem.phases(1).path(ii+obj.MIOFF).integral && ~obj.problem.phases(1).path(ii+obj.MIOFF).derivative 121 | penaltyBufferNonLinearPath_ii(ii+obj.MIOFF) = obj.problem.phases(1).path(ii+obj.MIOFF).buffer.lambda*max(0,typeSign*(timeDisc*sum(f,2) - limit)./scalePath); 122 | if linearToo,penaltyBufferLinearPath_ii(ii+obj.MIOFF) = obj.problem.phases(1).path(ii+obj.MIOFF).buffer.lambda*max(0,typeSign*(tauDisc/2*(timeFinal_p-timeInitial_p)*sum(f_ii + sum(dG_dX_ii.*(X - X_p),1) + sum(dG_dU_ii.*(U - U_p),1),2) + tauDisc/2*(timeFinal - timeFinal_p)*sum(f_ii,2) - limit)./scalePath);end 123 | elseif ~obj.problem.phases(1).path(ii+obj.MIOFF).integral && obj.problem.phases(1).path(ii+obj.MIOFF).derivative 124 | penaltyBufferPathNonLinear_max = max(0,typeSign*(f(2:end) - f(1:end-1) - limit*timeDisc)./scalePath); 125 | penaltyBufferPathNonLinear_node = norm(penaltyBufferPathNonLinear_max,obj.problem.phases(1).path(ii+obj.MIOFF).buffer.penaltyAdaptive); 126 | if linearToo,penaltyBufferPathLinear_max = max(0,typeSign*(f_ii(2:end) + sum(dG_dX_ii(:,2:end).*(X(:,2:end) - X_p(:,2:end)),1) + sum(dG_dU_ii(:,2:end).*(U(:,2:end) - U_p(:,2:end)),1) - f_ii(1:end-1) - sum(dG_dX_ii(:,1:end-1).*(X(:,1:end-1) - X_p(:,1:end-1)),1) - sum(dG_dU_ii(:,1:end-1).*(U(:,1:end-1) - U_p(:,1:end-1)),1) - limit*timeDisc)./scalePath);end 127 | if linearToo,penaltyBufferPathLinear_node = norm(penaltyBufferPathLinear_max,obj.problem.phases(1).path(ii+obj.MIOFF).buffer.penaltyAdaptive);end 128 | 129 | penaltyBufferNonLinearPath_ii(ii+obj.MIOFF) = obj.problem.phases(1).path(ii+obj.MIOFF).buffer.lambda*penaltyBufferPathNonLinear_node; 130 | if linearToo,penaltyBufferLinearPath_ii(ii+obj.MIOFF) = obj.problem.phases(1).path(ii+obj.MIOFF).buffer.lambda*penaltyBufferPathLinear_node;end 131 | elseif ~obj.problem.phases(1).path(ii+obj.MIOFF).integral && ~obj.problem.phases(1).path(ii+obj.MIOFF).derivative 132 | penaltyBufferPathNonLinear_max = max(0,typeSign*(f - limit)./scalePath); 133 | penaltyBufferPathNonLinear_node = norm(penaltyBufferPathNonLinear_max,obj.problem.phases(1).path(ii+obj.MIOFF).buffer.penaltyAdaptive); 134 | if linearToo,penaltyBufferPathLinear_max = max(0,typeSign*(f_ii + sum(dG_dX_ii.*(X - X_p),1) + sum(dG_dU_ii.*(U - U_p),1) - limit)./scalePath);end 135 | if linearToo,penaltyBufferPathLinear_node = norm(penaltyBufferPathLinear_max,obj.problem.phases(1).path(ii+obj.MIOFF).buffer.penaltyAdaptive);end 136 | 137 | penaltyBufferNonLinearPath_ii(ii+obj.MIOFF) = obj.problem.phases(1).path(ii+obj.MIOFF).buffer.lambda*penaltyBufferPathNonLinear_node; 138 | if linearToo,penaltyBufferLinearPath_ii(ii+obj.MIOFF) = obj.problem.phases(1).path(ii+obj.MIOFF).buffer.lambda*penaltyBufferPathLinear_node;end 139 | else 140 | error('SCOPT Error, Path Constraint is not defined for nonlinear integral and derivative, or derivative terms \n Aborting') 141 | end 142 | case 'quasiconvex' % Could be included in the trust region error computation, but omitted 143 | penaltyBufferNonLinearPath_ii(ii+obj.MIOFF) = 0; 144 | penaltyBufferLinearPath_ii(ii+obj.MIOFF) = 0; 145 | case 'non-linear-1var' % Could be included in the trust region error computation, but omitted 146 | penaltyBufferNonLinearPath_ii(ii+obj.MIOFF) = 0; 147 | penaltyBufferLinearPath_ii(ii+obj.MIOFF) = 0; 148 | case {'convex','soc'} 149 | penaltyBufferNonLinearPath_ii(ii+obj.MIOFF) = 0; 150 | penaltyBufferLinearPath_ii(ii+obj.MIOFF) = 0; 151 | otherwise 152 | warning('SCOPT Warning, Path function of type %s not implemented \n Continuing without constraint %i \n',Path(ii+obj.MIOFF).funType,ii) 153 | continue 154 | end 155 | end 156 | penaltyBuffersNonLinearPath = sum(penaltyBufferNonLinearPath_ii); 157 | penaltyBuffersLinearPath = sum(penaltyBufferLinearPath_ii); 158 | 159 | penaltyBufferNonLinearEvents_ii = zeros(1,obj.problem.phases.n.events); 160 | penaltyBufferLinearEvents_ii = zeros(1,obj.problem.phases.n.events); 161 | for ii = 0:obj.problem.phases.n.events-1 162 | switch obj.problem.phases(1).events(ii+obj.MIOFF).funType 163 | case 'non-linear' 164 | scaleEvents = obj.problem.phases(1).events(ii+obj.MIOFF).scale; 165 | limit = obj.problem.phases(1).events(ii+obj.MIOFF).limit; 166 | switch obj.problem.phases(1).events(ii+obj.MIOFF).where 167 | case 'final' 168 | indexWhere = Nodes - 1 +obj.MIOFF; 169 | case 'initial' 170 | indexWhere = 0 + obj.MIOFF; 171 | case 'index' 172 | indexWhere = obj.problem.phases(1).events(ii+obj.MIOFF).indexWhere; 173 | end 174 | switch obj.problem.phases(1).events(ii+obj.MIOFF).type 175 | case 'lower' 176 | typeSign = -1; 177 | case 'upper' 178 | typeSign = 1; 179 | otherwise 180 | error('SCOPT Error, Events Constraints of constraint type %s not implemented. Use inequality constraints instead \n Aborting',Path(ii+obj.MIOFF).type) 181 | end 182 | 183 | f = obj.problem.phases(1).events(ii+obj.MIOFF).function(time(indexWhere),X(:,indexWhere),U(:,indexWhere),indexWhere); 184 | penaltyBufferNonLinearEvents_ii(ii+obj.MIOFF) = obj.problem.phases(1).events(ii+obj.MIOFF).buffer.lambda*max(0,typeSign*(f - limit)/scaleEvents); 185 | if linearToo 186 | f_ii = obj.problem.phases(1).events(ii+obj.MIOFF).function(time_p(indexWhere),X_p(:,indexWhere),U_p(:,indexWhere),indexWhere); 187 | dG_dX_ii = obj.problem.phases(1).events(ii+obj.MIOFF).jacobian.states(time_p(indexWhere),X_p(:,indexWhere),U_p(:,indexWhere),indexWhere); 188 | dG_dU_ii = obj.problem.phases(1).events(ii+obj.MIOFF).jacobian.controls(time_p(indexWhere),X_p(:,indexWhere),U_p(:,indexWhere),indexWhere); 189 | penaltyBufferLinearEvents_ii(ii+obj.MIOFF) = obj.problem.phases(1).events(ii+obj.MIOFF).buffer.lambda*max(0,typeSign*(f_ii + sum(dG_dX_ii.*(X(:,indexWhere) - X_p(:,indexWhere)),1) + sum(dG_dU_ii.*(U(:,indexWhere) - U_p(:,indexWhere)),1) - limit)/scaleEvents); 190 | end 191 | case 'quasiconvex' % Could be included in the trust region error computation, but omitted 192 | penaltyBufferNonLinearEvents_ii(ii+obj.MIOFF) = 0; 193 | penaltyBufferLinearEvents_ii(ii+obj.MIOFF) = 0; 194 | case 'non-linear-1var' % Could be included in the trust region error computation, but omitted 195 | penaltyBufferNonLinearEvents_ii(ii+obj.MIOFF) = 0; 196 | penaltyBufferLinearEvents_ii(ii+obj.MIOFF) = 0; 197 | otherwise 198 | penaltyBufferNonLinearEvents_ii(ii+obj.MIOFF) = 0; 199 | penaltyBufferLinearEvents_ii(ii+obj.MIOFF) = 0; 200 | end 201 | end 202 | penaltyBuffersNonLinearEvents = sum(penaltyBufferNonLinearEvents_ii); 203 | penaltyBuffersLinearEvents = sum(penaltyBufferLinearEvents_ii); 204 | 205 | penaltyBuffersNonLinear = penaltyBuffersNonLinearEvents + penaltyBuffersNonLinearPath; 206 | penaltyBuffersLinear = penaltyBuffersLinearEvents + penaltyBuffersLinearPath ; 207 | if ~obj.quiet 208 | fprintf(['Non-linear Penalisation of Constraints [' repmat('%0.2e ',1,obj.problem.phases.n.path) repmat('%0.2e ',1,obj.problem.phases.n.events) '] \n'],[penaltyBufferNonLinearPath_ii,penaltyBufferNonLinearEvents_ii]); 209 | fprintf(['Linear Penalisation of Constraints [' repmat('%0.2e ',1,obj.problem.phases.n.path) repmat('%0.2e ',1,obj.problem.phases.n.events) '] \n'],[penaltyBufferLinearPath_ii,penaltyBufferLinearEvents_ii]); 210 | end 211 | else 212 | penaltyBuffersNonLinear = 0; 213 | penaltyBuffersLinear = 0; 214 | end 215 | %% Soft Time Trust Region Penalty 216 | if obj.problem.phases(1).freeTimeFinal && strcmp(obj.algorithm.sequential.trustRegion.phases(1).timeFinal.type,'soft') 217 | penaltyTime = obj.algorithm.sequential.trustRegion.phases(1).timeFinal.lambda*norm(timeFinal_n - timeFinal_n_p); 218 | else 219 | penaltyTime = 0 ; 220 | end 221 | %% Trust Region Computation and Penalty 222 | if strcmp(obj.algorithm.sequential.trustRegion.type,'soft') && strcmp(obj.algorithm.sequential.type,'trust-region') && obj.algorithm.sequential.activate 223 | AuxTrust = zeros(0,Nodes); 224 | if obj.algorithm.sequential.trustRegion.include.states 225 | AuxTrust = [AuxTrust;X_n - X_n_p]; 226 | end 227 | if obj.algorithm.sequential.trustRegion.include.controls 228 | AuxTrust = [AuxTrust;U_n - U_n_p]; 229 | end 230 | 231 | switch obj.algorithm.sequential.trustRegion.variablesPenalty 232 | case 0 233 | trustRegionP1 = sum(AuxTrust.*AuxTrust,1); 234 | case {1,2,inf} 235 | trustRegionP1 = vecnorm(AuxTrust,obj.algorithm.sequential.trustRegion.variablesPenalty,1); 236 | otherwise 237 | error('SCOPT ERROR \n Trust Region with variable penalty type %i not implemented \n',obj.algorithm.sequential.trustRegion.variablesPenalty ) 238 | end 239 | 240 | switch obj.algorithm.sequential.trustRegion.nodePenalty 241 | case {1,2,inf} 242 | trustRegionP2 = vecnorm(trustRegionP1,obj.algorithm.sequential.trustRegion.nodePenalty,2); 243 | otherwise 244 | error('SCOPT ERROR \n Trust Region with node penalty type %i not implemented \n',obj.algorithm.sequential.trustRegion.nodePenalty ) 245 | end 246 | penaltyTrustRegion = obj.algorithm.sequential.trustRegion.lambda * trustRegionP2; 247 | else 248 | penaltyTrustRegion = 0 ; 249 | end 250 | end 251 | 252 | 253 | -------------------------------------------------------------------------------- /src/@scopt2/computeAugmentedObjectiveFunctions.m: -------------------------------------------------------------------------------- 1 | function [Jaug,Laug] = computeAugmentedObjectiveFunctions(obj,time_n,X_n,U_n,time_n_p,X_n_p,U_n_p,linearToo) 2 | if nargin<8 3 | linearToo = 1; 4 | end 5 | [penaltyDefectsNonLinear,penaltyBuffersNonLinear,penaltyTrustRegion,penaltyTime,penaltyDefectsLinear,penaltyBuffersLinear] = computeAugmentedObjectiveFunctionTerms(obj,time_n,X_n,U_n,time_n_p,X_n_p,U_n_p,linearToo); 6 | 7 | Reg = computeRegularisation(obj,time_n,X_n,U_n) ; 8 | if linearToo 9 | [J,L] = computeObjectiveFunctionsNormal(obj,time_n,X_n,U_n,time_n_p,X_n_p,U_n_p); 10 | Laug = L + Reg + penaltyDefectsLinear + penaltyBuffersLinear + penaltyTrustRegion + penaltyTime ; 11 | else 12 | [J] = computeObjectiveFunctionsNormal(obj,time_n,X_n,U_n); 13 | end 14 | 15 | 16 | Jaug = J + Reg + penaltyDefectsNonLinear + penaltyBuffersNonLinear + penaltyTrustRegion + penaltyTime ; 17 | end 18 | 19 | -------------------------------------------------------------------------------- /src/@scopt2/computeConvergenceHistory.m: -------------------------------------------------------------------------------- 1 | function variablesNorm = computeConvergenceHistory(obj) 2 | % Computes Convergence History as defined by the norm type 3 | % variable tolerance 4 | states_n_IG = obj.problem.phases.initialGuess.states_n; 5 | controls_n_IG = obj.problem.phases.initialGuess.controls_n; 6 | states_n_ALL = cat(3,states_n_IG,obj.debugging.states_n_ALL); 7 | controls_n_ALL = cat(3,controls_n_IG,obj.debugging.controls_n_ALL); 8 | variables_ALL = cat(1,states_n_ALL,controls_n_ALL); 9 | trustRegionAux = diff(variables_ALL,1,3); 10 | if obj.algorithm.sequential.variablesTol.include.timeFinal == 1 11 | timeFinal_n_IG = obj.problem.phases.initialGuess.timeFinal_n; 12 | timeFinal_n_ALL = [timeFinal_n_IG,obj.debugging.timeFinal_n_ALL]; 13 | trustRegionTimeFinalAux = diff(timeFinal_n_ALL,1,2); 14 | end 15 | for kk = 2:obj.solution.iterations+0 16 | if obj.debugging.rejected(kk-1) 17 | kkValid = find(obj.debugging.rejected(1:kk-1)==0,1,'last'); 18 | trustRegionAux(:,:,kk) = variables_ALL(:,:,kk+1)-variables_ALL(:,:,kkValid+1); 19 | if obj.algorithm.sequential.variablesTol.include.timeFinal == 1 20 | trustRegionTimeFinalAux(1,kk) = timeFinal_n_ALL(1,kk+1) - timeFinal_n_ALL(1,kkValid+1); 21 | end 22 | end 23 | end 24 | trustRegionAux = reshape(trustRegionAux,(size(states_n_ALL,1)+size(controls_n_ALL,1))*size(states_n_ALL,2),obj.solution.iterations); 25 | if obj.algorithm.sequential.variablesTol.include.timeFinal == 1 26 | trustRegionAux = [trustRegionAux;trustRegionTimeFinalAux]; 27 | end 28 | variablesNorm = vecnorm(trustRegionAux,obj.algorithm.sequential.variablesTol.norm,1); 29 | end 30 | 31 | 32 | -------------------------------------------------------------------------------- /src/@scopt2/computeDirectionalDerivative.m: -------------------------------------------------------------------------------- 1 | function [directionalDerivative] = computeDirectionalDerivative(obj,searchDirection,Z_p,M_p) 2 | % Computes directional derivatives for the contraction type 3 | % line search routine 4 | switch obj.algorithm.sequential.globalisation.lineSearch.directionalDerivativeType 5 | case 'numeric' 6 | epsilon = obj.algorithm.sequential.globalisation.lineSearch.directionalDerivativeEpsilon; 7 | M_eps = computeMeritFunction(obj,searchDirection,epsilon,Z_p); 8 | % Compute Directional Derivative 9 | directionalDerivative = (M_eps - M_p)/epsilon; 10 | case 'analytical' 11 | error('SCOPT Error, Analytical Directional Derivative not implemented \n') 12 | otherwise 13 | error('SCOPT Error, Directional Derivative type not defined \n') 14 | end 15 | end 16 | 17 | -------------------------------------------------------------------------------- /src/@scopt2/computeMeritFunction.m: -------------------------------------------------------------------------------- 1 | function M = computeMeritFunction(obj,searchDirection,stepLength,Z_p) 2 | % computes merit function on new point 3 | X_n_p = Z_p(obj.problem.phases.index.states); 4 | U_n_p = reshape(Z_p(obj.problem.phases.index.controls),size(obj.problem.phases.index.controls)); 5 | timeInitial_n_p = Z_p(end-1); 6 | timeFinal_n_p = Z_p(end); 7 | timeDisc_n_p = (timeFinal_n_p - timeInitial_n_p)/(obj.problem.phases.Nodes-1); 8 | time_n_p = (timeInitial_n_p:timeDisc_n_p:timeFinal_n_p); 9 | 10 | % Extract Points 11 | Z = computeNewPoint(obj,searchDirection,stepLength,Z_p); 12 | X_n = Z(obj.problem.phases.index.states); 13 | U_n = reshape(Z(obj.problem.phases.index.controls),size(obj.problem.phases.index.controls)); 14 | timeInitial_n = Z(end-1); 15 | timeFinal_n = Z(end); 16 | 17 | timeDisc_n = (timeFinal_n - timeInitial_n)/(obj.problem.phases.Nodes-1); 18 | time_n = (timeInitial_n:timeDisc_n:timeFinal_n); 19 | 20 | % Compute Merit Function at Estimate Point 21 | M = computeAugmentedObjectiveFunctions(obj,time_n,X_n,U_n,time_n_p,X_n_p,U_n_p,0); 22 | 23 | end 24 | 25 | -------------------------------------------------------------------------------- /src/@scopt2/computeNewPoint.m: -------------------------------------------------------------------------------- 1 | function Z = computeNewPoint(~,searchDirection,stepLength,Z_p) 2 | % Computes new point along search direction 3 | Z = Z_p + searchDirection*stepLength; 4 | end 5 | 6 | -------------------------------------------------------------------------------- /src/@scopt2/computeObjectiveFunctions.m: -------------------------------------------------------------------------------- 1 | function [Jreal,Lreal] = computeObjectiveFunctions(obj,time,X,U,time_p,X_p,U_p) 2 | % Computes unscaled nonlinear (Jreal) and convexified (Lreal) 3 | % objective functions 4 | Objective = obj.problem.objective; 5 | 6 | Nodes = obj.problem.phases.Nodes; 7 | timeInterval = time(end)-time(1); 8 | 9 | if nargin>4 10 | timeInterval_p = time_p(end)-time_p(1); 11 | BodyMap_prev = obj.problem.phases(1).BodyMap_prev;BodyMap_prev.evaluated = 0; 12 | end 13 | if ~strcmp(Objective.type,'feasibility') 14 | % Mayer Term 15 | switch Objective.mayer.where 16 | case 'final' 17 | indexWhere = obj.problem.phases.Nodes - 1 + obj.MIOFF; 18 | case 'initial' 19 | indexWhere = 0 + obj.MIOFF; 20 | case 'index' 21 | indexWhere = Objective.mayer.indexWhere; 22 | otherwise 23 | end 24 | switch Objective.mayer.funType 25 | case 'linear' 26 | Jreal = (obj.problem.phases(1).freeTimeFinal*time(indexWhere)*Objective.mayer.timeFinal + dot(X(:,indexWhere),Objective.mayer.states) + dot(U(:,indexWhere),Objective.mayer.controls)); 27 | if nargin>4 28 | Lreal = Jreal; 29 | else 30 | Lreal = 0; 31 | end 32 | case {'convex','soc'} 33 | Jreal = (norm([Objective.mayer.cone.norm.states;Objective.mayer.cone.norm.controls]'*[X(:,indexWhere);U(:,indexWhere)] + Objective.mayer.cone.norm.cons(:))... 34 | - Objective.mayer.cone.right.cons-[Objective.mayer.cone.right.states(:);Objective.mayer.cone.right.controls(:)]'*[X(:,indexWhere);U(:,indexWhere)]); 35 | if nargin>4 36 | Lreal = Jreal; 37 | else 38 | Lreal = 0; 39 | end 40 | case 'non-linear' 41 | Jreal = 1/Objective.scale*Objective.mayer.function(time(indexWhere),X(:,indexWhere),U(:,indexWhere),obj.problem.phases(1).BodyMap,obj.problem.phases(1).index.nodes(indexWhere)); 42 | if nargin>4 43 | Lreal = (Objective.mayer.function(time_p(indexWhere),X_p(:,indexWhere),U_p(:,indexWhere),BodyMap_prev,obj.problem.phases(1).index.nodes(indexWhere))+... 44 | sum(Objective.mayer.jacobian.states(time_p(indexWhere),X_p(:,indexWhere),U_p(:,indexWhere),BodyMap_prev,obj.problem.phases(1).index.nodes(indexWhere)).*(X(:,indexWhere) - X_p(:,indexWhere)),1)+... 45 | sum(Objective.mayer.jacobian.controls(time_p(indexWhere),X_p(:,indexWhere),U_p(:,indexWhere),BodyMap_prev,obj.problem.phases(1).index.nodes(indexWhere)).*(U(:,indexWhere) - U_p(:,indexWhere)),1)); 46 | else 47 | Lreal = 0; 48 | end 49 | end 50 | 51 | % Lagrange Term 52 | switch obj.problem.objective.lagrange.funType 53 | case 'linear' 54 | f_ii = sum(repmat(Objective.lagrange.states(:),1,Nodes).*X,1) + sum(repmat(Objective.lagrange.controls(:),1,Nodes).*U,1);if obj.problem.objective.lagrange.absolute.nodes,f_ii = abs(f_ii);end 55 | 56 | I = sum(obj.problem.phases(1).quadratureVector.*f_ii); 57 | 58 | Jreal = timeInterval*I; 59 | if nargin>4 60 | fp = sum(repmat(Objective.lagrange.states(:),1,Nodes).*X_p,1) + sum(repmat(Objective.lagrange.controls(:),1,Nodes).*U_p,1);if obj.problem.objective.lagrange.absolute.nodes,fp = abs(fp);end 61 | Ip = sum(obj.problem.phases(1).quadratureVector.*fp); 62 | 63 | Lreal = timeInterval_p*I + Ip*(time(end)-time_p(end)); 64 | else 65 | Lreal = 0; 66 | end 67 | case {'convex','soc'} 68 | % J(kk) = 1/Objective.scale*timeDisc_opt*sum(Z_opt(index.objectiveLagrange)); %ATTENTION LAGRANGE TERM COMPUTE THE FUNCTION AGAIN 69 | NormTermDim = zeros(Objective.lagrange.cone.dimensions-1,Nodes); 70 | for dd = 0:Objective.lagrange.cone.dimensions-2 71 | NormTermDim(dd+obj.MIOFF,:)=[Objective.lagrange.cone.norm.states(:,dd+obj.MIOFF);Objective.lagrange.cone.norm.controls(:,dd+obj.MIOFF)]'*[X;U] + Objective.lagrange.cone.norm.cons(dd+obj.MIOFF); 72 | end 73 | NormTerm = vecnorm(NormTermDim,2,1); 74 | f_ii = (NormTerm - Objective.lagrange.cone.right.cons-[Objective.lagrange.cone.right.states(:);Objective.lagrange.cone.right.controls(:)]'*[X;U]); 75 | I = sum(obj.problem.phases(1).quadratureVector.*f_ii); 76 | % J = timeDisc*sum(f_ii); 77 | Jreal = timeInterval*I; 78 | 79 | if nargin>4 80 | for dd = 0:Objective.lagrange.cone.dimensions-2 81 | NormTermDim(dd+obj.MIOFF,:)=[Objective.lagrange.cone.norm.states(:,dd+obj.MIOFF);Objective.lagrange.cone.norm.controls(:,dd+obj.MIOFF)]'*[X_p;U_p] + Objective.lagrange.cone.norm.cons(dd+obj.MIOFF); 82 | end 83 | NormTerm = vecnorm(NormTermDim,2,1); 84 | f_ii_p = (NormTerm - Objective.lagrange.cone.right.cons-[Objective.lagrange.cone.right.states(:);Objective.lagrange.cone.right.controls(:)]'*[X_p;U_p]); 85 | Ip = sum(obj.problem.phases(1).quadratureVector.*f_ii_p); 86 | % L = (timeDisc_p*sum(f_ii) + tauDisc*sum(f_ii_p)/2*(time(end)-time_p(end))); 87 | Lreal = timeInterval_p*I + Ip*(time(end)-time_p(end)); 88 | else 89 | Lreal = 0; 90 | end 91 | case 'non-linear' 92 | f_ii = Objective.lagrange.function(time,X,U,obj.problem.phases(1).BodyMap,obj.problem.phases(1).index.nodes);if obj.problem.objective.lagrange.absolute.nodes,f_ii=abs(f_ii);end 93 | 94 | I = sum(obj.problem.phases(1).quadratureVector.*f_ii); 95 | Jreal = timeInterval*I; 96 | if nargin>4 97 | f_ii_p = Objective.lagrange.function(time_p,X_p,U_p,BodyMap_prev,obj.problem.phases(1).index.nodes); 98 | AX_p = sum(Objective.lagrange.jacobian.states(time_p,X_p,U_p,BodyMap_prev,obj.problem.phases(1).index.nodes).*(X-X_p),1); 99 | BU_p = sum(Objective.lagrange.jacobian.controls(time_p,X_p,U_p,BodyMap_prev,obj.problem.phases(1).index.nodes).*(U-U_p),1); 100 | if obj.problem.objective.lagrange.absolute.nodes,f_ii_pabs=abs(f_ii_p);else,f_ii_pabs=f_ii_p ;end 101 | f_ii_lin = (f_ii_p + AX_p + BU_p);if obj.problem.objective.lagrange.absolute.nodes,f_ii_lin=abs(f_ii_lin);end 102 | 103 | I_p = sum(obj.problem.phases(1).quadratureVector.*f_ii_pabs); 104 | I_aux = sum(obj.problem.phases(1).quadratureVector.*f_ii_lin); 105 | Lreal = timeInterval_p*I_aux + I_p*(time(end)-time_p(end)); 106 | else 107 | Lreal = 0; 108 | end 109 | end 110 | 111 | % minmax Term 112 | switch obj.problem.objective.minmax.funType 113 | case 'linear' 114 | f_ii = sum(Objective.minmax.states(:).*X,1) + sum(Objective.minmax.controls(:).*U,1);if obj.problem.objective.minmax.absolute.nodes,f_ii = abs(f_ii);end 115 | switch obj.problem.objective.type 116 | case 'minimize' 117 | Jreal = max(f_ii); 118 | case 'maximize' 119 | Jreal = min(f_ii); 120 | end 121 | if nargin>4 122 | Lreal = Jreal; 123 | else 124 | Lreal = 0; 125 | end 126 | case {'convex','soc'} 127 | NormTermDim = zeros(Objective.minmax.cone.dimensions-1,Nodes); 128 | for dd = 0:Objective.minmax.cone.dimensions-2 129 | NormTermDim(dd+obj.MIOFF,:)=[Objective.minmax.cone.norm.states(:,dd+obj.MIOFF);Objective.minmax.cone.norm.controls(:,dd+obj.MIOFF)]'*[X;U] + Objective.minmax.cone.norm.cons(dd+obj.MIOFF); 130 | end 131 | NormTerm = vecnorm(NormTermDim,2,1); 132 | f_ii = (NormTerm - Objective.minmax.cone.right.cons-[Objective.minmax.cone.right.states(:);Objective.minmax.cone.right.controls(:)]'*[X;U]); 133 | switch obj.problem.objective.type 134 | case 'minimize' 135 | Jreal = max(f_ii); 136 | case 'maximize' 137 | Jreal = min(f_ii); 138 | end 139 | if nargin>4 140 | Lreal = Jreal; 141 | else 142 | Lreal = 0; 143 | end 144 | case 'non-linear' 145 | f_ii = Objective.minmax.function(time,X,U,obj.problem.phases(1).BodyMap,obj.problem.phases(1).index.nodes);if obj.problem.objective.minmax.absolute.nodes,f_ii=abs(f_ii);end 146 | 147 | switch obj.problem.objective.type 148 | case 'minimize' 149 | Jreal = max(f_ii); 150 | case 'maximize' 151 | Jreal = min(f_ii); 152 | end 153 | if nargin>4 154 | f_ii_p = Objective.minmax.function(time_p,X_p,U_p,BodyMap_prev,obj.problem.phases(1).index.nodes); 155 | AX_p = sum(Objective.minmax.jacobian.states(time_p,X_p,U_p,BodyMap_prev,obj.problem.phases(1).index.nodes).*(X-X_p),1); 156 | BU_p = sum(Objective.minmax.jacobian.controls(time_p,X_p,U_p,BodyMap_prev,obj.problem.phases(1).index.nodes).*(U-U_p),1); 157 | f_ii_lin = (f_ii_p + AX_p + BU_p);if obj.problem.objective.minmax.absolute.nodes,f_ii_lin=abs(f_ii_lin);end 158 | 159 | switch obj.problem.objective.type 160 | case 'minimize' 161 | Lreal = max(f_ii_lin); 162 | case 'maximize' 163 | Lreal = min(f_ii_lin); 164 | end 165 | else 166 | Lreal = 0; 167 | end 168 | end 169 | else 170 | Jreal = 0; 171 | Lreal = 0; 172 | end 173 | end 174 | 175 | 176 | -------------------------------------------------------------------------------- /src/@scopt2/computeObjectiveFunctionsNormal.m: -------------------------------------------------------------------------------- 1 | function [J,L,Jreal,Lreal] = computeObjectiveFunctionsNormal(obj,time_n,X_n,U_n,time_n_p,X_n_p,U_n_p) 2 | % Computes scaled and unscaled (real) nonlinear (J) and convexificed 3 | % (L) objective functions 4 | obj.problem.phases(1).BodyMap.evaluated = 0; 5 | 6 | X = obj.scaleStatesToReal(1,X_n); 7 | U = obj.scaleControlsToReal(1,U_n); 8 | timeFinal = obj.scaleTimeToReal(1,time_n(end)); 9 | timeInitial = obj.scaleTimeToReal(1,time_n(1)); 10 | 11 | timeDisc = (timeFinal - timeInitial)/(obj.problem.phases.Nodes-1); 12 | time = (timeInitial:timeDisc:timeFinal); 13 | 14 | if nargin>4 15 | X_p = obj.scaleStatesToReal(1,X_n_p); 16 | U_p = obj.scaleControlsToReal(1,U_n_p); 17 | timeFinal_p = obj.scaleTimeToReal(1,time_n_p(end)); 18 | timeInitial_p = obj.scaleTimeToReal(1,time_n_p(1)); 19 | 20 | timeDisc_p = (timeFinal_p - timeInitial_p)/(obj.problem.phases.Nodes-1); 21 | time_p = (timeInitial_p:timeDisc_p:timeFinal_p); 22 | [Jreal,Lreal] = computeObjectiveFunctions(obj,time,X,U,time_p,X_p,U_p); 23 | J = Jreal/obj.problem.objective.scale*obj.problem.objective.sign; 24 | L = Lreal/obj.problem.objective.scale*obj.problem.objective.sign; 25 | else 26 | [Jreal] = computeObjectiveFunctions(obj,time,X,U); 27 | Lreal = 0; 28 | J = Jreal/obj.problem.objective.scale*obj.problem.objective.sign; 29 | L = 0; 30 | end 31 | end 32 | 33 | -------------------------------------------------------------------------------- /src/@scopt2/computePenaltiesVirtualControls.m: -------------------------------------------------------------------------------- 1 | function [virtualControlsP1,virtualControlsP2] = computePenaltiesVirtualControls(obj,virtualControls) 2 | % Computes penalties P1 and P2 produced by the use of virtual 3 | % controls based on the state penalties (per node) and their 4 | % global norm. Handles multiple iterations arrays. 5 | EvirtualControls = repmat(obj.algorithm.virtualControl.phases(1).valE,1,size(virtualControls,2),size(virtualControls,3)).*virtualControls; 6 | switch obj.algorithm.virtualControl.phases(1).statePenalty 7 | case 0 8 | virtualControlsP1 = sum(EvirtualControls.*EvirtualControls,1); 9 | case {1,2,Inf} 10 | virtualControlsP1 = vecnorm(EvirtualControls,obj.algorithm.virtualControl.phases(1).statePenalty,1); 11 | otherwise 12 | error('SCOPT ERROR \n Virtual Controls with state penalty type %i not implemented \n',obj.algorithm.sequential.trustRegion.variablesPenalty ) 13 | end 14 | 15 | switch obj.algorithm.virtualControl.phases(1).nodePenalty 16 | case {1,2,Inf} 17 | virtualControlsP2 = reshape(vecnorm(virtualControlsP1,obj.algorithm.virtualControl.phases(1).nodePenalty,2),1,[]); 18 | otherwise 19 | error('SCOPT ERROR \n Virtual Controls with node penalty type %i not implemented \n',obj.algorithm.sequential.trustRegion.nodePenalty ) 20 | end 21 | end 22 | 23 | 24 | -------------------------------------------------------------------------------- /src/@scopt2/computeRegularisation.m: -------------------------------------------------------------------------------- 1 | function [Reg] = computeRegularisation(obj,~,X_n,U_n) 2 | % Regularisation Term for Final Time not Enabled. 3 | % Does not descale the values and does not multiply by dt 4 | Reg = obj.problem.objective.regularisation.weight*sum(sum(repmat(obj.problem.objective.regularisation.states,1,size(X_n,2)).*X_n,1) + sum(repmat(obj.problem.objective.regularisation.controls,1,size(X_n,2)).*U_n,1),2); 5 | end 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/@scopt2/evaluateSufficientDecrease.m: -------------------------------------------------------------------------------- 1 | function [sufficientDecrease] = evaluateSufficientDecrease(obj,searchDirection,stepLength,directionalDerivative,Z_p,M_p) 2 | % Evaluates the sufficient decrease condition for the 3 | % contraction line search type 4 | 5 | % Compute Merit Function at Estimate Point 6 | M = computeMeritFunction(obj,searchDirection,stepLength,Z_p); 7 | 8 | % Compute Change 9 | MChange = (M_p - M)/stepLength; % should be positive for a decrease 10 | % Compute Lower Bound 11 | MChangeLow = -obj.algorithm.sequential.globalisation.lineSearch.k1*directionalDerivative; 12 | % Compute Upper Bound 13 | MChangeUpp = -obj.algorithm.sequential.globalisation.lineSearch.k2*directionalDerivative; 14 | 15 | sufficientDecrease = (MChangeLow <= MChange) && (MChange <= MChangeUpp); 16 | end 17 | 18 | -------------------------------------------------------------------------------- /src/@scopt2/generateFigures.m: -------------------------------------------------------------------------------- 1 | function generateFigures(obj) 2 | % Routine to generate output figures if sopecified by debugging 3 | % and solution flags 4 | obj.plotRealObjective(); 5 | obj.plotSolutionMatrices(); 6 | obj.plotAdaptiveTrustRegionMonitor(); 7 | obj.plotConvergenceHistory(); 8 | 9 | if obj.debugging.saveTrustRegion && obj.algorithm.sequential.activate 10 | obj.plotTrustRegionHistory(); 11 | obj.plotTrustRegionErrorHistory(); 12 | end 13 | if obj.debugging.saveVirtualControls && obj.algorithm.virtualControl.phases(1).include 14 | obj.plotVirtualControlHistory(); 15 | obj.plotVirtualControlErrorHistory(); 16 | end 17 | if obj.debugging.saveVirtualBuffers && obj.algorithm.virtualBuffer.phases(1).include 18 | obj.plotVirtualBuffersHistory(); 19 | obj.plotVirtualBuffersErrorHistory(); 20 | end 21 | if obj.debugging.generateFigureDefects 22 | obj.plotVerificationDefectConstraints(); % That defect constraints are zero (compute constraint satisfaction with virtual controls) 23 | end 24 | % obj.plotVerificationPathConstraints(); % That buffer zones satisfy path constraints 25 | obj.plotTime(); 26 | end 27 | 28 | -------------------------------------------------------------------------------- /src/@scopt2/plotAdaptiveTrustRegionMonitor.m: -------------------------------------------------------------------------------- 1 | function hfig = plotAdaptiveTrustRegionMonitor(obj,fig) 2 | % Plots a monitor of the adaptive trust region and objective 3 | % values 4 | vectorIterations = 1:obj.solution.iterations; 5 | 6 | if nargin == 2 7 | figure(fig) 8 | else 9 | figure 10 | end 11 | 12 | set(gcf,'units','normalized','position',[0 0 0.7 1]) 13 | subplot 311 14 | plot([0 vectorIterations],[obj.problem.initialGuess.J obj.debugging.J],'-*','linewidth',2);hold on 15 | plot(vectorIterations,obj.debugging.L,'-.*','linewidth',2); 16 | plot(vectorIterations,obj.debugging.Jaug,'--*','linewidth',2); 17 | plot(vectorIterations,obj.debugging.Laug,':*','linewidth',2); 18 | 19 | xlabel('Iteration $k$ [-]','interpreter','latex') 20 | ylabel('Objective [-]','interpreter','latex') 21 | legend({'Non-linear Cost Function','Cost Function','Non-linear Penalised Cost Function','Penalised Cost Function'},'interpreter','latex') 22 | 23 | if sum(obj.debugging.J<=0)<=0 24 | set(gca, 'YScale', 'log') 25 | end 26 | grid on 27 | 28 | subplot 312 29 | indexRejected = find(obj.debugging.rejected>0); 30 | plot(vectorIterations,obj.debugging.rho,'r-*','linewidth',2);hold on 31 | plot(vectorIterations,obj.algorithm.sequential.trustRegion.rho0*ones(1,obj.solution.iterations),'k--','linewidth',1); 32 | plot(vectorIterations,obj.algorithm.sequential.trustRegion.rho1*ones(1,obj.solution.iterations),'k-.','linewidth',1); 33 | plot(vectorIterations,obj.algorithm.sequential.trustRegion.rho2*ones(1,obj.solution.iterations),'k:','linewidth',1); 34 | labels = {'$\rho$','$\rho_0$','$\rho_1$','$\rho_2$'}; 35 | if ~isempty(indexRejected) 36 | plot(vectorIterations(indexRejected),obj.debugging.rejected(indexRejected).*obj.debugging.rho(indexRejected),'bo'); 37 | labels = [labels,'Rejected Step']; 38 | end 39 | xlabel('Iteration [-]','interpreter','latex') 40 | ylabel('Ratio $\Delta J$/$\Delta L$ [-]','interpreter','latex') 41 | legend(labels,'interpreter','latex','location','best'); 42 | grid on 43 | 44 | subplot 313 45 | indexRejected = find(obj.debugging.rejected>0); 46 | 47 | if strcmp(obj.algorithm.sequential.trustRegion.type,'hard') 48 | plot(vectorIterations,obj.debugging.radius,'r-*','linewidth',2);hold on 49 | plot(vectorIterations,obj.debugging.radius(1)*ones(1,obj.solution.iterations),'k--','linewidth',1); 50 | labels = {'$\eta_y$','$\eta_{y1}$'}; 51 | if ~isempty(indexRejected) 52 | plot(vectorIterations(indexRejected),obj.debugging.rejected(indexRejected).*obj.debugging.radius(indexRejected),'bo'); 53 | labels = [labels,'Rejected Step']; 54 | end 55 | if find(obj.algorithm.sequential.trustRegion.radius_l==obj.debugging.radius) 56 | plot(vectorIterations,obj.algorithm.sequential.trustRegion.radius_l *ones(1,obj.solution.iterations),'b-.','linewidth',1); 57 | labels = [labels,'$\eta_l$']; 58 | end 59 | if find(obj.algorithm.sequential.trustRegion.radius_u==obj.debugging.radius) 60 | plot(vectorIterations,obj.algorithm.sequential.trustRegion.radius_u *ones(1,obj.solution.iterations),'g-.','linewidth',1); 61 | labels = [labels,'$\eta_u$']; 62 | end 63 | ylabel('$\eta_y$ [-]','interpreter','latex') 64 | elseif strcmp(obj.algorithm.sequential.trustRegion.type,'soft') 65 | plot(vectorIterations,obj.debugging.lambda,'r-*','linewidth',2);hold on 66 | plot(vectorIterations,obj.debugging.lambda(1)*ones(1,obj.solution.iterations),'k--','linewidth',1); 67 | labels = {'$\lambda_y$','$\lambda_{y1}$'}; 68 | if ~isempty(indexRejected) 69 | plot(vectorIterations(indexRejected),obj.debugging.rejected(indexRejected).*obj.debugging.lambda(indexRejected),'bo'); 70 | labels = [labels,'Rejected Step']; 71 | end 72 | if find(obj.algorithm.sequential.trustRegion.lambda_l==obj.debugging.lambda) 73 | plot(vectorIterations,obj.algorithm.sequential.trustRegion.lambda_l *ones(1,obj.solution.iterations),'b-.','linewidth',1); 74 | labels = [labels,'$\lambda_l$']; 75 | end 76 | if find(obj.algorithm.sequential.trustRegion.lambda_u==obj.debugging.radius) 77 | plot(vectorIterations,obj.algorithm.sequential.trustRegion.lambda_u *ones(1,obj.solution.iterations),'g-.','linewidth',1); 78 | labels = [labels,'$\lambda_u$']; 79 | end 80 | ylabel('Weight [-]','interpreter','latex') 81 | end 82 | 83 | xlabel('Iteration [-]','interpreter','latex') 84 | legend(labels,'interpreter','latex','location','best'); 85 | set(gca, 'YScale', 'log') 86 | grid on 87 | 88 | scoptSave2pdf(obj,'adaptiveTrustRegionMonitor') 89 | hfig = gcf; 90 | 91 | drawnow 92 | 93 | end 94 | 95 | 96 | -------------------------------------------------------------------------------- /src/@scopt2/plotConvergenceHistory.m: -------------------------------------------------------------------------------- 1 | function hfig = plotConvergenceHistory(obj,fig,variablesNorm) 2 | % plots convergence history of the variables. user should 3 | % verify it decreases during successive iterations 4 | if nargin <3 5 | variablesNorm = computeConvergenceHistory(obj); 6 | end 7 | if nargin >= 2 8 | if ~ishandle(fig) 9 | figure(fig) 10 | else 11 | hold on 12 | end 13 | else 14 | figure 15 | end 16 | semilogy(1:obj.solution.iterations,variablesNorm,'-*','linewidth',2); 17 | ax = gca; 18 | ax.YGrid = 'on'; 19 | ax.YMinorGrid = 'on'; 20 | xlabel('Iteration $k$','interpreter','latex') 21 | ylabel('${||y^k-y^{k-1}||}_2$','interpreter','latex') 22 | scoptSave2pdf(obj,'trustRegionConvergenceMonitor') 23 | 24 | hfig = gcf; 25 | end 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/@scopt2/plotNonZeroEntries.m: -------------------------------------------------------------------------------- 1 | function plotNonZeroEntries(MAT,figNum,index) 2 | % Function to plot nonzero entries in a matrix 3 | [m,n]=size(MAT); 4 | [row,col] = find(abs(MAT)>0.00000001); 5 | 6 | if nargin >=3 7 | figure(figNum) 8 | end 9 | 10 | plot(col,row,'o') ; 11 | hold on 12 | plot([0.5 n+0.5],[0.5 0.5],'k'); 13 | plot([0.5 n+0.5],[m+0.5 m+0.5],'k'); 14 | plot([0.5 0.5],[0.5 m+0.5],'k'); 15 | plot([n+0.5 n+0.5],[0.5 m+0.5],'k'); 16 | 17 | 18 | axis([0 n+1 0 m+1]); 19 | 20 | if nargin==4 21 | plot(xlim+[0.5,-0.5],index.controls(1)*ones(1,2)-0.5,'r','linewidth',1) 22 | if ~isempty(index.virtualControls) 23 | plot(xlim+[0.5,-0.5],index.virtualControls(1)*ones(1,2)-0.5,'r','linewidth',1) 24 | end 25 | if ~isempty(index.virtualControlsP1s) 26 | plot(xlim+[0.5,-0.5],index.virtualControlsP1s(1)*ones(1,2)-0.5,'r','linewidth',1) 27 | end 28 | if ~isempty(index.virtualControlsP1) 29 | plot(xlim+[0.5,-0.5],index.virtualControlsP1(1)*ones(1,2)-0.5,'r','linewidth',1) 30 | end 31 | if ~isempty(index.virtualControlsP2) 32 | plot(xlim+[0.5,-0.5],index.virtualControlsP2*ones(1,2)-0.5,'r','linewidth',1) 33 | end 34 | if ~isempty(index.virtualBuffers.events) 35 | plot(xlim+[0.5,-0.5],index.virtualBuffers.events(1)*ones(1,2)-0.5,'r','linewidth',1) 36 | end 37 | if ~isempty(index.virtualBuffers.eventsP) 38 | plot(xlim+[0.5,-0.5],index.virtualBuffers.eventsP(1)*ones(1,2)-0.5,'r','linewidth',1) 39 | end 40 | if ~isempty(index.virtualBuffers.path) 41 | plot(xlim+[0.5,-0.5],index.virtualBuffers.path(1)*ones(1,2)-0.5,'r','linewidth',1) 42 | end 43 | if ~isempty(index.virtualBuffers.pathPs) 44 | plot(xlim+[0.5,-0.5],index.virtualBuffers.pathPs(1)*ones(1,2)-0.5,'r','linewidth',1) 45 | end 46 | if ~isempty(index.virtualBuffers.pathP) 47 | plot(xlim+[0.5,-0.5],index.virtualBuffers.pathP(1)*ones(1,2)-0.5,'r','linewidth',1) 48 | end 49 | if ~isempty(index.objectiveLagrange) 50 | plot(xlim+[0.5,-0.5],index.objectiveLagrange(1)*ones(1,2)-0.5,'r','linewidth',1) 51 | end 52 | if ~isempty(index.objectiveSlack) 53 | plot(xlim+[0.5,-0.5],index.objectiveSlack*ones(1,2)-0.5,'r','linewidth',1) 54 | end 55 | 56 | if ~isempty(index.trustRegionP1) 57 | plot(xlim+[0.5,-0.5],index.trustRegionP1(1)*ones(1,2)-0.5,'r','linewidth',1) 58 | end 59 | if ~isempty(index.trustRegionP2) 60 | plot(xlim+[0.5,-0.5],index.trustRegionP2*ones(1,2)-0.5,'r','linewidth',1) 61 | end 62 | 63 | if ~isempty(index.timeFinalSlack) 64 | plot(xlim+[0.5,-0.5],index.timeFinalSlack*ones(1,2)-0.5,'r','linewidth',1) 65 | end 66 | if ~isempty(index.timeFinal) 67 | plot(xlim+[0.5,-0.5],index.timeFinal*ones(1,2)-0.5,'r','linewidth',1) 68 | end 69 | end 70 | 71 | grid on 72 | set (gca,'Ydir','reverse') 73 | end 74 | 75 | 76 | -------------------------------------------------------------------------------- /src/@scopt2/plotNonZeroEntriesAB.m: -------------------------------------------------------------------------------- 1 | 2 | function plotNonZeroEntriesAB(A,b,figNum,index) 3 | % Function to plot nonzero entries in matrix A and column 4 | % vector b side by side 5 | figure(figNum) 6 | n = 7; 7 | subplot(1,n,1:n-1) 8 | scopt2.plotNonZeroEntries(A); 9 | if nargin ==5 10 | hold on 11 | % plot(index.states(1)*ones(1,2),ylim,'r','linewidth',2) 12 | plot(index.controls(1)*ones(1,2)-0.5,ylim+[0.5,-0.5],'r','linewidth',1) 13 | if ~isempty(index.virtualControls) 14 | plot(index.virtualControls(1)*ones(1,2)-0.5,ylim+[0.5,-0.5],'r','linewidth',1) 15 | end 16 | if ~isempty(index.virtualControlsP1s) 17 | plot(index.virtualControlsP1s(1)*ones(1,2)-0.5,ylim+[0.5,-0.5],'r','linewidth',1) 18 | end 19 | if ~isempty(index.virtualControlsP1) 20 | plot(index.virtualControlsP1(1)*ones(1,2)-0.5,ylim+[0.5,-0.5],'r','linewidth',1) 21 | end 22 | if ~isempty(index.virtualControlsP2) 23 | plot(index.virtualControlsP2*ones(1,2)-0.5,ylim+[0.5,-0.5],'r','linewidth',1) 24 | end 25 | if ~isempty(index.virtualBuffers.events) 26 | plot(index.virtualBuffers.events(1)*ones(1,2)-0.5,ylim+[0.5,-0.5],'r','linewidth',1) 27 | end 28 | if ~isempty(index.virtualBuffers.eventsP) 29 | plot(index.virtualBuffers.eventsP(1)*ones(1,2)-0.5,ylim+[0.5,-0.5],'r','linewidth',1) 30 | end 31 | if ~isempty(index.virtualBuffers.path) 32 | plot(index.virtualBuffers.path(1)*ones(1,2)-0.5,ylim+[0.5,-0.5],'r','linewidth',1) 33 | end 34 | if ~isempty(index.virtualBuffers.pathPs) 35 | plot(index.virtualBuffers.pathPs(1)*ones(1,2)-0.5,ylim+[0.5,-0.5],'r','linewidth',1) 36 | end 37 | if ~isempty(index.virtualBuffers.pathP) 38 | plot(index.virtualBuffers.pathP(1)*ones(1,2)-0.5,ylim+[0.5,-0.5],'r','linewidth',1) 39 | end 40 | if ~isempty(index.objectiveLagrange) 41 | plot(index.objectiveLagrange(1)*ones(1,2)-0.5,ylim+[0.5,-0.5],'r','linewidth',1) 42 | end 43 | if ~isempty(index.objectiveSlack) 44 | plot(index.objectiveSlack*ones(1,2)-0.5,ylim+[0.5,-0.5],'r','linewidth',1) 45 | end 46 | 47 | if ~isempty(index.trustRegionP1) 48 | plot(index.trustRegionP1(1)*ones(1,2)-0.5,ylim+[0.5,-0.5],'r','linewidth',1) 49 | end 50 | 51 | if ~isempty(index.trustRegionP2) 52 | plot(index.trustRegionP2*ones(1,2)-0.5,ylim+[0.5,-0.5],'r','linewidth',1) 53 | end 54 | 55 | if ~isempty(index.timeFinalSlack) 56 | plot(index.timeFinalSlack*ones(1,2)-0.5,ylim+[0.5,-0.5],'r','linewidth',1) 57 | end 58 | if ~isempty(index.timeFinal) 59 | plot(index.timeFinal*ones(1,2)-0.5,ylim+[0.5,-0.5],'r','linewidth',1) 60 | end 61 | end 62 | 63 | subplot(1,n,n) 64 | scopt2.plotNonZeroEntries(b); 65 | end 66 | 67 | -------------------------------------------------------------------------------- /src/@scopt2/plotRealObjective.m: -------------------------------------------------------------------------------- 1 | function hfig = plotRealObjective(obj,fig) 2 | % Plots the real objective values 3 | vectorIterations = 1:obj.solution.iterations; 4 | 5 | if nargin == 2 6 | figure(fig) 7 | else 8 | figure 9 | end 10 | plot([0,vectorIterations],[obj.problem.initialGuess.Jreal,obj.debugging.Jreal],'-*','linewidth',2);hold on 11 | plot(vectorIterations,obj.debugging.Lreal,'-o','linewidth',2);hold on 12 | 13 | xlabel('Iteration $k$ [-]','interpreter','latex') 14 | ylabel('Objective [-]','interpreter','latex') 15 | legend({'Non-convex','Convex'},'interpreter','latex','location','best') 16 | % legend({'Non-linear Cost Function','Cost Function','Non-linear Penalized Cost Function','Penalized Cost Function'},'interpreter','latex') 17 | grid on 18 | 19 | scoptSave2pdf(obj,'realObjective') 20 | hfig = gcf; 21 | 22 | drawnow 23 | end 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/@scopt2/plotSolutionMatrices.m: -------------------------------------------------------------------------------- 1 | function hfig = plotSolutionMatrices(obj) 2 | % Plots nonempty entries in solution matrices 3 | Gaug = obj.problem.transcription.Gaug; 4 | haug = obj.problem.transcription.haug; 5 | MEQ = obj.problem.transcription.MEQ; 6 | pEQ = obj.problem.transcription.pEQ; 7 | f = obj.problem.transcription.f; 8 | fnp = obj.problem.transcription.fnp; 9 | dims = obj.problem.transcription.dims; 10 | 11 | MINEQ = Gaug(1:dims.l,:); 12 | pINEQ = haug(1:dims.l,1); 13 | Gsoc = Gaug(dims.l+1:end,:); 14 | hsoc = haug(dims.l+1:end,1); 15 | 16 | 17 | scopt2.plotNonZeroEntriesAB(MEQ,pEQ,11,obj.problem.phases(1).index) 18 | title('Equality Constraints') 19 | scoptSave2pdf(obj,'equalityMatrices') 20 | scopt2.plotNonZeroEntriesAB(MINEQ,pINEQ,12,obj.problem.phases(1).index) 21 | title('Inequality Constraints') 22 | scoptSave2pdf(obj,'inequalityMatrices') 23 | scopt2.plotNonZeroEntriesAB(Gsoc,hsoc,13,obj.problem.phases(1).index) 24 | title('Second Order Cone Constraints') 25 | scoptSave2pdf(obj,'socMatrices') 26 | scopt2.plotNonZeroEntries(f,14,obj.problem.phases(1).index) 27 | title('Objective Vector') 28 | scoptSave2pdf(obj,'objectiveVector') 29 | scopt2.plotNonZeroEntries(fnp,14,obj.problem.phases(1).index) 30 | title('Augmented Objective Vector') 31 | scoptSave2pdf(obj,'augmentedObjectiveVector') 32 | 33 | drawnow 34 | 35 | hfig = gcf; 36 | 37 | end 38 | 39 | 40 | -------------------------------------------------------------------------------- /src/@scopt2/plotTime.m: -------------------------------------------------------------------------------- 1 | function hfig = plotTime(obj,fig) 2 | % Plots the free final time values 3 | if obj.problem.phases(1).freeTimeFinal 4 | vectorIterations = 0:obj.solution.iterations; 5 | 6 | if nargin == 2 7 | figure(fig) 8 | else 9 | figure 10 | end 11 | 12 | plot(vectorIterations,[obj.problem.phases.initialGuess.timeFinal,obj.debugging.timeFinal_ALL],'-*','linewidth',2);hold on 13 | plot(vectorIterations,[obj.problem.phases.initialGuess.timeInitial,obj.debugging.timeInitial_ALL],'-o','linewidth',2);hold on 14 | 15 | xlabel('Iteration $k$ [-]','interpreter','latex') 16 | ylabel('Time [-]','interpreter','latex') 17 | legend({'Final Time','Initial Time'},'interpreter','latex','location','best') 18 | % legend({'Non-linear Cost Function','Cost Function','Non-linear Penalized Cost Function','Penalized Cost Function'},'interpreter','latex') 19 | grid on 20 | 21 | scoptSave2pdf(obj,'monitorTime') 22 | hfig = gcf; 23 | 24 | drawnow 25 | else 26 | hfig = nan; 27 | end 28 | end 29 | 30 | -------------------------------------------------------------------------------- /src/@scopt2/plotTrustRegionErrorHistory.m: -------------------------------------------------------------------------------- 1 | function [trustRegionErrorP1_ALL,trustRegionErrorP2_ALL,hfig1,hfig2] = plotTrustRegionErrorHistory(obj,fig1,fig2) 2 | % plots to verify trust regions 3 | if strcmp(obj.algorithm.sequential.type,'trust-region') 4 | trustRegionP1 = obj.debugging.trustRegionP1; 5 | trustRegionP2 = obj.debugging.trustRegionP2; 6 | radius = obj.debugging.radius; 7 | 8 | iterations = obj.solution.iterations; 9 | 10 | % lineOpacity_vector = linspace(1,0,iterations); 11 | defaultColor = autumn(iterations); 12 | 13 | Nodes = size(trustRegionP1,2); 14 | [trustRegionP1_ALL,trustRegionP2_ALL] = plotTrustRegionHistory(obj,0,0); 15 | 16 | trustRegionErrorP1_ALL = abs(trustRegionP1 - trustRegionP1_ALL); 17 | trustRegionErrorP2_ALL = abs(trustRegionP2 - trustRegionP2_ALL); 18 | 19 | if obj.algorithm.sequential.activate 20 | radiusErrorP2_ALL = abs(radius - trustRegionP2_ALL); 21 | end 22 | 23 | if nargin >= 2 24 | if fig1~=0 25 | figure(fig1) 26 | dontPlot = 0; 27 | else 28 | dontPlot = 1; 29 | end 30 | else 31 | figure 32 | dontPlot = 0; 33 | end 34 | if ~dontPlot 35 | % labelLegend = strsplit(num2str(1:iterations)); 36 | labelLegend = {}; 37 | for kk = 1:iterations 38 | hplot(kk) = semilogy(1:Nodes,trustRegionErrorP1_ALL(1,:,kk),'-*','linewidth',2,'color',defaultColor(kk,:)); hold on 39 | labelLegend = [labelLegend , ['$k$ = ' num2str(kk)]]; 40 | end 41 | ax = gca; 42 | ax.YGrid = 'on'; 43 | ax.YMinorGrid = 'on'; 44 | xlabel('Node $i$ [-]','interpreter','latex') 45 | ylabel('$|P^c\left(y^k_i-y^{k-1}_i\right)-\eta^{P1}|$ [-]','interpreter','latex') 46 | legend(hplot,labelLegend,'location','best','interpreter','latex') 47 | scoptSave2pdf(obj,'trustRegionPenaltyCErrors') 48 | hfig1 = gcf; 49 | else 50 | hfig1 = nan; 51 | end 52 | if nargin >= 3 53 | if fig2~=0 54 | figure(fig2) 55 | dontPlot = 0; 56 | else 57 | dontPlot = 1; 58 | end 59 | else 60 | figure 61 | dontPlot = 0; 62 | end 63 | if ~dontPlot 64 | hplotP2(1) = semilogy(1:iterations,squeeze(trustRegionErrorP2_ALL),'-*','linewidth',2); hold on 65 | legendLabelP2{1} = '$|P^n\left(P^c\left(y^k-y^{k-1}\right)\right)-\eta^{P2}|$'; 66 | if strcmp(obj.algorithm.sequential.trustRegion.type,'hard') && obj.algorithm.sequential.activate 67 | hplotP2(2) = semilogy(1:iterations,radiusErrorP2_ALL,'k--','linewidth',2); hold on 68 | legendLabelP2{2} = '$|P^n\left(P^c\left(y^k-y^{k-1}\right)\right)-\eta_{y}|$'; 69 | end 70 | ax = gca; 71 | ax.YGrid = 'on'; 72 | ax.YMinorGrid = 'on'; 73 | xlabel('Iteration $k$ [-]','interpreter','latex') 74 | ylabel('Trust Region Error [-]','interpreter','latex') 75 | legend(hplotP2,legendLabelP2,'location','best','interpreter','latex') 76 | scoptSave2pdf(obj,'trustRegionPenaltyNErrors') 77 | hfig2 = gcf; 78 | else 79 | hfig2 = nan; 80 | end 81 | else 82 | hfig1 = nan; 83 | hfig2 = nan; 84 | end 85 | end 86 | 87 | 88 | -------------------------------------------------------------------------------- /src/@scopt2/plotTrustRegionHistory.m: -------------------------------------------------------------------------------- 1 | function [trustRegionP1_ALL,trustRegionP2_ALL,hfig1,hfig2] = plotTrustRegionHistory(obj,fig1,fig2) 2 | 3 | states_n_IG = obj.problem.phases.initialGuess.states_n; 4 | controls_n_IG = obj.problem.phases.initialGuess.controls_n; 5 | states_n_ALL = cat(3,states_n_IG,obj.debugging.states_n_ALL); 6 | controls_n_ALL = cat(3,controls_n_IG,obj.debugging.controls_n_ALL); 7 | radius = obj.debugging.radius; 8 | 9 | iterations = obj.solution.iterations; 10 | 11 | % lineOpacity_vector = linspace(1,0,iterations); 12 | defaultColor = autumn(iterations); 13 | 14 | Nodes = size(states_n_ALL,2); 15 | variables_ALL = zeros(0,Nodes,iterations+1); 16 | 17 | 18 | if obj.algorithm.sequential.trustRegion.include.states 19 | variables_ALL = cat(1,variables_ALL,states_n_ALL); 20 | end 21 | 22 | if obj.algorithm.sequential.trustRegion.include.controls 23 | variables_ALL = cat(1,variables_ALL,controls_n_ALL); 24 | end 25 | 26 | trustRegionAux = diff(variables_ALL,1,3); 27 | 28 | % IndexRejected = find(obj.debugging.rejected>0); 29 | for kk = 2:iterations+0 30 | if obj.debugging.rejected(kk-1) 31 | kkValid = find(obj.debugging.rejected(1:kk-1)==0,1,'last'); 32 | trustRegionAux(:,:,kk) = variables_ALL(:,:,kk+1)-variables_ALL(:,:,kkValid+1); 33 | end 34 | % trustRegionAux(:,:,IndexRejected+1) = variables_ALL(:,:,IndexRejected+2)-variables_ALL(:,:,IndexRejected-0); 35 | end 36 | 37 | switch obj.algorithm.sequential.trustRegion.variablesPenalty 38 | case 0 39 | trustRegionP1_ALL = sum(trustRegionAux.*trustRegionAux,1); 40 | case {1,2,inf} 41 | trustRegionP1_ALL = vecnorm(trustRegionAux,obj.algorithm.sequential.trustRegion.variablesPenalty,1); 42 | otherwise 43 | error('SCOPT ERROR \n Trust Region with variable penalty type %i not implemented \n',obj.algorithm.sequential.trustRegion.variablesPenalty ) 44 | end 45 | 46 | switch obj.algorithm.sequential.trustRegion.nodePenalty 47 | case 0 48 | error('SCOPT ERROR \n Trust Region with node penalty type %i not implemented \n',obj.algorithm.sequential.trustRegion.nodePenalty ) 49 | case {1,2,inf} 50 | trustRegionP2_ALL = reshape(vecnorm(trustRegionP1_ALL,obj.algorithm.sequential.trustRegion.nodePenalty,2),1,[]); 51 | end 52 | 53 | if nargin >= 2 54 | if fig1~=0 55 | figure(fig1) 56 | dontPlot = 0; 57 | else 58 | dontPlot = 1; 59 | end 60 | else 61 | figure 62 | dontPlot = 0; 63 | end 64 | if ~dontPlot 65 | labelLegend = {}; 66 | for kk = 1:iterations 67 | hplot(kk) = semilogy(1:Nodes,trustRegionP1_ALL(1,:,kk),'-*','linewidth',2,'color',defaultColor(kk,:)); hold on 68 | labelLegend = [labelLegend , ['$k$ = ' num2str(kk)]]; 69 | end 70 | ax = gca; 71 | ax.YGrid = 'on'; 72 | ax.YMinorGrid = 'on'; 73 | xlabel('Node $i$ [-]','interpreter','latex') 74 | ylabel('$P^c\left(y^k_i-y^{k-1}_i\right)$ [-]','interpreter','latex') 75 | legend(hplot,labelLegend,'location','best','interpreter','latex') 76 | scoptSave2pdf(obj,'trustRegionPenaltyC') 77 | hfig1 = gcf; 78 | else 79 | hfig1 = nan; 80 | end 81 | if nargin >= 3 82 | if fig2~=0 83 | figure(fig2) 84 | dontPlot = 0; 85 | else 86 | dontPlot = 1; 87 | end 88 | else 89 | figure 90 | dontPlot = 0; 91 | end 92 | if ~dontPlot 93 | hplotP2(1) = semilogy(1:iterations,squeeze(trustRegionP2_ALL),'-*','linewidth',2); hold on 94 | legendLabelP2{1} = '$P^n\left(P^c\left(y^k-y^{k-1}\right)\right)$'; 95 | if strcmp(obj.algorithm.sequential.trustRegion.type,'hard') && obj.algorithm.sequential.activate 96 | hplotP2(2) = semilogy(1:iterations,radius,'k--o','linewidth',2); hold on 97 | legendLabelP2{2} = '$\eta_{y}$'; 98 | end 99 | ax = gca; 100 | ax.YGrid = 'on'; 101 | ax.YMinorGrid = 'on'; 102 | xlabel('Iteration $k$ [-]','interpreter','latex') 103 | ylabel('Trust Region Radius [-]','interpreter','latex') 104 | legend(hplotP2,legendLabelP2,'location','best','interpreter','latex') 105 | scoptSave2pdf(obj,'trustRegionPenaltyN') 106 | hfig2 = gcf; 107 | else 108 | hfig2 = nan; 109 | end 110 | end 111 | 112 | 113 | -------------------------------------------------------------------------------- /src/@scopt2/plotVerificationDefectConstraints.m: -------------------------------------------------------------------------------- 1 | function [hfig1,hfig2,hfig3] = plotVerificationDefectConstraints(obj,fig1,fig2,fig3) 2 | % verifies the implementartion of the dynamic equations, 3 | % figurwes should show values with small errors 4 | states_IG = obj.problem.phases.initialGuess.states; 5 | controls_IG = obj.problem.phases.initialGuess.controls; 6 | time_IG = obj.problem.phases.initialGuess.time; 7 | timeFinal_IG = obj.problem.phases.initialGuess.timeFinal; 8 | timeInitial_IG = obj.problem.phases.initialGuess.timeInitial; 9 | states_ALL = cat(3,states_IG,obj.debugging.states_ALL); 10 | controls_ALL = cat(3,controls_IG,obj.debugging.controls_ALL); 11 | time_ALL = cat(3,time_IG,obj.debugging.time_ALL); 12 | timeFinal_ALL = cat(2,timeFinal_IG,obj.debugging.timeFinal_ALL); 13 | timeInitial_ALL = cat(2,timeInitial_IG,obj.debugging.timeInitial_ALL); 14 | 15 | 16 | iterations = obj.solution.iterations; 17 | 18 | % lineOpacity_vector = linspace(1,0,iterations); 19 | 20 | 21 | Nodes = size(states_ALL,2); 22 | nDefects = size(states_ALL,1); 23 | defectsjColor = jet(nDefects); 24 | iterationsColor = autumn(iterations); 25 | lineOpacity = linspace(0,1,iterations+1);lineOpacity(1) = []; 26 | 27 | statesDiff = diff(states_ALL,1,2); 28 | dtau = 2/(Nodes-1); 29 | dt = dtau*(timeFinal_ALL-timeInitial_ALL)/2; 30 | if obj.algorithm.virtualControl.phases(1).include 31 | virtualControls_ALL = obj.debugging.virtualControls; 32 | valEvirtualControls = obj.algorithm.virtualControl.phases(1).valE; 33 | rowEvirtualControls = obj.algorithm.virtualControl.phases(1).rowE; 34 | colEvirtualControls = obj.algorithm.virtualControl.phases(1).colE; 35 | scaleE = obj.algorithm.virtualControl.phases(1).scale; 36 | Evirtual = full(sparse(rowEvirtualControls,colEvirtualControls,valEvirtualControls,nDefects,max(colEvirtualControls))); 37 | else 38 | virtualControls_ALL = zeros(0,Nodes-1,iterations); 39 | scaleE = 0; 40 | Evirtual = zeros(nDefects,0); 41 | end 42 | stateDerivativeFunction = obj.problem.phases.dynamics.stateDerivativeFunction; 43 | stateMatrixFunction = obj.problem.phases.dynamics.stateMatrixFunction; 44 | controlMatrixFunction = obj.problem.phases.dynamics.controlMatrixFunction; 45 | timeDerivativeFunction = obj.problem.phases.dynamics.timeDerivativeFunction; 46 | 47 | obj.problem.phases(1).BodyMap.evaluated = 0; 48 | 49 | defectConstraints = zeros(nDefects,Nodes-1,iterations); 50 | for kk = 1:iterations 51 | if kk>1 52 | if obj.debugging.rejected(kk-1)>0 53 | kklin = find(obj.debugging.rejected(1:kk-1)==0,1,'last')+1; 54 | else 55 | kklin = kk; 56 | end 57 | else 58 | kklin = kk; 59 | end 60 | f = stateDerivativeFunction(time_ALL(:,:,kklin),states_ALL(:,:,kklin),controls_ALL(:,:,kklin),obj.problem.phases.BodyMap,1:Nodes); 61 | dfdt = timeDerivativeFunction(time_ALL(:,:,kklin),states_ALL(:,:,kklin),controls_ALL(:,:,kklin),obj.problem.phases.BodyMap,1:Nodes); 62 | switch obj.algorithm.collocationMethod 63 | case {'euler','trapezoidal'} 64 | if obj.problem.phases(1).dynamics.typeStateMatrix 65 | [rowA,colA,valA] = stateMatrixFunction(time_ALL(:,:,kklin),states_ALL(:,:,kklin),controls_ALL(:,:,kklin),obj.problem.phases.BodyMap,1:Nodes); 66 | else 67 | A = stateMatrixFunction(time_ALL(:,:,kklin),states_ALL(:,:,kklin),controls_ALL(:,:,kklin),obj.problem.phases.BodyMap,1:Nodes); 68 | end 69 | if obj.problem.phases(1).dynamics.typeControlMatrix 70 | [rowB,colB,valB] = controlMatrixFunction(time_ALL(:,:,kklin),states_ALL(:,:,kklin),controls_ALL(:,:,kklin),obj.problem.phases.BodyMap,1:Nodes); 71 | else 72 | B = controlMatrixFunction(time_ALL(:,:,kklin),states_ALL(:,:,kklin),controls_ALL(:,:,kklin),obj.problem.phases.BodyMap,1:Nodes); 73 | end 74 | case 'exact' 75 | if obj.problem.phases(1).dynamics.typeStateMatrix 76 | [rowA,colA,valA,~,rowA1,colA1,valA1] = stateMatrixFunction(time_ALL(:,:,kklin),states_ALL(:,:,kklin),controls_ALL(:,:,kklin),obj.problem.phases.BodyMap,1:Nodes); 77 | else 78 | [A,A1] = stateMatrixFunction(time_ALL(:,:,kklin),states_ALL(:,:,kklin),controls_ALL(:,:,kklin),obj.problem.phases.BodyMap,1:Nodes); 79 | end 80 | if obj.problem.phases(1).dynamics.typeControlMatrix 81 | [rowB,colB,valB,~,rowB1,colB1,valB1] = controlMatrixFunction(time_ALL(:,:,kklin),states_ALL(:,:,kklin),controls_ALL(:,:,kklin),obj.problem.phases.BodyMap,1:Nodes); 82 | else 83 | [B,B1] = controlMatrixFunction(time_ALL(:,:,kklin),states_ALL(:,:,kklin),controls_ALL(:,:,kklin),obj.problem.phases.BodyMap,1:Nodes); 84 | end 85 | end 86 | for ii = 1:Nodes-1 % Depends on discretisation option used 87 | switch obj.algorithm.collocationMethod 88 | case 'euler' 89 | fii = f(:,ii); 90 | if obj.problem.phases(1).dynamics.typeStateMatrix 91 | Aii = full(sparse(rowA(:,ii),colA(:,ii),valA(:,ii),nDefects,nDefects)); 92 | else 93 | Aii = A(:,:,ii); 94 | end 95 | if obj.problem.phases(1).dynamics.typeControlMatrix 96 | Bii = full(sparse(rowB(:,ii),colB(:,ii),valB(:,ii),nDefects,size(controls_IG,1))); 97 | else 98 | Bii = B(:,:,ii); 99 | end 100 | dfdtii = dfdt(:,ii); 101 | 102 | defectConstraints(:,ii,kk) = obj.problem.phases.scale.states.^(-1).*statesDiff(:,ii,kk+1) - obj.problem.phases.scale.states.^(-1).*( ... 103 | dt(kklin)*(... 104 | fii... 105 | + Aii*( states_ALL(:,ii,kk+1) - states_ALL(:,ii,kklin) )... 106 | + Bii*( controls_ALL(:,ii,kk+1) - controls_ALL(:,ii,kklin) ) )... 107 | + (dtau/2*fii+dt(kklin)*dfdtii)*(timeFinal_ALL(kk+1)-timeFinal_ALL(kklin)))... 108 | - scaleE*Evirtual*virtualControls_ALL(:,ii,kk); 109 | case 'trapezoidal' 110 | fii = f(:,ii); 111 | fii1 = f(:,ii+1); 112 | if obj.problem.phases(1).dynamics.typeStateMatrix 113 | Aii = full(sparse(rowA(:,ii),colA(:,ii),valA(:,ii),nDefects,nDefects)); 114 | Aii1 = full(sparse(rowA(:,ii+1),colA(:,ii+1),valA(:,ii+1),nDefects,nDefects)); 115 | else 116 | Aii = A(:,:,ii); 117 | Aii1 = A(:,:,ii+1); 118 | end 119 | if obj.problem.phases(1).dynamics.typeControlMatrix 120 | Bii = full(sparse(rowB(:,ii),colB(:,ii),valB(:,ii),nDefects,size(controls_IG,1))); 121 | Bii1 = full(sparse(rowB(:,ii+1),colB(:,ii+1),valB(:,ii+1),nDefects,size(controls_IG,1))); 122 | else 123 | Bii = B(:,:,ii); 124 | Bii1 = B(:,:,ii+1); 125 | end 126 | dfdtii = dfdt(:,ii); 127 | dfdtii1 = dfdt(:,ii+1); 128 | 129 | defectConstraints(:,ii,kk) = obj.problem.phases.scale.states.^(-1).*statesDiff(:,ii,kk+1) - obj.problem.phases.scale.states.^(-1).*(... 130 | dt(kklin)*(... 131 | (fii+fii1)/2 ... 132 | + ( Aii*( states_ALL(:,ii,kk+1) - states_ALL(:,ii,kklin) ) + Aii1*( states_ALL(:,ii+1,kk+1) - states_ALL(:,ii+1,kklin) ) )/2 ... 133 | + ( Bii*( controls_ALL(:,ii,kk+1) - controls_ALL(:,ii,kklin) ) + Bii1*( controls_ALL(:,ii+1,kk+1) - controls_ALL(:,ii+1,kklin) ) )/2 ) ... 134 | + ( dtau/2*(fii+fii1)/2+dt(kklin)*(dfdtii+dfdtii1)/2)*(timeFinal_ALL(kk+1)-timeFinal_ALL(kklin)))... 135 | - scaleE*Evirtual*virtualControls_ALL(:,ii,kk); 136 | case 'exact' 137 | fii = f(:,ii); 138 | if obj.problem.phases(1).dynamics.typeStateMatrix 139 | Aii = full(sparse(rowA(:,ii),colA(:,ii),valA(:,ii),nDefects,nDefects)); 140 | Aii1 = full(sparse(rowA1(:,ii),colA1(:,ii),valA1(:,ii),nDefects,nDefects)); 141 | else 142 | Aii = A(:,:,ii); 143 | Aii1 = A1(:,:,ii); 144 | end 145 | if obj.problem.phases(1).dynamics.typeControlMatrix 146 | Bii = full(sparse(rowB(:,ii),colB(:,ii),valB(:,ii),nDefects,size(controls_IG,1))); 147 | Bii1 = full(sparse(rowB1(:,ii),colB1(:,ii),valB1(:,ii),nDefects,size(controls_IG,1))); 148 | else 149 | Bii = B(:,:,ii); 150 | Bii1 = B1(:,:,ii); 151 | end 152 | dfdtii = dfdt(:,ii); 153 | 154 | defectConstraints(:,ii,kk) = obj.problem.phases.scale.states.^(-1).*statesDiff(:,ii,kk+1) - obj.problem.phases.scale.states.^(-1).*( fii ... 155 | + Aii*( states_ALL(:,ii,kk+1) - states_ALL(:,ii,kklin) ) + Aii1*( states_ALL(:,ii+1,kk+1) - states_ALL(:,ii+1,kklin) ) ... 156 | + Bii*( controls_ALL(:,ii,kk+1) - controls_ALL(:,ii,kklin) ) + Bii1*( controls_ALL(:,ii+1,kk+1) - controls_ALL(:,ii+1,kklin) ) ... 157 | + dfdtii*dtau/2*( timeFinal_ALL(kk+1) - timeFinal_ALL(kklin) + timeInitial_ALL(kk+1) + timeInitial_ALL(kklin) ) )... 158 | - scaleE*Evirtual*virtualControls_ALL(:,ii,kk); 159 | otherwise 160 | error('SCOPT Error: Collocation method does not exist') 161 | end 162 | end 163 | end 164 | % defectConstraints = defectConstraints/scaleE; 165 | 166 | switch obj.algorithm.sequential.trustRegion.defectsCompErrorPenalty 167 | case 0 168 | defectConstraintsP1 = sum(defectConstraints.*defectConstraints,1); 169 | case {1,2,Inf} 170 | defectConstraintsP1 = vecnorm(defectConstraints,obj.algorithm.sequential.trustRegion.defectsCompErrorPenalty,1); 171 | end 172 | 173 | switch obj.algorithm.sequential.trustRegion.defectsNodeErrorPenalty 174 | case {1,2,Inf} 175 | defectConstraintsP2 = vecnorm(defectConstraintsP1,obj.algorithm.sequential.trustRegion.defectsNodeErrorPenalty,2); 176 | end 177 | 178 | % Figure one is error at each state component at each node at 179 | % each iteration 180 | % - (colours contour for each component) 181 | % - (opacity for iteration) 182 | % - (x axis is each node) 183 | 184 | % Figure two is norm2 error at each node at each iteration 185 | % - (colours contour for each iteration) 186 | % - (x axis is each node) 187 | % Figure three is norm2 error for concatenate full defect 188 | % constraints vector at each iteration 189 | % - (x axis is each iteration) 190 | 191 | 192 | if nargin >= 2 193 | if fig1~=0 194 | figure(fig1) 195 | dontPlot = 0; 196 | else 197 | dontPlot = 1; 198 | end 199 | else 200 | figure 201 | dontPlot = 0; 202 | end 203 | if ~dontPlot 204 | labelLegend = {}; 205 | for jj = 1:nDefects 206 | for kk = 1:iterations 207 | if kk ==iterations 208 | markerEdgeColor = [0 0 0]; 209 | else 210 | markerEdgeColor = defectsjColor(jj,:); 211 | end 212 | hplotAux = semilogy(1:Nodes-1,abs(defectConstraints(jj,:,kk)),'o-','linewidth',1,'color',[1,1,1]*(1-lineOpacity(kk)) + lineOpacity(kk)*defectsjColor(jj,:),'MarkerFaceColor',[1,1,1]*(1-lineOpacity(kk)) + lineOpacity(kk)*defectsjColor(jj,:),'MarkerEdgeColor',markerEdgeColor); hold on 213 | end 214 | hplot(jj) = hplotAux; 215 | labelLegend = [labelLegend , ['Defect $j$ = ' num2str(jj)]]; 216 | end 217 | ax = gca; 218 | ax.YGrid = 'on'; 219 | ax.YMinorGrid = 'on'; 220 | xlabel('Defect $i$ [-]','interpreter','latex') 221 | ylabel('Error [-]','interpreter','latex') 222 | legend(hplot,labelLegend,'location','best','interpreter','latex') 223 | scoptSave2pdf(obj,'verificationDefectsj') 224 | hfig1 = gcf; 225 | else 226 | hfig1 = nan; 227 | end 228 | if nargin >= 3 229 | if fig2~=0 230 | figure(fig2) 231 | dontPlot = 0; 232 | else 233 | dontPlot = 1; 234 | end 235 | else 236 | figure 237 | dontPlot = 0; 238 | end 239 | if ~dontPlot 240 | labelLegend = {}; 241 | for kk = 1:iterations 242 | hplot(kk) = semilogy(1:Nodes-1,defectConstraintsP1(1,:,kk),'-*','linewidth',2,'color',iterationsColor(kk,:)); hold on 243 | labelLegend = [labelLegend , ['$k$ = ' num2str(kk)]]; 244 | end 245 | ax = gca; 246 | ax.YGrid = 'on'; 247 | ax.YMinorGrid = 'on'; 248 | xlabel('Defect $i$ [-]','interpreter','latex') 249 | ylabel('Error [-]','interpreter','latex') 250 | legend(hplot,labelLegend,'location','best','interpreter','latex') 251 | scoptSave2pdf(obj,'verificationDefectsPC') 252 | hfig2 = gcf; 253 | else 254 | hfig2 = nan; 255 | end 256 | if nargin >= 4 257 | if fig3~=0 258 | figure(fig3) 259 | dontPlot = 0; 260 | else 261 | dontPlot = 1; 262 | end 263 | else 264 | figure 265 | dontPlot = 0; 266 | end 267 | if ~dontPlot 268 | labelLegend = {}; 269 | hplotP2(1) = semilogy(1:iterations,squeeze(defectConstraintsP2),'-*','linewidth',2); hold on 270 | ax = gca; 271 | ax.YGrid = 'on'; 272 | ax.YMinorGrid = 'on'; 273 | xlabel('Iteration $k$ [-]','interpreter','latex') 274 | ylabel('Error [-]','interpreter','latex') 275 | scoptSave2pdf(obj,'verificationDefectsPN') 276 | hfig3 = gcf; 277 | else 278 | hfig3 = nan; 279 | end 280 | end 281 | 282 | 283 | -------------------------------------------------------------------------------- /src/@scopt2/plotVirtualBuffersErrorHistory.m: -------------------------------------------------------------------------------- 1 | function [virtualBuffersPathErrorPs,virtualBuffersPathErrorP,virtualBuffersEventsErrorP,hfig1,hfig2] = plotVirtualBuffersErrorHistory(obj,fig1,fig2) 2 | % generates plots to verify virtual buffers 3 | if obj.algorithm.virtualBuffer.phases(1).include 4 | 5 | virtualBuffersPathPs = obj.debugging.virtualBuffers.pathPs; 6 | virtualBuffersPathP = obj.debugging.virtualBuffers.pathP; 7 | virtualBuffersEventsP = obj.debugging.virtualBuffers.eventsP; 8 | 9 | [virtualBuffersPathPs_ALL,virtualBuffersPathP_ALL,virtualBuffersEventsP_ALL] = plotVirtualBuffersHistory(obj,0,0); 10 | 11 | iterations = obj.solution.iterations; 12 | Nodes = size(virtualBuffersPathPs,2); 13 | NvbPath = size(virtualBuffersPathPs,1); 14 | NvbEvent = size(virtualBuffersEventsP,1); 15 | 16 | 17 | defaultColorPath = autumn(NvbPath); 18 | defaultColorEvent = winter(NvbEvent); 19 | lineOpacity = linspace(0,1,iterations+1);lineOpacity(1) = []; 20 | 21 | virtualBuffersPathErrorPs = abs(virtualBuffersPathPs - virtualBuffersPathPs_ALL); 22 | virtualBuffersPathErrorP = abs(virtualBuffersPathP - virtualBuffersPathP_ALL); 23 | virtualBuffersEventsErrorP = abs(virtualBuffersEventsP - virtualBuffersEventsP_ALL); 24 | 25 | if nargin >= 2 26 | if fig1~=0 27 | figure(fig1) 28 | dontPlot = 0; 29 | else 30 | dontPlot = 1; 31 | end 32 | else 33 | figure 34 | dontPlot = 0; 35 | end 36 | if ~dontPlot && (NvbPath)>0 37 | labelLegend = {}; 38 | for jj = 1:NvbPath 39 | for kk = 1:iterations 40 | hplotAux = semilogy(1:Nodes,virtualBuffersPathErrorPs(jj,:,kk),'o-','linewidth',1,'Color',[1,1,1]*(1-lineOpacity(kk)) + lineOpacity(kk)*defaultColorPath(jj,:),'MarkerFaceColor',[1,1,1]*(1-lineOpacity(kk)) + lineOpacity(kk)*defaultColorPath(jj,:),'MarkerEdgeColor',defaultColorPath(jj,:)); hold on 41 | end 42 | hplot(jj) = hplotAux; 43 | labelLegend = [labelLegend , ['Path Buffer Zone $j$ = ' num2str(jj)]]; 44 | end 45 | 46 | ax = gca; 47 | ax.YGrid = 'on'; 48 | ax.YMinorGrid = 'on'; 49 | xlabel('Node $i$ [-]','interpreter','latex') 50 | ylabel('$|\max{\left(0,\xi_{g,i,j}\right)} - \xi^{P1}_{g,i,j}|$ [-]','interpreter','latex') 51 | legend(hplot,labelLegend,'location','best','interpreter','latex') 52 | scoptSave2pdf(obj,'virtualBuffersPathPenaltyMaxErrors') 53 | hfig1 = gcf; 54 | else 55 | hfig1 = nan; 56 | end 57 | if nargin >= 3 58 | if fig2~=0 59 | figure(fig2) 60 | dontPlot = 0; 61 | else 62 | dontPlot = 1; 63 | end 64 | else 65 | figure 66 | dontPlot = 0; 67 | end 68 | if ~dontPlot && (NvbPath+NvbEvent)>0 69 | labelLegend = {}; 70 | for jj = 1:NvbPath 71 | hplot(jj) = semilogy(1:iterations,squeeze(virtualBuffersPathErrorP(jj,:)),'o-','linewidth',1,'color',defaultColorPath(jj,:)); hold on 72 | labelLegend = [labelLegend , ['Path Buffer Zone $j$ = ' num2str(jj)]]; 73 | end 74 | 75 | for jj = 1:NvbEvent 76 | hplot(jj+NvbPath) = semilogy(1:iterations,squeeze(virtualBuffersEventsErrorP(jj,:)),'*-','linewidth',1,'color',defaultColorEvent(jj,:)); hold on 77 | labelLegend = [labelLegend , ['Event Buffer Zone $j$ = ' num2str(jj)]]; 78 | end 79 | ax = gca; 80 | ax.YGrid = 'on'; 81 | ax.YMinorGrid = 'on'; 82 | xlabel('Iteration $k$ [-]','interpreter','latex') 83 | ylabel('$|P^n\left(\max{\left(0,\xi_{g,j}\right)}\right)-\xi^{P2}_{e,j}|,\max{\left(0,\xi_{e,j}\right)} - \xi^{P1}_{e,j}$ [-]','interpreter','latex') 84 | legend(hplot,labelLegend,'location','best','interpreter','latex') 85 | scoptSave2pdf(obj,'virtualBufferZonesFinalPenalty') 86 | hfig2 = gcf; 87 | else 88 | hfig2 = nan; 89 | end 90 | else 91 | hfig1 = figure(fig1);set(hfig1, 'Visible','off') 92 | hfig2 = figure(fig2);set(hfig2, 'Visible','off') 93 | virtualBuffersPathErrorPs = nan; 94 | virtualBuffersPathErrorP = nan; 95 | virtualBuffersEventsErrorP = nan; 96 | end 97 | end 98 | 99 | 100 | -------------------------------------------------------------------------------- /src/@scopt2/plotVirtualBuffersHistory.m: -------------------------------------------------------------------------------- 1 | function [virtualBuffersPathP1_ALL,virtualBuffersPathP2_ALL,virtualBuffersEventsP1_ALL,hfig1,hfig2] = plotVirtualBuffersHistory(obj,fig1,fig2) 2 | % plots virtual buffers slack variable values 3 | if obj.algorithm.virtualBuffer.phases(1).include 4 | 5 | virtualBuffersPath = obj.debugging.virtualBuffers.path; 6 | virtualBuffersEvent = obj.debugging.virtualBuffers.events; 7 | 8 | iterations = obj.solution.iterations; 9 | Nodes = size(virtualBuffersPath,2); 10 | NvbPath = size(virtualBuffersPath,1); 11 | NvbEvent = size(virtualBuffersEvent,1); 12 | 13 | defaultColorPath = autumn(NvbPath); 14 | defaultColorEvent = winter(NvbEvent); 15 | lineOpacity = linspace(0,1,iterations+1);lineOpacity(1) = []; 16 | 17 | virtualBuffersPathP1_ALL = max(0,virtualBuffersPath); 18 | virtualBuffersEventsP1_ALL = max(0,virtualBuffersEvent); 19 | virtualBuffersPathP2_ALL = reshape(sum(virtualBuffersPathP1_ALL,2),NvbPath,iterations); 20 | 21 | if nargin >= 2 22 | if fig1~=0 23 | figure(fig1) 24 | dontPlot = 0; 25 | else 26 | dontPlot = 1; 27 | end 28 | else 29 | figure 30 | dontPlot = 0; 31 | end 32 | if ~dontPlot && NvbPath>0 33 | labelLegend = {}; 34 | for jj = 1:NvbPath 35 | for kk = 1:iterations 36 | hplotAux = semilogy(1:Nodes,virtualBuffersPathP1_ALL(jj,:,kk),'o-','linewidth',1,'color',[1,1,1]*(1-lineOpacity(kk)) + lineOpacity(kk)*defaultColorPath(jj,:),'MarkerFaceColor',[1,1,1]*(1-lineOpacity(kk)) + lineOpacity(kk)*defaultColorPath(jj,:),'MarkerEdgeColor',defaultColorPath(jj,:)); hold on 37 | end 38 | hplot(jj) = hplotAux; 39 | labelLegend = [labelLegend , ['Path Buffer Zone $j$ = ' num2str(jj)]]; 40 | end 41 | 42 | ax = gca; 43 | ax.YGrid = 'on'; 44 | ax.YMinorGrid = 'on'; 45 | xlabel('Node $i$ [-]','interpreter','latex') 46 | ylabel('$\max{\left(0,\xi_{g,i,j}\right)}$ [-]','interpreter','latex') 47 | legend(hplot,labelLegend,'location','best','interpreter','latex') 48 | scoptSave2pdf(obj,'virtualBuffersPathPenaltyMax') 49 | hfig1 = gcf; 50 | else 51 | hfig1 = nan; 52 | end 53 | if nargin >= 3 54 | if fig2~=0 55 | figure(fig2) 56 | dontPlot = 0; 57 | else 58 | dontPlot = 1; 59 | end 60 | else 61 | figure 62 | dontPlot = 0; 63 | end 64 | if ~dontPlot && (NvbPath+NvbEvent)>0 65 | labelLegend = {}; 66 | for jj = 1:NvbPath 67 | hplot2(jj) = semilogy(1:iterations,squeeze(virtualBuffersPathP2_ALL(jj,:)),'o-','linewidth',1,'color',defaultColorPath(jj,:)); hold on 68 | labelLegend = [labelLegend , ['Path Buffer Zone $j$ = ' num2str(jj)]]; 69 | end 70 | 71 | for jj = 1:NvbEvent 72 | hplot2(jj+NvbPath) = semilogy(1:iterations,squeeze(virtualBuffersEventsP1_ALL(jj,:)),'*-','linewidth',1,'color',defaultColorEvent(jj,:)); hold on 73 | labelLegend = [labelLegend , ['Event Buffer Zone $j$ = ' num2str(jj)]]; 74 | end 75 | ax = gca; 76 | ax.YGrid = 'on'; 77 | ax.YMinorGrid = 'on'; 78 | xlabel('Iteration $k$ [-]','interpreter','latex') 79 | ylabel('$P^n\left(\max{\left(0,\xi_{g,j}\right)}\right),\max{\left(0,\xi_{e,j}\right)}$ [-]','interpreter','latex') 80 | legend(hplot2,labelLegend,'location','best','interpreter','latex') 81 | scoptSave2pdf(obj,'virtualBufferZonesFinalPenalty') 82 | hfig2 = gcf; 83 | else 84 | hfig2 = nan; 85 | end 86 | else 87 | hfig1 = figure(fig1);set(hfig1, 'Visible','off') 88 | hfig2 = figure(fig2);set(hfig2, 'Visible','off') 89 | virtualBuffersPathP1_ALL = nan; 90 | virtualBuffersPathP2_ALL = nan; 91 | virtualBuffersEventsP1_ALL = nan; 92 | end 93 | end 94 | 95 | 96 | -------------------------------------------------------------------------------- /src/@scopt2/plotVirtualControlErrorHistory.m: -------------------------------------------------------------------------------- 1 | function [virtualControlsErrorP1_ALL,virtualControlsErrorP2_ALL,hfig1,hfig2] = plotVirtualControlErrorHistory(obj,fig1,fig2) 2 | % generates figures to verify virtual control errors, values 3 | % should have small numbers 4 | if obj.algorithm.virtualControl.phases.include 5 | virtualControlsP1 = obj.debugging.virtualControlsP1; 6 | virtualControlsP2 = obj.debugging.virtualControlsP2; 7 | iterations = obj.solution.iterations; 8 | defaultColor = autumn(iterations); 9 | 10 | Nodes = size(virtualControlsP1,2); 11 | [virtualControlsP1_ALL,virtualControlsP2_ALL] = computePenaltiesVirtualControls(obj,obj.debugging.virtualControls); 12 | 13 | virtualControlsErrorP1_ALL = abs(virtualControlsP1 - virtualControlsP1_ALL); 14 | virtualControlsErrorP2_ALL = abs(virtualControlsP2 - virtualControlsP2_ALL); 15 | 16 | if nargin >= 2 17 | if fig1~=0 18 | figure(fig1) 19 | dontPlot = 0; 20 | else 21 | dontPlot = 1; 22 | end 23 | else 24 | figure 25 | dontPlot = 0; 26 | end 27 | if ~dontPlot 28 | labelLegend = {}; 29 | for kk = 1:iterations 30 | hplot(kk) = semilogy(1:Nodes,virtualControlsErrorP1_ALL(1,:,kk),'-*','linewidth',2,'color',defaultColor(kk,:)); hold on 31 | labelLegend = [labelLegend , ['$k$ = ' num2str(kk)]]; 32 | end 33 | ax = gca; 34 | ax.YGrid = 'on'; 35 | ax.YMinorGrid = 'on'; 36 | xlabel('Node $i$ [-]','interpreter','latex') 37 | ylabel('$|P^c\left(E\nu_i\right)-\nu^{P1}|$ [-]','interpreter','latex') 38 | legend(hplot,labelLegend,'location','best','interpreter','latex') 39 | scoptSave2pdf(obj,'virtualControlsPenaltyCErrors') 40 | hfig1 = gcf; 41 | else 42 | hfig1 = nan; 43 | end 44 | if nargin >= 3 45 | if fig2~=0 46 | figure(fig2) 47 | dontPlot = 0; 48 | else 49 | dontPlot = 1; 50 | end 51 | else 52 | figure 53 | dontPlot = 0; 54 | end 55 | if ~dontPlot 56 | semilogy(1:iterations,squeeze(virtualControlsErrorP2_ALL),'-*','linewidth',2); hold on 57 | ax = gca; 58 | ax.YGrid = 'on'; 59 | ax.YMinorGrid = 'on'; 60 | xlabel('Iteration $k$ [-]','interpreter','latex') 61 | ylabel('$|P^n\left(P^c\left(E\nu\right)\right)-\nu^{P2}|$ [-]','interpreter','latex') 62 | scoptSave2pdf(obj,'virtualControlsPenaltyNErrors') 63 | hfig2 = gcf; 64 | else 65 | hfig2 = nan; 66 | end 67 | end 68 | end 69 | 70 | 71 | -------------------------------------------------------------------------------- /src/@scopt2/plotVirtualControlHistory.m: -------------------------------------------------------------------------------- 1 | function [virtualControlsP1_ALL,virtualControlsP2_ALL,hfig1,hfig2] = plotVirtualControlHistory(obj,fig1,fig2) 2 | % Plots virtual controls internal slack variable. User has to 3 | % verify their values are close to zero or nonsignificant 4 | % during the last iterations 5 | if obj.algorithm.virtualControl.phases(1).include 6 | [virtualControlsP1_ALL,virtualControlsP2_ALL] = computePenaltiesVirtualControls(obj,obj.debugging.virtualControls); 7 | 8 | defaultColor = autumn(obj.solution.iterations); 9 | if nargin >= 2 10 | if fig1~=0 11 | figure(fig1) 12 | dontPlot = 0; 13 | else 14 | dontPlot = 1; 15 | end 16 | else 17 | figure 18 | dontPlot = 0; 19 | end 20 | if ~dontPlot 21 | labelLegend = {}; 22 | for kk = 1:obj.solution.iterations 23 | semilogy(1:(obj.solution.Nodes-1),virtualControlsP1_ALL(1,:,kk),'-*','linewidth',2,'color',defaultColor(kk,:)); hold on 24 | labelLegend = [labelLegend , ['$k$ = ' num2str(kk)]]; 25 | end 26 | ax = gca; 27 | ax.YGrid = 'on'; 28 | ax.YMinorGrid = 'on'; 29 | xlabel('Node $i$ [-]','interpreter','latex') 30 | ylabel('$P^c\left(E\nu_i\right)$ [-]','interpreter','latex') 31 | legend(labelLegend,'location','best','interpreter','latex') 32 | scoptSave2pdf(obj,'virtualControlsPenaltyC') 33 | hfig1 = gcf; 34 | else 35 | hfig1 = nan; 36 | end 37 | if nargin >= 3 38 | if fig2~=0 39 | figure(fig2) 40 | dontPlot = 0; 41 | else 42 | dontPlot = 1; 43 | end 44 | else 45 | figure 46 | dontPlot = 0; 47 | end 48 | if ~dontPlot 49 | semilogy(1:obj.solution.iterations,squeeze(virtualControlsP2_ALL),'-*','linewidth',2); 50 | ax = gca; 51 | ax.YGrid = 'on'; 52 | ax.YMinorGrid = 'on'; 53 | xlabel('Iteration $k$ [-]','interpreter','latex') 54 | ylabel('$P^n\left(P^c\left(E\nu\right)\right)$ [-]','interpreter','latex') 55 | legend(labelLegend,'location','best','interpreter','latex') 56 | scoptSave2pdf(obj,'virtualControlsPenaltyN') 57 | hfig2 = gcf; 58 | else 59 | hfig2 = nan; 60 | end 61 | else 62 | hfig1 = nan; 63 | hfig2 = nan; 64 | end 65 | end 66 | 67 | 68 | -------------------------------------------------------------------------------- /src/@scopt2/scaleControlsToNormalized.m: -------------------------------------------------------------------------------- 1 | function Us_n = scaleControlsToNormalized(obj,ph,Us,index) 2 | % Scales controls 3 | scale = obj.problem.phases(ph).scale.controls; 4 | shift = obj.problem.phases(ph).shift.controls; 5 | if nargin<4 6 | index = (1:obj.problem.phases(ph).n.controls)'; 7 | end 8 | if isempty(Us) 9 | Us_n = Us; 10 | else 11 | Nodes= size(Us,2); 12 | Us_n = repmat((1./scale(index)),1,Nodes).*(Us - repmat(shift(index),1,Nodes)) ; 13 | end 14 | end 15 | 16 | -------------------------------------------------------------------------------- /src/@scopt2/scaleControlsToReal.m: -------------------------------------------------------------------------------- 1 | function Us = scaleControlsToReal(obj,ph,Us_n,index) 2 | % Unscales controls and retrieves actual values 3 | scale = obj.problem.phases(ph).scale.controls; 4 | shift = obj.problem.phases(ph).shift.controls; 5 | if nargin<4 6 | index = (1:obj.problem.phases(ph).n.controls)'; 7 | end 8 | Nodes = size(Us_n,2); 9 | Us = repmat(scale(index),1,Nodes).*Us_n + repmat(shift(index),1,Nodes) ; 10 | end 11 | 12 | -------------------------------------------------------------------------------- /src/@scopt2/scaleStatesToNormalized.m: -------------------------------------------------------------------------------- 1 | function Xs_n = scaleStatesToNormalized(obj,ph,Xs,index) 2 | % Scales states 3 | scale = obj.problem.phases(ph).scale.states; 4 | shift = obj.problem.phases(ph).shift.states; 5 | if nargin<4 6 | index = (1:obj.problem.phases(ph).n.states)'; 7 | end 8 | if isempty(Xs) 9 | Xs_n = Xs; 10 | else 11 | Nodes = size(Xs,2); 12 | Xs_n = repmat((1./scale(index)),1,Nodes).*(Xs - repmat(shift(index),1,Nodes)) ; 13 | end 14 | end 15 | 16 | -------------------------------------------------------------------------------- /src/@scopt2/scaleStatesToReal.m: -------------------------------------------------------------------------------- 1 | function Xs = scaleStatesToReal(obj,ph,Xs_n,index) 2 | % Unscales states and retrieves actual values 3 | scale = obj.problem.phases(ph).scale.states; 4 | shift = obj.problem.phases(ph).shift.states; 5 | if nargin<4 6 | index = (1:obj.problem.phases(ph).n.states)'; 7 | end 8 | Nodes = size(Xs_n,2); 9 | Xs = repmat(scale(index),1,Nodes).*Xs_n + repmat(shift(index),1,Nodes) ; 10 | end 11 | 12 | -------------------------------------------------------------------------------- /src/@scopt2/scaleTimeToNormalized.m: -------------------------------------------------------------------------------- 1 | function t_n = scaleTimeToNormalized(obj,ph,t) 2 | % Scales time 3 | scale = obj.problem.phases(ph).scale.time; 4 | shift = obj.problem.phases(ph).shift.time; 5 | t_n = (1./scale).*(t - shift) ; 6 | end 7 | 8 | -------------------------------------------------------------------------------- /src/@scopt2/scaleTimeToReal.m: -------------------------------------------------------------------------------- 1 | function t = scaleTimeToReal(obj,ph,t_n) 2 | % Unscales time and retrieves actual values 3 | scale = obj.problem.phases(ph).scale.time; 4 | shift = obj.problem.phases(ph).shift.time; 5 | t = scale.*(t_n) + shift ; 6 | end 7 | 8 | -------------------------------------------------------------------------------- /src/@scopt2/scopt2.m: -------------------------------------------------------------------------------- 1 | %% *SCOPT2* 2 | % Successive Convexification OPtimal ConTrol 2.0 3 | % Optimal control tool to solve single phase autonomous problems 4 | % using successive convexification. 5 | % Copyright (C) 2020 Guillermo J. Dominguez Calabuig 6 | 7 | % This program is free software: you can redistribute it and/or modify 8 | % it under the terms of the GNU General Public License as published by 9 | % the Free Software Foundation, either version 3 of the License, or 10 | % (at your option) any later version. 11 | 12 | % This program is distributed in the hope that it will be useful, 13 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % GNU General Public License for more details. 16 | 17 | % You should have received a copy of the GNU General Public License 18 | % along with this program. If not, see . 19 | 20 | %% Description 21 | % Handle class object defining and solving an optimal control problem 22 | % based on successive convexification and linearisation. Can also solve 23 | % basic SOCP/Linear problems with one iteration. 24 | % It containts three main structures: 25 | % 26 | % * algorithm: Defines the settings necessary for the sequential socp 27 | % solver, as the socp solver settings, virtual controls, buffer 28 | % zones, trust regions, scaling, line search strategies, and 29 | % collocation method 30 | % * problem: Defines the Optimum Control Problem per phase. Currently 31 | % it only supports single phase problems. 32 | % * solution: Stores the solution of the Optimum Control Problem 33 | % 34 | % A full Description is available in the Git Repository: 35 | % https://github.com/guidoca/scopt2.git 36 | 37 | % and in a Master Thesis from the TU Delft Repository: 38 | % 'Optimum On-board Abort Guidance based on Successive Convexification 39 | % for Atmospheric Re-Entry Vehicles', Master Thesis, Guillermo 40 | % Joaquin Dominguez Calabuig. 41 | % 42 | 43 | classdef scopt2 < matlab.mixin.Copyable 44 | 45 | properties (Constant) 46 | infCOPT = 1e19; 47 | MIOFF = 1 ; % Matlab Index Offset 48 | end 49 | properties 50 | algorithm = struct; 51 | problem = struct; 52 | solution = struct; 53 | debugging = struct; 54 | quiet = 0; 55 | end 56 | 57 | methods 58 | %% SCOPT Constructor 59 | function obj = scopt2() 60 | %scopt Construct an instance of the scopt class loading the 61 | %default fields for the problem and algorithm 62 | addpath(genpath('toolbox')); 63 | warning('on','all'); 64 | %% problem Default Settings 65 | obj.problem.nphases = 1 ; % number of phases 66 | for ph = 1:obj.problem.nphases 67 | obj.problem.phases(ph).n.states = []; % number of states, requires initialisation 68 | obj.problem.phases(ph).n.controls = []; % number of controls, requires initialisation 69 | obj.problem.phases(ph).n.parameters = 0 ; % number of parameters, 0 by default 70 | obj.problem.phases(ph).n.events = 0 ; % number of events, 0 by default 71 | obj.problem.phases(ph).n.path = 0 ; % number of path constraints, 0 by default 72 | obj.problem.phases(ph).freeTimeFinal = 0 ; % indicated if free final time phase, false by default 73 | obj.problem.phases(ph).freeTimeInitial = 0 ; % indicated if free initial time phase, false by default 74 | end 75 | %% algorithm Default Settings 76 | % SOCP Solver settings. Defines the maximum number of SOCP 77 | % subiterations, solver type, and feasibility, relative and absolute 78 | % tolerances 79 | obj.algorithm.conicSolver.type = 'ecos'; 80 | obj.algorithm.conicSolver.maxIter = 100; 81 | obj.algorithm.conicSolver.feasTol = eps^(1/2); 82 | obj.algorithm.conicSolver.relTol = eps^(1/2); 83 | obj.algorithm.conicSolver.absTol = eps^(1/2); 84 | 85 | % Sequential algorithm settings 86 | obj.algorithm.sequential.activate = 1; % successive convexification is active by default 87 | obj.algorithm.sequential.type = 'trust-region'; % trust region 88 | obj.algorithm.sequential.maxIter = 100; % Maximum number of successive convexification iterations 89 | obj.algorithm.sequential.minIter = 3; % Minimum number of successive convexification iterations 90 | obj.algorithm.sequential.objTol = 0.05; % Maximum percentage of augmentation to consider a solution feasible, heuristic term. 91 | obj.algorithm.sequential.Jtol = 1e-4; % Stopping condition tolerance for the objective function (if difference is lower than this number, stops) 92 | obj.algorithm.sequential.JtolChange = 1e-4; % Stopping condition tolerance for the objective function (if relative change is lower than this number, stops) 93 | obj.algorithm.sequential.variablesTol.etaTol = 1.0e-4; % Stopping condition tolerance the change in variables 94 | obj.algorithm.sequential.variablesTol.type = 'norm'; % Type of norm for variables and states, norm or componentwise, default is norm 95 | obj.algorithm.sequential.variablesTol.norm = 2; % Type of norm used 96 | obj.algorithm.sequential.variablesTol.include.states = 1; % flag to include states 97 | obj.algorithm.sequential.variablesTol.include.controls = 1; % flag to include controls 98 | obj.algorithm.sequential.variablesTol.include.timeFinal = 1; % flag to include final time 99 | obj.algorithm.sequential.variablesTol.include.timeInitial = 1; % flag to include initial time 100 | 101 | % Sequential algorithm line search settings 102 | obj.algorithm.sequential.globalisation.type = 'line-search'; % line-search 103 | obj.algorithm.sequential.globalisation.activate = 0; % inactive by default 104 | obj.algorithm.sequential.globalisation.lineSearch.type = 'golden-section'; % line-search type, options are contraction, polynomial, and golden section 105 | obj.algorithm.sequential.globalisation.lineSearch.contractionFactor = 0.5; % contraction factor for the contraction line search type 106 | obj.algorithm.sequential.globalisation.lineSearch.directionalDerivativeType = 'numeric';% Numerical directional derivative 107 | obj.algorithm.sequential.globalisation.lineSearch.directionalDerivativeEpsilon = 1e-4; % Used for numerical directional derivative steps, only for contraint type 108 | obj.algorithm.sequential.globalisation.lineSearch.k1 = 1e-4; % Used for contraction line search type 109 | obj.algorithm.sequential.globalisation.lineSearch.k2 = 0.9 ; % Used for contraction line search type 110 | obj.algorithm.sequential.globalisation.lineSearch.bracketTol = 1e-2; % Used for bracketing methods as golden section 111 | obj.algorithm.sequential.globalisation.lineSearch.golden = (sqrt(5)-1)/2; % Golden ratio 112 | obj.algorithm.sequential.globalisation.lineSearch.stepLengthMax = 1.0; % Maximum step length along direction and magnitude defined by SOCP solver 113 | obj.algorithm.sequential.globalisation.lineSearch.forceIfRejected = 1 ; % Flag specifiying if the line search should still be called if the adaptive trust reegion algorithmn has rejected the line search step 114 | obj.algorithm.sequential.globalisation.lineSearch.debugging = 0 ; % Flag to call debugging routines in line search 115 | 116 | % Sequential algorithm trust region settings 117 | obj.algorithm.sequential.trustRegion.defectsCompErrorPenalty = nan ; % 2nd norm, 0 means quadratic norm This is for the computation of the adaptive trust region error 118 | obj.algorithm.sequential.trustRegion.defectsNodeErrorPenalty = nan ; % 2nd norm This is for the computation of the adaptive trust region error 119 | obj.algorithm.sequential.trustRegion.radius_l = eps^(1/2) ; % lower boundary for adaptive hard trust region radius 120 | obj.algorithm.sequential.trustRegion.radius_u = obj.infCOPT ; % upper boundary for adaptive hard trust region radius 121 | obj.algorithm.sequential.trustRegion.lambda_l = eps^(1/2) ; % lower boundary for adaptive soft trust region multiplier 122 | obj.algorithm.sequential.trustRegion.lambda_u = obj.infCOPT ; % upper boundary for adaptive soft trust region multiplier 123 | obj.algorithm.sequential.trustRegion.rho0 = 0.0 ; % Adaptive trust region metric. Lower means it is a bad appropximation and should be rejected 124 | obj.algorithm.sequential.trustRegion.rho1 = 0.2 ; % Adaptive trust region metric. Lower means it is a bad appropximation and hard(soft) trust region should be reduced(increase) 125 | obj.algorithm.sequential.trustRegion.rho2 = 0.6 ; % Adaptive trust region metric. Lower Means approximation is OK. Higher means it is a good approximation and hard(soft) trust region should be increased(reduced) 126 | obj.algorithm.sequential.trustRegion.alpha = 2.0 ; % Adaptive trust region increasing factor for good approximations 127 | obj.algorithm.sequential.trustRegion.beta = 2.0 ; % Adaptive trust region decreasing factor for bad approximations 128 | obj.algorithm.sequential.trustRegion.type = 'hard'; % 'hard' or 'soft' trust region type 129 | obj.algorithm.sequential.trustRegion.adaptive = 0 ; % 0 or 1. Default is a rigid trust region 130 | obj.algorithm.sequential.trustRegion.include.states = 0 ; % 0 or 1. Default does not apply trust region to controls 131 | obj.algorithm.sequential.trustRegion.include.controls = 1 ; % 0 or 1. Default does applies trust region to controls 132 | obj.algorithm.sequential.trustRegion.include.pathAndEventsErrors = 1 ; % 0 or 1 if path and event convexification errors should be included in metric 133 | obj.algorithm.sequential.trustRegion.variablesPenalty = 2 ; % 2nd norm, 0 means quadratic norm for trust region component terms 134 | obj.algorithm.sequential.trustRegion.nodePenalty = inf ; % 2nd norm, norm for trust region node wise terms (on components) 135 | obj.algorithm.sequential.trustRegion.radius = [] ; % Shall be initialised 136 | obj.algorithm.sequential.trustRegion.lambda = [] ; % Shall be initialised 137 | 138 | % Other Algorithm Settings 139 | obj.algorithm.initialGuessType = 'automatic'; % Initial guess type. 'automatic', 'user', or 'none' 140 | obj.algorithm.collocationMethod = 'trapezoidal'; % Collocation method. Numerical types: 'trapezoidal' or 'euler'. Or exact ode solution: 'exact'. 141 | obj.algorithm.scaling.variables = 'automatic'; % Scaling for states and controls variable options. 'automatic', 'user', or 'none' 142 | obj.algorithm.scaling.events = 'automatic-jacobian'; % Alternative is 'none' 'user' 'automatic-limit' 'automatic-jacobian' 143 | obj.algorithm.scaling.path = 'automatic-jacobian'; % Alternative is 'none' 'user' 'automatic-limit' 'automatic-jacobian' 144 | obj.algorithm.scaling.objective = 'automatic-jacobian'; % Alternative is 'automatic-jacobian' 145 | obj.algorithm.scaling.trustRegion = 'none'; % Alternative is 'automatic' or 'none' 146 | obj.algorithm.meshRefinement = 'none'; % No Mesh Refinement Implemented (YET) 147 | obj.algorithm.quadrature.type = 'trapezoidal'; % Quadrature types for integral terms. Alternative is 'euler' 148 | 149 | 150 | obj.algorithm.speedUp = 0; % To avoid double calculations in objective, dynamics, states and control variables. Default is inactive 151 | 152 | %% Default Post Processing Options in Solution Struct 153 | obj.solution.outputFolder = [pwd '\scoptOutput']; % outpud folder directory 154 | obj.solution.figuresFolder = 'Figures'; % Postprocessing Figures Folder Directioy 155 | obj.solution.dataFolder = 'Data'; % Postprocessing Data Folder Directory 156 | obj.solution.saveFigures = 1; % Flag to save figures (enabled by default) 157 | obj.solution.generateFigures= 0; % Flag to generate figures (disabled by default) 158 | obj.solution.generateOutput = 1; % Flag to generate output (enabled by default) 159 | obj.solution.figuresQuality = 600; % quality of figures in pdf format 160 | %% Debugging options 161 | obj.debugging.saveVirtualControls = 1; % flag to save virtual control verification monitors 162 | obj.debugging.saveTrustRegion = 1; % flag to save trust region verification monitors 163 | obj.debugging.saveVirtualBuffers = 1; % flag to save virtual buffers verification monitors 164 | obj.debugging.saveVariableConvergence= 1; % flag to save variable convergence monitor 165 | obj.debugging.generateFigureDefects = 1; % flag to generate figures for the defect constraints (dynamics) 166 | obj.debugging.internalVerifications = 1; % flag to do all of the above 167 | end 168 | %% Setup Methods 169 | obj = setUpSCOPT_level1(obj) 170 | obj = setUpSCOPT_level2(obj) 171 | 172 | %% Solve Methods 173 | solveSCOPTProblem(obj); 174 | problemTranscription(obj,timeInitial_n,timeFinal_n,X_n,U_n,radius,lambda) 175 | solverOutput = solveSOCPProblem(obj) 176 | 177 | %% Globalisation Methods 178 | Z = computeNewPoint(~,searchDirection,stepLength,Z_p) 179 | M = computeMeritFunction(obj,searchDirection,stepLength,Z_p) 180 | directionalDerivative = computeDirectionalDerivative(obj,searchDirection,Z_p,M_p) 181 | sufficientDecrease = evaluateSufficientDecrease(obj,searchDirection,stepLength,directionalDerivative,Z_p,M_p) 182 | [radius,lambda,rejected] = adaptTrustRegion(obj,radius,lambda,rho,numericalIssues) 183 | 184 | %% Objective Function Related Methods 185 | [J,L,Jreal,Lreal] = computeObjectiveFunctionsNormal(obj,time_n,X_n,U_n,time_n_p,X_n_p,U_n_p) 186 | [Jreal,Lreal] = computeObjectiveFunctions(obj,time,X,U,time_p,X_p,U_p) 187 | [Jaug,Laug] = computeAugmentedObjectiveFunctions(obj,time_n,X_n,U_n,time_n_p,X_n_p,U_n_p,linearToo) 188 | [Reg] = computeRegularisation(obj,~,X_n,U_n) 189 | [penaltyDefectsNonLinear,penaltyBuffersNonLinear,penaltyTrustRegion,penaltyTime,penaltyDefectsLinear,penaltyBuffersLinear] = computeAugmentedObjectiveFunctionTerms(obj,time_n,X_n,U_n,time_n_p,X_n_p,U_n_p,linearToo) 190 | %% Termination Auxiliary Methods 191 | varTol = terminationVariableConvergence(obj,timeInitial_n_opt,timeFinal_n_opt,X_n_opt,U_n_opt,timeInitial_n_prev,timeFinal_n_prev,X_n_prev,U_n_prev) 192 | %% Variables Scaling Methods 193 | Xs_n = scaleStatesToNormalized(obj,ph,Xs,index) 194 | Xs = scaleStatesToReal(obj,ph,Xs_n,index) 195 | Us_n = scaleControlsToNormalized(obj,ph,Us,index) 196 | Us = scaleControlsToReal(obj,ph,Us_n,index) 197 | t_n = scaleTimeToNormalized(obj,ph,t) 198 | t = scaleTimeToReal(obj,ph,t_n) 199 | %% Postprocessing Methods 200 | generateFigures(obj) 201 | [hfig1,hfig2,hfig3] = plotVerificationDefectConstraints(obj,fig1,fig2,fig3) 202 | [trustRegionErrorP1_ALL,trustRegionErrorP2_ALL,hfig1,hfig2] = plotTrustRegionErrorHistory(obj,fig1,fig2) 203 | [trustRegionP1_ALL,trustRegionP2_ALL,hfig1,hfig2] = plotTrustRegionHistory(obj,fig1,fig2) 204 | [virtualBuffersPathErrorPs,virtualBuffersPathErrorP,virtualBuffersEventsErrorP,hfig1,hfig2] = plotVirtualBuffersErrorHistory(obj,fig1,fig2) 205 | [virtualBuffersPathP1_ALL,virtualBuffersPathP2_ALL,virtualBuffersEventsP1_ALL,hfig1,hfig2] = plotVirtualBuffersHistory(obj,fig1,fig2) 206 | [virtualControlsErrorP1_ALL,virtualControlsErrorP2_ALL,hfig1,hfig2] = plotVirtualControlErrorHistory(obj,fig1,fig2) 207 | [virtualControlsP1,virtualControlsP2] = computePenaltiesVirtualControls(obj,virtualControls) 208 | [virtualControlsP1_ALL,virtualControlsP2_ALL,hfig1,hfig2] = plotVirtualControlHistory(obj,fig1,fig2) 209 | variablesNorm = computeConvergenceHistory(obj) 210 | hfig = plotConvergenceHistory(obj,fig,variablesNorm) 211 | hfig = plotRealObjective(obj,fig) 212 | hfig = plotTime(obj,fig) 213 | hfig = plotAdaptiveTrustRegionMonitor(obj,fig) 214 | hfig = plotSolutionMatrices(obj) 215 | scoptSave2pdf(obj,pdfFileName) 216 | 217 | 218 | end 219 | %% Static Methods 220 | methods (Static) 221 | %% Create Cone 222 | function [s] = initCone(dimensions,n,Nodes) 223 | % Initializes a cone with given deimensions, for the states, 224 | % controls and parameters and for multidimensional number of 225 | % nodes if necessary 226 | if ~exist('Nodes','var') 227 | Nodes = 1; 228 | end 229 | s.nodes = Nodes; 230 | s.dimensions = dimensions; 231 | s.norm.cons = zeros(dimensions-1,Nodes); 232 | s.right.cons = zeros(1,Nodes); 233 | 234 | s.norm.states = zeros(n.states,dimensions-1,Nodes); 235 | s.norm.controls = zeros(n.controls,dimensions-1,Nodes); 236 | s.norm.parameters = zeros(n.parameters,dimensions-1,Nodes); 237 | s.right.states = zeros(n.states,Nodes); 238 | s.right.controls = zeros(n.controls,Nodes); 239 | s.right.parameters = zeros(n.parameters,Nodes); 240 | end 241 | %% Postprocessing Static Methods 242 | plotNonZeroEntries(MAT,figNum,index) 243 | plotNonZeroEntriesAB(A,b,figNum,index) 244 | end 245 | end 246 | 247 | -------------------------------------------------------------------------------- /src/@scopt2/scoptSave2pdf.m: -------------------------------------------------------------------------------- 1 | function scoptSave2pdf(obj,pdfFileName) 2 | %SAVE2PDF Saves a figure as a properly cropped pdf 3 | % 4 | % save2pdf(pdfFileName,handle,dpi) 5 | % 6 | % - pdfFileName: Destination to write the pdf to. 7 | % - handle: (optional) Handle of the figure to write to a pdf. If 8 | % omitted, the current figure is used. Note that handles 9 | % are typically the figure number. 10 | % - dpi: (optional) Integer value of dots per inch (DPI). Sets 11 | % resolution of output pdf. Note that 150 dpi is the Matlab 12 | % default and this function's default, but 600 dpi is typical for 13 | % production-quality. 14 | % 15 | % Saves figure as a pdf with margins cropped to match the figure size. 16 | 17 | % (c) Gabe Hoffmann, gabe.hoffmann@gmail.com 18 | % Written 8/30/2007 19 | % Revised 9/22/2007 20 | % Revised 1/14/2007 21 | % (c) G. J. Dominguez C., guilledcalabuig@gmail.com 22 | % Modified 20/11/2019 23 | 24 | % Verify correct number of arguments 25 | if obj.solution.saveFigures 26 | error(nargchk(0,3,nargin)); 27 | 28 | % If no handle is provided, use the current figure as default 29 | if nargin<2 30 | [fileName,pathName] = uiputfile('*.pdf','Save to PDF file:'); 31 | if fileName == 0; return; end 32 | pdfFileName = [pathName,fileName]; 33 | end 34 | handle = gcf; 35 | 36 | dpi = obj.solution.figuresQuality; 37 | 38 | pdfFileName = [obj.solution.outputFolder '/' obj.solution.figuresFolder '/' pdfFileName]; 39 | 40 | if ~exist(obj.solution.outputFolder, 'dir') 41 | mkdir(obj.solution.outputFolder) 42 | end 43 | 44 | if ~exist([obj.solution.outputFolder '/' obj.solution.figuresFolder], 'dir') 45 | mkdir([obj.solution.outputFolder '/' obj.solution.figuresFolder]) 46 | end 47 | 48 | if exist(pdfFileName,'file') 49 | delete(pdfFileName) 50 | end 51 | 52 | % Backup previous settings 53 | prePaperType = get(handle,'PaperType'); 54 | prePaperUnits = get(handle,'PaperUnits'); 55 | preUnits = get(handle,'Units'); 56 | prePaperPosition = get(handle,'PaperPosition'); 57 | prePaperSize = get(handle,'PaperSize'); 58 | 59 | set(handle, 'Renderer', 'painters'); 60 | 61 | % Make changing paper type possible 62 | set(handle,'PaperType',''); 63 | 64 | % Set units to all be the same 65 | set(handle,'PaperUnits','inches'); 66 | set(handle,'Units','inches'); 67 | 68 | % Set the page size and position to match the figure's dimensions 69 | paperPosition = get(handle,'PaperPosition'); 70 | position = get(handle,'Position'); 71 | set(handle,'PaperPosition',[0,0,position(3:4)]); 72 | set(handle,'PaperSize',position(3:4)); 73 | 74 | % Save the pdf (this is the same method used by "saveas") 75 | print(handle,'-dpdf',pdfFileName,sprintf('-r%d',dpi)) 76 | 77 | % Restore the previous settings 78 | set(handle,'PaperType',prePaperType); 79 | set(handle,'PaperUnits',prePaperUnits); 80 | set(handle,'Units',preUnits); 81 | set(handle,'PaperPosition',prePaperPosition); 82 | set(handle,'PaperSize',prePaperSize); 83 | end 84 | end 85 | 86 | -------------------------------------------------------------------------------- /src/@scopt2/setUpSCOPT_level1.m: -------------------------------------------------------------------------------- 1 | function obj = setUpSCOPT_level1(obj) 2 | %% Level 1 SCOPT Setup 3 | % Setup Fields with Default Values based on the number of 4 | % states, controls, parameters, free and final initial time 5 | % fields, and phases 6 | for ph = 0:obj.problem.nphases-1 7 | % Retrieve Phase Sepcific fields 8 | numberOfStates = obj.problem.phases(ph+obj.MIOFF).n.states ; 9 | numberOfControls = obj.problem.phases(ph+obj.MIOFF).n.controls ; 10 | numberOfParameters = obj.problem.phases(ph+obj.MIOFF).n.parameters ; 11 | numberOfNodes = obj.problem.phases(ph+obj.MIOFF).Nodes ; 12 | 13 | % Only active in the phase if speedUp flag is enabled. Runs 14 | % some routines before. Not necessary anymroe with 15 | % griddedInterpolant fast use 16 | obj.problem.phases(ph+obj.MIOFF).interpolateParameters = 1; 17 | 18 | % Tolerances for each variable if specified as desired 19 | % tolernace criterion 20 | obj.algorithm.sequential.variablesTol.phases(ph+obj.MIOFF).states = eps^(1/4)*ones(numberOfStates,1); %[-] 21 | obj.algorithm.sequential.variablesTol.phases(ph+obj.MIOFF).controls = eps^(1/4)*ones(numberOfControls,1); %[-] 22 | obj.algorithm.sequential.variablesTol.phases(ph+obj.MIOFF).timeFinal = eps^(1/4)*ones(1,1); %[-] 23 | obj.algorithm.sequential.variablesTol.phases(ph+obj.MIOFF).timeInitial = eps^(1/4)*ones(1,1); %[-] 24 | 25 | % Dynamics functions. Default uses sparse matrix 26 | % representation for state jacobians and control 27 | % jacobians. Dependency on ii (node number) has to be 28 | % specified by user 29 | obj.problem.phases(ph+obj.MIOFF).dynamics.stateDerivativeFunction = @(t,X,U,BodyMap,ii) nan(numberOfStates,length(t)); 30 | obj.problem.phases(ph+obj.MIOFF).dynamics.typeStateMatrix = 1; % 0 for 3D, 1 for sparse representation 31 | obj.problem.phases(ph+obj.MIOFF).dynamics.stateMatrixFunction = @(t,X,U,BodyMap,ii) nan(numberOfStates,numberOfStates,length(t)); 32 | obj.problem.phases(ph+obj.MIOFF).dynamics.typeControlMatrix = 1; % 0 for 3D, 1 for sparse representation 33 | obj.problem.phases(ph+obj.MIOFF).dynamics.controlMatrixFunction = @(t,X,U,BodyMap,ii) nan(numberOfStates,numberOfControls,length(t)); 34 | obj.problem.phases(ph+obj.MIOFF).dynamics.timeDerivativeFunction = @(t,X,U,BodyMap,ii) zeros(numberOfStates,length(t)); 35 | obj.problem.phases(ph+obj.MIOFF).dynamics.typeParameterMatrix = 0; % 0 for 3D, 1 for sparse representation 36 | obj.problem.phases(ph+obj.MIOFF).dynamics.parametersMatrixFunction = @(t,X,U,BodyMap,ii) nan(numberOfStates,numberOfParameters,length(t)); 37 | 38 | % Initialize Scaling Terms 39 | obj.problem.phases(ph+obj.MIOFF).scale.states = ones(numberOfStates,1); 40 | obj.problem.phases(ph+obj.MIOFF).scale.controls = ones(numberOfControls,1); 41 | obj.problem.phases(ph+obj.MIOFF).scale.time = 1; 42 | obj.problem.phases(ph+obj.MIOFF).shift.states = zeros(numberOfStates,1); 43 | obj.problem.phases(ph+obj.MIOFF).shift.controls = zeros(numberOfControls,1); 44 | obj.problem.phases(ph+obj.MIOFF).shift.time = 0; 45 | 46 | % Initialzes Initial Conditions 47 | obj.problem.phases(ph+obj.MIOFF).initial.states.equal.index = []; 48 | obj.problem.phases(ph+obj.MIOFF).initial.states.equal.value = zeros(obj.problem.phases(ph+obj.MIOFF).initial.states.equal.index,1); 49 | obj.problem.phases(ph+obj.MIOFF).initial.controls.equal.index = []; 50 | obj.problem.phases(ph+obj.MIOFF).initial.controls.equal.value = zeros(obj.problem.phases(ph+obj.MIOFF).initial.controls.equal.index,1); 51 | obj.problem.phases(ph+obj.MIOFF).initial.states.lower.index = []; 52 | obj.problem.phases(ph+obj.MIOFF).initial.states.lower.value = zeros(obj.problem.phases(ph+obj.MIOFF).initial.states.lower.index,1); 53 | obj.problem.phases(ph+obj.MIOFF).initial.controls.lower.index = []; 54 | obj.problem.phases(ph+obj.MIOFF).initial.controls.lower.value = zeros(obj.problem.phases(ph+obj.MIOFF).initial.controls.lower.index,1); 55 | obj.problem.phases(ph+obj.MIOFF).initial.states.upper.index = []; 56 | obj.problem.phases(ph+obj.MIOFF).initial.states.upper.value = zeros(obj.problem.phases(ph+obj.MIOFF).initial.states.upper.index,1); 57 | obj.problem.phases(ph+obj.MIOFF).initial.controls.upper.index = []; 58 | obj.problem.phases(ph+obj.MIOFF).initial.controls.upper.value = zeros(obj.problem.phases(ph+obj.MIOFF).initial.controls.upper.index,1); 59 | 60 | % Initializes Final Conditions 61 | obj.problem.phases(ph+obj.MIOFF).final.states.equal.index = []; 62 | obj.problem.phases(ph+obj.MIOFF).final.states.equal.value = zeros(obj.problem.phases(ph+obj.MIOFF).final.states.equal.index,1); 63 | obj.problem.phases(ph+obj.MIOFF).final.controls.equal.index = []; 64 | obj.problem.phases(ph+obj.MIOFF).final.controls.equal.value = zeros(obj.problem.phases(ph+obj.MIOFF).final.controls.equal.index,1); 65 | obj.problem.phases(ph+obj.MIOFF).final.states.lower.index = []; 66 | obj.problem.phases(ph+obj.MIOFF).final.states.lower.value = zeros(obj.problem.phases(ph+obj.MIOFF).final.states.lower.index,1); 67 | obj.problem.phases(ph+obj.MIOFF).final.controls.lower.index = []; 68 | obj.problem.phases(ph+obj.MIOFF).final.controls.lower.value = zeros(obj.problem.phases(ph+obj.MIOFF).final.controls.lower.index,1); 69 | obj.problem.phases(ph+obj.MIOFF).final.states.upper.index = []; 70 | obj.problem.phases(ph+obj.MIOFF).final.states.upper.value = zeros(obj.problem.phases(ph+obj.MIOFF).final.states.upper.index,1); 71 | obj.problem.phases(ph+obj.MIOFF).final.controls.upper.index = []; 72 | obj.problem.phases(ph+obj.MIOFF).final.controls.upper.value = zeros(obj.problem.phases(ph+obj.MIOFF).final.controls.upper.index,1); 73 | 74 | % Initializes States and Controls Bounds 75 | obj.problem.phases(ph+obj.MIOFF).bounds.states.upper = zeros(numberOfStates,0); 76 | obj.problem.phases(ph+obj.MIOFF).bounds.states.lower = zeros(numberOfStates,0); 77 | obj.problem.phases(ph+obj.MIOFF).bounds.controls.upper = zeros(numberOfControls,0); 78 | obj.problem.phases(ph+obj.MIOFF).bounds.controls.lower = zeros(numberOfControls,0); 79 | obj.problem.phases(ph+obj.MIOFF).bounds.timeFinal.upper = []; 80 | obj.problem.phases(ph+obj.MIOFF).bounds.timeFinal.lower = []; 81 | obj.problem.phases(ph+obj.MIOFF).bounds.timeInitial.upper = []; 82 | obj.problem.phases(ph+obj.MIOFF).bounds.timeInitial.lower = []; 83 | 84 | 85 | 86 | % Initializes Event Constraints 87 | obj.problem.phases(ph+obj.MIOFF).events = struct; 88 | for ii = 0:obj.problem.phases(ph+obj.MIOFF).n.events-1 89 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).limit = 0; 90 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).funType = 'undefined'; % default 91 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).type = 'undefined'; % default 92 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).where = 'undefined'; % default 93 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).indexWhere = []; % default is empty, only if index where type 94 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).cone = obj.initCone(3,obj.problem.phases(ph+obj.MIOFF).n); 95 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).states = zeros(numberOfStates,1); 96 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).controls = zeros(numberOfControls,1); 97 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).cons = 0; 98 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).order = 1; % or 2nd order 99 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).variableIndex = nan; 100 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).statesIndex = 1:numberOfStates; 101 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).controlsIndex = 1:numberOfControls; 102 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).variableType = 'undefined'; 103 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).function = @(t,X,U,BodyMap,ii) zeros(1,1); 104 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).jacobian.states = @(t,X,U,BodyMap,ii) zeros(numberOfStates,1); 105 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).jacobian.controls = @(t,X,U,BodyMap,ii) zeros(numberOfControls,1); 106 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).jacobian.variable = @(t,X,U,BodyMap,ii) zeros(1,1); % this is for 2nd order 1var only 107 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).hessian.variable = @(t,X,U,BodyMap,ii) zeros(1,1); % this is for 2nd order 1var only 108 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).scale = 1; 109 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).buffer.include = 0; % Only if Virtual Buffers are Enabled 110 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).buffer.lambda = 1; % Buffer multiplier term. The higher the more penalty 111 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).buffer.penalty = 1; % Buffer Penalty Type for violation (1 or 2) 112 | obj.problem.phases(ph+obj.MIOFF).events(ii+obj.MIOFF).buffer.penaltyAdaptive = 1; % Buffer adaptive penalty type 113 | end 114 | 115 | % Initializes Path Constraints 116 | obj.problem.phases(ph+obj.MIOFF).path = struct; 117 | for ii = 0:obj.problem.phases(ph+obj.MIOFF).n.path-1 118 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).limit = 0; 119 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).funType = 'undefined'; % default. 120 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).type = 'undefined'; % default 121 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).indexWhere = []; % default is empty, not implemented but if empty, should apply [1:Node] 122 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).integral = 0; 123 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).derivative = 0; 124 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).cone = obj.initCone(3,obj.problem.phases(ph+obj.MIOFF).n); 125 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).states = zeros(numberOfStates,1); 126 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).controls = zeros(numberOfControls,1); 127 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).cons = 0; 128 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).order = 1; % or 2nd order 129 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).variableIndex = nan; 130 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).statesIndex = 1:numberOfStates; 131 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).controlsIndex = 1:numberOfControls; 132 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).variableType = 'undefined'; 133 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).function = @(t,X,U,BodyMap,ii) zeros(1,size(X,2)); 134 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).jacobian.states = @(t,X,U,BodyMap,ii) zeros(numberOfStates,size(X,2)); 135 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).jacobian.controls = @(t,X,U,BodyMap,ii) zeros(numberOfControls,size(X,2)); 136 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).jacobian.variable = @(t,X,U,BodyMap,ii) zeros(1,size(X,2)); 137 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).hessian.issparse = 0; 138 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).hessian.states = @(t,X,U,BodyMap,ii) zeros(numberOfStates,numberOfStates,length(t)); 139 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).hessian.controls = @(t,X,U,BodyMap,ii) zeros(numberOfControls,numberOfControls,length(t)); 140 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).hessian.crossXU = @(t,X,U,BodyMap,ii) zeros(numberOfStates,numberOfControls,length(t)); 141 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).hessian.crossUX = @(t,X,U,BodyMap,ii) zeros(numberOfControls,numberOfStates,length(t)); 142 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).hessian.variable = @(t,X,U,BodyMap,ii) zeros(1,size(X,2)); 143 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).scale = 1; 144 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).buffer.include = 0; % Only if Virtual Buffers are Enabled 145 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).buffer.lambda = 1; % Buffer multiplier term. The higher the more penalty 146 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).buffer.penalty = 1; % Buffer Penalty Type for violation (1 or 2) 147 | obj.problem.phases(ph+obj.MIOFF).path(ii+obj.MIOFF).buffer.penaltyAdaptive = 1; % Buffer adaptive penalty type 148 | 149 | end 150 | 151 | % Initializes states and controls statewise trust regions 152 | % and final time trust regions 153 | obj.algorithm.sequential.trustRegion.phases(ph+obj.MIOFF).states.component = ones(numberOfStates,1); 154 | obj.algorithm.sequential.trustRegion.phases(ph+obj.MIOFF).controls.component = ones(numberOfControls,1); 155 | obj.algorithm.sequential.trustRegion.phases(ph+obj.MIOFF).timeFinal.radius = 1.0; 156 | obj.algorithm.sequential.trustRegion.phases(ph+obj.MIOFF).timeFinal.lambda = 1.0; 157 | obj.algorithm.sequential.trustRegion.phases(ph+obj.MIOFF).timeFinal.type = 'soft'; 158 | obj.algorithm.sequential.trustRegion.phases(ph+obj.MIOFF).timeFinal.adaptive = 0; 159 | obj.algorithm.sequential.trustRegion.phases(ph+obj.MIOFF).controls.lambda = 1.0e0; 160 | 161 | % Virtual Control fields. 162 | obj.algorithm.virtualControl.phases(ph+obj.MIOFF).include = 0; % Inactive by default 163 | obj.algorithm.virtualControl.phases(ph+obj.MIOFF).states = 1:numberOfStates; % Index specifying states affected 164 | obj.algorithm.virtualControl.phases(ph+obj.MIOFF).lambda = 1e0; % Augmentation term of the cost function 165 | obj.algorithm.virtualControl.phases(ph+obj.MIOFF).statePenalty = 1 ; % 1st or 2nd norm for the state-wise norm 166 | obj.algorithm.virtualControl.phases(ph+obj.MIOFF).nodePenalty = 1 ; % 1st or 2nd norm for the node wise norm (applied to all state-wise norms) 167 | obj.algorithm.virtualControl.phases(ph+obj.MIOFF).scale = nan; % Setted later 168 | 169 | % Virtual Buffer fields 170 | obj.algorithm.virtualBuffer.phases(ph+obj.MIOFF).include = 0; % Inactive by default 171 | obj.algorithm.virtualBuffer.phases(ph+obj.MIOFF).nodePenalty = 1 ; % 1st or 2nd norm for all buffers 172 | 173 | % Initial guess fields for all variables 174 | obj.problem.phases(ph+obj.MIOFF).initialGuess.timeFinal = []; % empty by default, needs an initialization (if fixed, a value) 175 | obj.problem.phases(ph+obj.MIOFF).initialGuess.timeInitial = 0; % 0 by default 176 | obj.problem.phases(ph+obj.MIOFF).initialGuess.time = zeros(1,numberOfNodes); % time is zero by default, for compatibility with SOCP solver 177 | obj.problem.phases(ph+obj.MIOFF).initialGuess.states = zeros(numberOfStates,numberOfNodes); % time is zero by default, for compatibility with SOCP solver 178 | obj.problem.phases(ph+obj.MIOFF).initialGuess.controls = zeros(numberOfControls,numberOfNodes); % time is zero by default, for compatibility with SOCP solver 179 | 180 | end 181 | 182 | % Objective 183 | obj.problem.objective.type = 'minimize'; % minimize or maximize. Minimisation by default 184 | obj.problem.objective.sign = [] ; % Initialized in level 2 with type 185 | obj.problem.objective.scale = 1 ; % Scale term for the objective 186 | 187 | % Objective Mayer term for a specific node 188 | obj.problem.objective.mayer.funType = 'undefined'; 189 | obj.problem.objective.mayer.where = 'undefined'; 190 | obj.problem.objective.mayer.indexWhere = []; 191 | obj.problem.objective.mayer.absolute.term = 0; % absolute of whole function 192 | obj.problem.objective.mayer.absolute.states = 0; % absolute function of states 193 | obj.problem.objective.mayer.absolute.controls = 0; % absolute function of controls 194 | obj.problem.objective.mayer.function = @(t,X,U,BodyMap,ii) 0; 195 | obj.problem.objective.mayer.jacobian.states = @(t,X,U,BodyMap,ii) zeros(numberOfStates,1); 196 | obj.problem.objective.mayer.jacobian.controls = @(t,X,U,BodyMap,ii) zeros(numberOfControls,1); 197 | obj.problem.objective.mayer.states = zeros(numberOfStates,1); 198 | obj.problem.objective.mayer.controls = zeros(numberOfControls,1); 199 | obj.problem.objective.mayer.timeFinal = 0; 200 | obj.problem.objective.mayer.cone = obj.initCone(2,obj.problem.phases(1).n); 201 | 202 | % Objective Lagrange term on all nodes 203 | obj.problem.objective.lagrange.funType = 'undefined'; 204 | obj.problem.objective.lagrange.absolute.term = 0; % absolute of whole function 205 | obj.problem.objective.lagrange.absolute.nodes = 0; % absolute of all nodes 206 | obj.problem.objective.lagrange.absolute.states = 0; % absolute function of states 207 | obj.problem.objective.lagrange.absolute.controls = 0; % absolute function of controls 208 | obj.problem.objective.lagrange.function = @(t,X,U,BodyMap,ii) zeros(1,size(X,2)); 209 | obj.problem.objective.lagrange.jacobian.states = @(t,X,U,BodyMap,ii) zeros(numberOfStates,size(X,2)); 210 | obj.problem.objective.lagrange.jacobian.controls = @(t,X,U,BodyMap,ii) zeros(numberOfControls,size(X,2)); 211 | obj.problem.objective.lagrange.states = zeros(numberOfStates,1); 212 | obj.problem.objective.lagrange.controls = zeros(numberOfControls,1); 213 | obj.problem.objective.lagrange.timeFinal = 0; 214 | obj.problem.objective.lagrange.cone = obj.initCone(2,obj.problem.phases(1).n); 215 | 216 | % Objective minimisation(maximization) term on node with larger(lower) value 217 | obj.problem.objective.minmax.funType = 'undefined'; 218 | obj.problem.objective.minmax.absolute.term = 0; 219 | obj.problem.objective.minmax.absolute.nodes = 0; 220 | obj.problem.objective.minmax.absolute.states = 0; 221 | obj.problem.objective.minmax.absolute.controls = 0; 222 | obj.problem.objective.minmax.function = @(t,X,U,BodyMap,ii) zeros(1,size(X,2)); 223 | obj.problem.objective.minmax.jacobian.states = @(t,X,U,BodyMap,ii) zeros(numberOfStates,size(X,2)); 224 | obj.problem.objective.minmax.jacobian.controls = @(t,X,U,BodyMap,ii) zeros(numberOfControls,size(X,2)); 225 | obj.problem.objective.minmax.states = zeros(numberOfStates,1); 226 | obj.problem.objective.minmax.controls = zeros(numberOfControls,1); 227 | obj.problem.objective.minmax.timeFinal = 0; 228 | obj.problem.objective.minmax.cone = obj.initCone(2,obj.problem.phases(1).n); 229 | 230 | % Objective regularisation term on whole function, can be 231 | % inserted in lagrange, but included as additional type as 232 | % dependency on time removed 233 | obj.problem.objective.regularisation.weight = 1; 234 | obj.problem.objective.regularisation.states = zeros(numberOfStates,1); 235 | obj.problem.objective.regularisation.controls = zeros(numberOfControls,1); 236 | obj.problem.objective.regularisation.timeFinal = 0; 237 | 238 | end 239 | 240 | -------------------------------------------------------------------------------- /src/@scopt2/solveSOCPProblem.m: -------------------------------------------------------------------------------- 1 | function solverOutput = solveSOCPProblem(obj) 2 | %% Routine to solve convex problem. Requires the definition of the SOC and Linear Matrices 3 | if ~obj.quiet,tic;end 4 | %% Retrieve 5 | maxIter = obj.algorithm.conicSolver.maxIter; 6 | feasTol = obj.algorithm.conicSolver.feasTol; 7 | relTol = obj.algorithm.conicSolver.relTol; 8 | absTol = obj.algorithm.conicSolver.absTol; 9 | Gaug = obj.problem.transcription.Gaug; 10 | haug = obj.problem.transcription.haug; 11 | MEQ = obj.problem.transcription.MEQ; 12 | pEQ = obj.problem.transcription.pEQ; 13 | f = obj.problem.transcription.f; 14 | dims = obj.problem.transcription.dims; 15 | solver_runTime = nan; 16 | solver_iterations = nan; 17 | %% Run Convex Optimization (internal) 18 | switch obj.algorithm.conicSolver.type 19 | case {'cvx-sdpt3','cvx-sedumi','cvx-ecos'} 20 | % Runs CVX interface with the defined solver 21 | if obj.quiet 22 | cvx_begin quiet 23 | else 24 | cvx_begin 25 | end 26 | cvx_solver(obj.algorithm.conicSolver.type(5:end)) 27 | % cvx_solver 28 | if ~obj.quiet 29 | fprintf('Setting up Convex Solver \n') 30 | end 31 | Ntot = length(f); 32 | variables Z(Ntot) 33 | minimize( f'*Z ) 34 | subject to 35 | %SOC 36 | if ~obj.quiet 37 | fprintf('Setting up SOC Constraints \n') 38 | end 39 | tic 40 | qprev = 0; 41 | for ii = 1:length(dims.q) 42 | rowInitial = dims.l +obj.MIOFF + qprev; 43 | rowEnd = dims.l +obj.MIOFF + sum(dims.q(1:ii)) - 1; 44 | qprev = sum(dims.q(1:ii)); 45 | norm((-Gaug((rowInitial+1):rowEnd,:))*Z+haug((rowInitial+1):rowEnd))<=(-Gaug(rowInitial,:))*Z+haug(rowInitial); 46 | end 47 | if ~obj.quiet 48 | fprintf('SOC Constraints setted. Time elapsed: %0.2f [s] \n',toc) 49 | end 50 | % Inequalities 51 | Gaug(0+obj.MIOFF:dims.l - 1+obj.MIOFF,:)*Z <= haug(0+obj.MIOFF:dims.l - 1+obj.MIOFF); 52 | % Equalities 53 | MEQ*Z == pEQ; 54 | if ~obj.quiet 55 | fprintf('Calling solver \n') 56 | end 57 | tic 58 | cvx_end 59 | solver_status = cvx_status; 60 | solver_optval = cvx_optval; 61 | 62 | if strcmp(solver_status,'Failed') || strcmp(solver_status,'Infeasible') || strcmp(solver_status,'Unbounded') 63 | solver_exitFlag = 1; 64 | else 65 | solver_exitFlag = 0; 66 | end 67 | Zopt = Z; 68 | case {'sdpt3'} 69 | % [obj,X,y,Z,info] = sdpt3(blk,At,C,b,OPTIONS); 70 | error('SCOPT Error: sdpt3 not implemented') 71 | case {'sedumi'} 72 | % Runs sedumi from Matlab 73 | pars.maxiter = maxIter; 74 | pars.eps = feasTol; 75 | pars.numtol = relTol; 76 | dims.l = dims.l + length(pEQ)*2; 77 | A = [MEQ;-MEQ;Gaug]; 78 | ct = [pEQ;-pEQ;haug]; 79 | % ct = [pEQ-feasTol/2;-pEQ-feasTol/2;haug]; 80 | [~,Z,info] = sedumi(A',-f,ct,dims,pars); 81 | Zopt = Z; 82 | solver_optval = sum(f.*Z); 83 | pinf = info.pinf; 84 | dinf = info.dinf; 85 | if pinf 86 | solver_exitFlag = 1; 87 | solver_status = 'Infeasible'; 88 | elseif dinf 89 | solver_exitFlag = 2; 90 | solver_status = 'Infeasible'; 91 | else 92 | solver_exitFlag = 0; 93 | solver_status = 'Feasible'; 94 | end 95 | if info.numerr 96 | solver_exitFlag = -2; 97 | solver_status = 'Numerical Problems'; 98 | end 99 | 100 | case {'ecos'} 101 | % Runs ecos from Matlab 102 | if obj.quiet 103 | verbose = 0; 104 | else 105 | verbose = 2; 106 | end 107 | opts = ecosoptimset('verbose',verbose,'maxit',maxIter,'feastol',feasTol,'reltol',relTol,'abstol',absTol); 108 | [Zopt,~,info] = ecos(f,Gaug,haug,dims,MEQ,pEQ,opts); 109 | 110 | solver_status = info.infostring; 111 | solver_optval = info.pcost; 112 | solver_exitFlag = info.exitflag; 113 | solver_runTime = info.timing.tsolve; 114 | solver_iterations = info.iter; 115 | 116 | case {'dual-simplex','interior-point','interior-point-legacy'} 117 | % Runs matlab linear programming solvers 118 | if isempty(dims.q) 119 | options = optimoptions('linprog','Algorithm',obj.algorithm.conicSolver.type,'Display','iter','MaxIterations',maxIter,'OptimalityTolerance',feasTol); 120 | [Zopt,fval,exitflag,output,~] = linprog(f,Gaug,haug,MEQ,pEQ,[],[],options); 121 | 122 | switch exitflag 123 | case 0 124 | solver_exitFlag = 10; 125 | case 1 126 | solver_exitFlag = 0; 127 | case 3 128 | solver_exitFlag = -2; 129 | case -2 130 | solver_exitFlag = 1; 131 | case -5 132 | solver_exitFlag = 2; 133 | case {-2,-3,-4,-6,-7,-8,-9} 134 | solver_exitFlag = -3; 135 | otherwise 136 | error('Check exit flags from linprog \n') 137 | end 138 | 139 | solver_optval = fval; 140 | solver_status = output.message; 141 | else 142 | error('The convex subproblem contains second order cone constraints and is not linear \n') 143 | end 144 | otherwise 145 | error('SCOPT Error: Invalid Solver. Time elapsed: %0.2f [s] \n Aborting Run \n',toc) 146 | end 147 | %% Store Output 148 | obj.solution.Zopt = Zopt; 149 | solverOutput.Zopt = Zopt; 150 | solverOutput.Foptval = solver_optval; 151 | solverOutput.status = solver_status; 152 | solverOutput.exitFlag = solver_exitFlag; 153 | solverOutput.runTime = solver_runTime; 154 | solverOutput.iterations = solver_iterations; 155 | if ~obj.quiet,fprintf('Solver Call Finished. Time elapsed: %0.2f [s] \n Retrieving State... \n',toc);end 156 | end 157 | 158 | -------------------------------------------------------------------------------- /src/@scopt2/terminationVariableConvergence.m: -------------------------------------------------------------------------------- 1 | function varTol = terminationVariableConvergence(obj,timeInitial_n_opt,timeFinal_n_opt,X_n_opt,U_n_opt,timeInitial_n_prev,timeFinal_n_prev,X_n_prev,U_n_prev) 2 | % Computes the termination convergence metrics 3 | Nodes = obj.problem.phases.Nodes; 4 | switch obj.algorithm.sequential.variablesTol.type 5 | case 'norm' 6 | terminationAuxVar = zeros((obj.problem.phases(1).n.states+obj.problem.phases(1).n.controls+2)*Nodes,1); 7 | terminationAuxVar(1:(obj.problem.phases(1).n.states+obj.problem.phases(1).n.controls)*Nodes) = reshape([X_n_opt - X_n_prev;U_n_opt - U_n_prev],[],1); 8 | if obj.problem.phases(1).freeTimeInitial 9 | terminationAuxVar(end-1) = timeInitial_n_opt-timeInitial_n_prev; 10 | end 11 | if obj.problem.phases(1).freeTimeFinal 12 | terminationAuxVar(end) = timeFinal_n_opt-timeFinal_n_prev; 13 | end 14 | terminationAuxNorm= norm(terminationAuxVar,obj.algorithm.sequential.variablesTol.norm); 15 | if ~obj.quiet 16 | fprintf('Convergance Metric = %0.2f%% \n',terminationAuxNorm/obj.algorithm.sequential.variablesTol.etaTol*100) 17 | end 18 | varTol = terminationAuxNorm<=obj.algorithm.sequential.variablesTol.etaTol; 19 | case 'component' 20 | if obj.algorithm.sequential.variablesTol.include.states 21 | convergedStates = sum(reshape(abs(X_n_opt - X_n_prev),[],1)<=repmat(obj.algorithm.sequential.variablesTol.phases.states_n,obj.problem.phases.Nodes,1)); 22 | requiredNumberStates = obj.problem.phases(1).n.states*obj.problem.phases.Nodes; 23 | terminationAuxStates = convergedStates==requiredNumberStates; 24 | if ~obj.quiet 25 | fprintf('Percentage of Converged States = %0.2f%% \n',convergedStates/requiredNumberStates*100) 26 | end 27 | else 28 | terminationAuxStates = 0; 29 | end 30 | if obj.algorithm.sequential.variablesTol.include.controls 31 | convergedControls = sum(reshape(abs(U_n_opt - U_n_prev),[],1)<=repmat(obj.algorithm.sequential.variablesTol.phases.controls_n,obj.problem.phases.Nodes,1)); 32 | requiredNumberControls = obj.problem.phases(1).n.controls*obj.problem.phases.Nodes; 33 | terminationAuxControls = convergedControls==requiredNumberControls; 34 | if ~obj.quiet 35 | fprintf('Percentage of Converged Controls = %0.2f%% \n',convergedControls/requiredNumberControls*100) 36 | end 37 | else 38 | terminationAuxControls = 1; 39 | end 40 | if obj.problem.phases(1).freeTimeFinal && obj.algorithm.sequential.variablesTol.include.timeFinal 41 | terminationAuxTimeFinal = abs(timeFinal_n_opt - timeFinal_n_prev)<=obj.algorithm.sequential.variablesTol.phases.timeFinal_n; 42 | if ~obj.quiet 43 | fprintf('Converged Final Time = %i \n',terminationAuxTimeFinal) 44 | end 45 | else 46 | terminationAuxTimeFinal = 1; 47 | end 48 | if obj.problem.phases(1).freeTimeInitial && obj.algorithm.sequential.variablesTol.include.timeInitial 49 | terminationAuxTimeInitial = abs(timeInitial_n_opt - timeInitial_n_prev)<=obj.algorithm.sequential.variablesTol.phases.timeInitial_n; 50 | if ~obj.quiet 51 | fprintf('Converged Initial Time = %i \n',terminationAuxTimeFinal) 52 | end 53 | else 54 | terminationAuxTimeInitial = 1; 55 | end 56 | varTol = terminationAuxStates && terminationAuxControls && terminationAuxTimeFinal && terminationAuxTimeInitial; 57 | case 'none' 58 | varTol = 1; 59 | otherwise 60 | error('Invalid type %s for variable convergence termination setting',obj.algorithm.sequential.variablesTol.type) 61 | end 62 | end 63 | 64 | -------------------------------------------------------------------------------- /src/toolbox/goldenSection/costFun_convex1D.m: -------------------------------------------------------------------------------- 1 | function cost = costFun_convex1D (x) 2 | % Convex 1 Dimensional cost function with minimum at 2 3 | 4 | cost = (x - 2).^2; -------------------------------------------------------------------------------- /src/toolbox/goldenSection/goldenSection.m: -------------------------------------------------------------------------------- 1 | function [x,fval,debug] = goldenSection (fhandle,a,b,xtol,numberOfIterations,Mbracket) 2 | % Golden section search routine. It is a one dimensional optimiser which 3 | % does not use derivative information and which can find the solution in a 4 | % pre-determined number of iterations dependent on the variable tolerance. 5 | % It searches within the inverval specified by a and b, and is based on the 'magic' ratio 0.6180 6 | % 7 | % fhandle IN: Handle object of Objective function to be minimised 8 | % a IN: lower interval boundary 9 | % b IN: upper interval boundary 10 | % xtol IN: variable tolerance for the search 11 | % numberOfIterations IN: number of iterations (if not inserted, computed 12 | % with xtol) 13 | % Mbracket IN: Not necessary. Initial objective value at a and 14 | % b, only if its desired to include boundaries in search 15 | % x OUT: Optimum variable 16 | % fval OUT: Optimum objective 17 | % debug OUT: Additional debugging information as number of 18 | % iterations 19 | % 20 | % Successive Convexification OPtimal ConTrol 2.0 21 | % Optimal control tool to solve single phase autonomous problems 22 | % using successive convexification. 23 | % Copyright (C) 2020 Guillermo J. Dominguez Calabuig 24 | % _________________________________________________________________________ 25 | %% Constants 26 | goldenRatio = (sqrt(5)-1)/2; 27 | %% Read Input and Specifiy Algorithm settings 28 | bracket = [a,b] ; 29 | bracketLength = abs(b-a); 30 | if ~exist('numberOfIterations','var') 31 | numberOfIterations = ceil(log(xtol / bracketLength) / log(goldenRatio)); 32 | elseif isempty(numberOfIterations) 33 | numberOfIterations = ceil(log(xtol / bracketLength) / log(goldenRatio)); 34 | end 35 | 36 | if ~exist('Mbracket','var') 37 | Mbracket = [fhandle(a) , fhandle(b)]; 38 | numberOfEvaluations = 2; 39 | else 40 | if strcmp(Mbracket,'omit-boundaries') 41 | Mbracket = [nan nan]; 42 | end 43 | numberOfEvaluations = 0; 44 | end 45 | %% Loop through steps 46 | % Generate solution on both fractions to choose initial direction 47 | x2 = bracket(1) + (1-goldenRatio)*(bracket(2)-bracket(1)); 48 | M2 = fhandle(x2) ; 49 | x3 = bracket(1) + goldenRatio*(bracket(2)-bracket(1)); 50 | M3 = fhandle(x3) ; 51 | numberOfEvaluations = numberOfEvaluations + 2; 52 | % Iterate to find best triplet until bracketing distance is lower than xtol 53 | for ii = 1:numberOfIterations 54 | if M2>M3 % chose minimum from triplets M2 M3 M4 and compute next M3 55 | bracket(1) = x2; 56 | Mbracket(1)= M2; 57 | triplet = [bracket(1) x3 bracket(2)]; 58 | tripletM = [Mbracket(1) M3 Mbracket(2)]; 59 | if ii = 1 43 | obj.machData = machData; 44 | else 45 | obj.machData = [0,0.2,1]; 46 | end 47 | 48 | if nargin >= 2 49 | obj.cdData = cdData; 50 | else 51 | obj.cdData = [1.0,1.0,0.6]; 52 | end 53 | 54 | if nargin >= 3 55 | obj.dragMethod = dragMethod; 56 | else 57 | obj.dragMethod = 'none'; 58 | end 59 | 60 | if nargin >= 4 61 | obj.clData = clData; 62 | else 63 | obj.clData = [0.0,0.0,0.0]; 64 | end 65 | 66 | if nargin >= 5 67 | obj.liftMethod = liftMethod; 68 | else 69 | obj.liftMethod = 'none'; 70 | end 71 | 72 | if nargin >= 6 73 | obj.area = area; 74 | else 75 | obj.area = 10; 76 | end 77 | end 78 | 79 | function [Cd,dCd] = computeCd(obj,mach,angleOfAttack) 80 | switch obj.dragMethod 81 | case 'none' 82 | Cd = 0; 83 | dCd = 0; 84 | 85 | case 'constant' 86 | Cd = obj.cdData(1); 87 | dCd = 0; 88 | 89 | case 'interpolate' 90 | Cd = obj.dragInterpolant(mach); 91 | dCd = NaN; % ?? 92 | 93 | case 'subsonic-fit' 94 | if mach < obj.machData(2) 95 | Cd = obj.cdData(2); 96 | dCd = 0; 97 | else 98 | Cd = obj.dragFit(1)*mach.^2+obj.dragFit(2)*mach+obj.dragFit(3); 99 | dCd = 2* obj.dragFit(1)*mach+obj.dragFit(2); 100 | end 101 | end 102 | end 103 | 104 | function [Cl,dCl] = computeCl(obj,mach,angleOfAttack) 105 | switch obj.dragMethod 106 | case 'none' 107 | Cl = 0; 108 | dCl = 0; 109 | 110 | case 'constant' 111 | Cl = obj.clData(1); 112 | dCl = 0; 113 | 114 | case 'interpolate' 115 | Cl = obj.liftInterpolant(mach); 116 | dCl = NaN; % ?? 117 | 118 | case 'subsonic-fit' 119 | if mach < obj.machData(2) 120 | Cl = obj.clData(2); 121 | dCl = 0; 122 | else 123 | Cl = obj.liftFit(1)*mach.^2+obj.liftFit(2)*mach+obj.liftFit(3); 124 | dCl = 2* obj.liftFit(1)*mach+obj.liftFit(2); 125 | end 126 | end 127 | end 128 | 129 | function obj = set.dragMethod(obj,dragMethod) 130 | switch dragMethod 131 | case 'none' 132 | 133 | case 'constant' 134 | 135 | case 'interpolate' 136 | obj.dragInterpolant = griddedInterpolant(obj.machData,obj.cdData); 137 | 138 | case 'subsonic-fit' 139 | obj.dragFit(1) = (obj.cdData(3)-obj.cdData(2))/(obj.machData(3)-obj.machData(2))^2; 140 | obj.dragFit(2) = -obj.machData(2)*2*obj.dragFit(1); 141 | obj.dragFit(3) = obj.cdData(2)+obj.machData(2)^2*obj.dragFit(1); 142 | otherwise 143 | error('Invalid aerodynamic drag coefficient method.'); 144 | end 145 | obj.dragMethod = dragMethod; 146 | end 147 | 148 | function obj = set.liftMethod(obj,liftMethod) 149 | switch liftMethod 150 | case 'none' 151 | 152 | case 'constant' 153 | 154 | case 'interpolate' 155 | obj.dragInterpolant = griddedInterpolant(obj.machData,obj.clData); 156 | 157 | case 'subsonic-fit' 158 | obj.liftFit(1) = (obj.clData(3)-obj.clData(2))/(obj.machData(3)-obj.machData(2))^2; 159 | obj.liftFit(2) = -obj.machData(2)*2*obj.liftFit(1); 160 | obj.liftFit(3) = obj.clData(2)+obj.machData(2)^2*obj.liftFit(1); 161 | otherwise 162 | error('Invalid aerodynamic lift coefficient method.'); 163 | end 164 | obj.liftMethod = liftMethod; 165 | end 166 | 167 | function includeDrag = get.includeDrag(obj) 168 | if strcmp(obj.dragMethod,'none') 169 | includeDrag = false; 170 | else 171 | includeDrag = true; 172 | end 173 | end 174 | 175 | function includeLift = get.includeLift(obj) 176 | if strcmp(obj.liftMethod,'none') 177 | includeLift = false; 178 | else 179 | includeLift = true; 180 | end 181 | end 182 | end 183 | end 184 | 185 | -------------------------------------------------------------------------------- /test/rocketLanding/bodyMap/atmosphereModel/atmosphereModelValidation.m: -------------------------------------------------------------------------------- 1 | %% Validates atmosphere model by comparison 2 | 3 | AtmosphereConst = atmosphereModel('const'); 4 | AtmosphereExp = atmosphereModel('exp'); 5 | AtmosphereTabulated = atmosphereModel('tabulated'); 6 | AtmosphereConstUs76 = atmosphereModel('us76'); 7 | 8 | fig1 = figure; 9 | fig2 = figure; 10 | 11 | AtmosphereConst.plot(fig1); hold on 12 | AtmosphereExp.plot(fig1); hold on 13 | AtmosphereTabulated.plot(fig1); hold on 14 | AtmosphereConstUs76.plot(fig1); hold on 15 | subplot 221 16 | legend({AtmosphereConst.model,AtmosphereExp.model,AtmosphereTabulated.model,AtmosphereConstUs76.model},'location','best'); 17 | subplot 222 18 | legend({AtmosphereConst.model,AtmosphereExp.model,AtmosphereTabulated.model,AtmosphereConstUs76.model},'location','best'); 19 | subplot 223 20 | legend({AtmosphereConst.model,AtmosphereExp.model,AtmosphereTabulated.model,AtmosphereConstUs76.model},'location','best'); 21 | subplot 224 22 | legend({AtmosphereConst.model,AtmosphereExp.model,AtmosphereTabulated.model,AtmosphereConstUs76.model},'location','best'); 23 | 24 | 25 | AtmosphereConst.plotDerivatives(fig2); hold on 26 | AtmosphereExp.plotDerivatives(fig2); hold on 27 | AtmosphereTabulated.plotDerivatives(fig2); hold on 28 | AtmosphereConstUs76.plotDerivatives(fig2); hold on 29 | subplot 221 30 | legend({AtmosphereConst.model,AtmosphereExp.model,AtmosphereTabulated.model,AtmosphereConstUs76.model},'location','best'); 31 | subplot 222 32 | legend({AtmosphereConst.model,AtmosphereExp.model,AtmosphereTabulated.model,AtmosphereConstUs76.model},'location','best'); 33 | subplot 223 34 | legend({AtmosphereConst.model,AtmosphereExp.model,AtmosphereTabulated.model,AtmosphereConstUs76.model},'location','best'); 35 | subplot 224 36 | legend({AtmosphereConst.model,AtmosphereExp.model,AtmosphereTabulated.model,AtmosphereConstUs76.model},'location','best'); 37 | 38 | -------------------------------------------------------------------------------- /test/rocketLanding/bodyMap/atmosphereModel/atmosusa76.m: -------------------------------------------------------------------------------- 1 | function [T, a, P, Rho,derivatives] = atmosusa76( height ,flagDerivative) 2 | %atmosusa76 Retrieve atmospheric properties based on US76 standard 3 | %atmosphere. Currently only working up to 11 km 4 | % Detailed explanation goes here 5 | if ~exist('flagDerivative','var') 6 | flagDerivative = 0; 7 | end 8 | derivatives = struct; 9 | %% Constants and Parameters 10 | R0e = 6356.766e3; % m Equatorial Altitude for US76 Atmosphere Model 11 | g0 = 9.80665 ; % m/s^2 Sea level Surface Gravity 12 | M0 = computeMolarMass(0) ; % %kg/kmol Temporary, Depends on Altitude for unmixed layers 13 | Rstar = 8.31432 ;% J/mol/K Universal Gas Constant 14 | gamma = 1.4 ; % Ratio of specific heat of air at constant pressure 15 | T_c = 263.1905 ; % K 16 | A = -76.3232 ; 17 | b = 19.9428 ; 18 | 19 | % Table 20 | I_i = [0 ] ; % Layer Index 21 | H_i = [0 ] ; % m Altitude 22 | TM_i = [288.15 ] ; % K Layer Base Temperature 23 | L_i = [-6.5e-3] ; % K/m % Layer Lapse Rate 24 | P_i = [1.013e5] ; % Pa % Layer Base Pressure 25 | %% 26 | % Compute Geopotential Altitude 27 | H = R0e.*height./(R0e+height); 28 | if flagDerivative % Derivative 29 | dHdh = R0e./(R0e+height) - R0e.*height./(R0e+height).^2 ; 30 | end 31 | % Compute Molecular Scale Temperature 32 | TM = TM_i(1) + L_i(1)*(H-H_i(1)); 33 | if flagDerivative % Derivative 34 | dTMdh = L_i(1).*dHdh; 35 | end 36 | % Compute Molar Mass 37 | M = computeMolarMass(H); 38 | if flagDerivative % Derivative 39 | dMdH = computeMolarMassDerivative(H) ; 40 | end 41 | % Compute Temperature 42 | T = TM .* M ./ M0 ; 43 | if flagDerivative % Derivative 44 | dTdh = ( dTMdh .* M + TM .* dMdH .* dHdh ) ./ M0 ; 45 | derivatives.dTdh = dTdh; 46 | end 47 | % Compute Pressure 48 | P = P_i.*exp(-g0.*M0.*(H - H_i(1))/Rstar./TM) ; 49 | if flagDerivative % Derivative 50 | dPdh = -g0.*M0/Rstar./TM.*P_i.*exp(-g0.*M0.*(H - H_i(1))/Rstar./TM).*(dHdh - (H - H_i(1))./TM.^2.*dTMdh) ; 51 | derivatives.dPdh = dPdh; 52 | end 53 | % Compute Density 54 | Rho = M0 / Rstar * P ./ TM ; 55 | if flagDerivative % Derivative 56 | dRhodh = M0 / Rstar *(dPdh./TM - P ./TM.^2.*dTMdh); 57 | derivatives.dRhodh = dRhodh; 58 | end 59 | % Compute Speed of Sound 60 | a = sqrt(gamma*Rstar/M0.*TM); 61 | if flagDerivative % Derivative 62 | dadh = sqrt(gamma*Rstar/M0./TM).*1/2.*dTMdh ; 63 | derivatives.dadh = dadh; 64 | end 65 | end 66 | 67 | function M = computeMolarMass(H) 68 | % Computes molar mass based on altitude and tabluated fraction table 69 | % currently only retrieves sea level one 70 | M0 = 28.9644/1000; % %kg/kmol 71 | 72 | M = M0*ones(size(H)); 73 | end 74 | 75 | function dMdH = computeMolarMassDerivative(H) 76 | % Computes molar mass derivative based on altitude and tabluated fraction table 77 | % currently only retrieves sea level one 78 | 79 | dMdH = zeros(size(H)); 80 | end -------------------------------------------------------------------------------- /test/rocketLanding/bodyMap/bodyMap.m: -------------------------------------------------------------------------------- 1 | function [ BodyMap ] = bodyMap( ) 2 | % Sets default bodyMap for problem 3 | BodyMap.GravityModel = gravityModel ; 4 | BodyMap.ThrustModel = thrustModel ; 5 | BodyMap.MassModel = massModel ; 6 | BodyMap.OrientationModel = orientationModel ; 7 | BodyMap.AerodynamicModel = aerodynamicModel ; 8 | BodyMap.AtmosphereModel = atmosphereModel ; 9 | BodyMap.DynamicModel.exactOde = 1; 10 | BodyMap.DynamicModel.controlVariable = 'thrust'; 11 | 12 | end 13 | 14 | -------------------------------------------------------------------------------- /test/rocketLanding/bodyMap/gravityModel/gravityModel.m: -------------------------------------------------------------------------------- 1 | classdef gravityModel 2 | %UNTITLED2 Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | model = 1; 7 | g0 = 9.807; 8 | g = 9.807; 9 | end 10 | 11 | end 12 | 13 | -------------------------------------------------------------------------------- /test/rocketLanding/bodyMap/massModel/massModel.m: -------------------------------------------------------------------------------- 1 | classdef massModel 2 | %Mass Model class with information on mass 3 | 4 | properties 5 | massInert = 10e3 ; % kg 6 | massWet = 15e3 ; % kg 7 | type = 0; 8 | end 9 | 10 | methods 11 | end 12 | 13 | end 14 | 15 | -------------------------------------------------------------------------------- /test/rocketLanding/bodyMap/orientationModel/orientationModel.m: -------------------------------------------------------------------------------- 1 | classdef orientationModel 2 | % Orientation model for coordinate system choice 3 | 4 | properties (Constant) 5 | rotRate_E = 7.2921159e-5 ; % rad/s 6 | end 7 | properties 8 | rotRate = 7.2921159e-5 ; 9 | rotRateDirection = [ 0 ; 1 ; 0] ; 10 | 11 | includeCentrifugal = 0 ; 12 | includeCentripetal = 0 ; 13 | end 14 | properties (Dependent) 15 | rotRateVector 16 | rotRateSkew 17 | end 18 | 19 | methods 20 | function [a] = computeRotatingAcceleration(obj,R,V) 21 | a = zeros(size(R)) ; 22 | if obj.includeCentrifugal 23 | a = a + computeCentrifugalAcceleration(obj,R); 24 | end 25 | if obj.includeCentripetal 26 | a = a + computeCentripetalAcceleration(obj,V); 27 | end 28 | end 29 | function [a] = computeCentrifugalAcceleration(obj,R) 30 | a = - obj.rotRateSkew*obj.rotRateSkew*R; 31 | end 32 | function [a] = computeCentripetalAcceleration(obj,V) 33 | a = - 2 * obj.rotRateSkew * V; 34 | end 35 | function rotRateVector = get.rotRateVector(obj) 36 | rotRateVector = obj.rotRateDirection*obj.rotRate; 37 | end 38 | function rotRateSkew = get.rotRateSkew(obj) 39 | rotRateSkew = [... 40 | 0 -obj.rotRateVector(3) obj.rotRateVector(2);... 41 | obj.rotRateVector(3) 0 -obj.rotRateVector(1);... 42 | -obj.rotRateVector(2) obj.rotRateVector(1) 0] ; 43 | end 44 | end 45 | 46 | end 47 | 48 | -------------------------------------------------------------------------------- /test/rocketLanding/bodyMap/thrustModel/thrustModel.m: -------------------------------------------------------------------------------- 1 | classdef thrustModel 2 | %Thrust Model class with information on rocket 3 | % propulsion characteristics 4 | properties (Constant) 5 | g0 = 9.807 ; % Earth surface gravity constant [m/s^2] 6 | end 7 | properties 8 | thrustVac = 250e3+0.5*100.0e3 ; % Maximum Vaccum Thrust [N] 9 | throttleMax = 1 ; % Maximum Throttle setting [-] 10 | throttleMin = (100e3+0.5*100.0e3)/(250e3+0.5*100.0e3) ; % Minimum Throttle setting [-] 11 | thrustRateMax = 100e3 ; % [N/s] 12 | IspVac = 300 ; % Effective vacuum exhaust speed [s] 13 | Ae = 0.5 ; % Nozzle Exit Area [m2] 14 | includeBackPressure = 1 ; 15 | includeThrustRateLimit= 1 ; 16 | collocationMethod = 'foh' ; % thrust profile assumed collocation strategy (zoh/floor,foh/linear,spline,else) 17 | end 18 | properties (Dependent) 19 | ceffVac ; 20 | mdotVac ; 21 | mdotMax ; 22 | mdotMin ; 23 | thrustMax ; 24 | thrustMin ; 25 | end 26 | methods 27 | function thrustVac = computeVacuumThrust(obj,throttle) 28 | thrustVac = obj.thrustVac*throttle ; 29 | end 30 | function thrust = computeThrust(obj,throttle,atmosPressure) 31 | thrust = computeVacuumThrust(obj,throttle) ; 32 | if obj.includeBackPressure 33 | thrust = thrust - obj.Ae*atmosPressure ; 34 | end 35 | end 36 | function Isp = computeEffectiveIsp(obj,atmosPressure) 37 | Isp = obj.IspVac - obj.Ae*atmosPressure./obj.mdotVac/obj.g0; 38 | end 39 | function ceff = computeEffectiveExhaustSpeed(obj,atmosPressure) 40 | ceff = obj.ceffVac - obj.Ae*atmosPressure./obj.mdotVac; 41 | end 42 | function ceff = computeEffectiveExhaustSpeedDerivative(obj) 43 | ceff = - obj.Ae./obj.mdotVac; 44 | end 45 | function mdot = computeMassFlow(obj,throttle) 46 | mdot = throttle*obj.mdotVac ; 47 | end 48 | function ceffVac = get.ceffVac(obj) 49 | ceffVac = obj.IspVac*obj.g0 ; 50 | end 51 | function mdotVac = get.mdotVac(obj) 52 | mdotVac = obj.thrustVac/obj.ceffVac ; 53 | end 54 | function thrustMax = get.thrustMax(obj) 55 | thrustMax = computeVacuumThrust(obj,obj.throttleMax) ; 56 | end 57 | function thrustMin = get.thrustMin(obj) 58 | thrustMin = computeVacuumThrust(obj,obj.throttleMin) ; 59 | end 60 | function mdotMax = get.mdotMax(obj) 61 | mdotMax = computeMassFlow(obj,obj.throttleMax); 62 | end 63 | function mdotMin = get.mdotMin(obj) 64 | mdotMin = computeMassFlow(obj,obj.throttleMin); 65 | end 66 | end 67 | 68 | end 69 | 70 | -------------------------------------------------------------------------------- /test/rocketLanding/dynamics/controlMatrixRocketLandingFull.m: -------------------------------------------------------------------------------- 1 | function B = controlMatrixRocketLandingFull(t,X,U,BodyMap) 2 | %UNTITLED Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | Nodes = size(X,2); 6 | 7 | %% Retrieve Environment 8 | % Earth Shape Model 9 | % Thrust Model 10 | cex = BodyMap.ThrustModel.ceffVac; 11 | 12 | %% Compute State Derivatives 13 | B = zeros(size(X,1),size(U,1),Nodes); 14 | 15 | switch BodyMap.DynamicModel.controlVariable 16 | case 'thrust' 17 | M = X(7,:); 18 | for ii = 1:Nodes 19 | B(:,:,ii) = [... 20 | zeros(3,4);... 21 | eye(3)./repmat(M(ii),3,1),zeros(3,1);... 22 | zeros(1,3),-1/cex;... 23 | ]; 24 | end 25 | case 'acceleration' 26 | for ii = 1:Nodes 27 | B(:,:,ii) = [... 28 | zeros(3,4);... 29 | eye(3),zeros(3,1);... 30 | zeros(1,3),-1/cex;... 31 | ]; 32 | end 33 | end 34 | 35 | 36 | end 37 | 38 | -------------------------------------------------------------------------------- /test/rocketLanding/dynamics/controlMatrixSolutionRocketLandingFull.m: -------------------------------------------------------------------------------- 1 | function [B,B1] = controlMatrixSolutionRocketLandingFull(t,X,U,BodyMap) 2 | %UNTITLED Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | Nodes = length(t); 6 | dt = diff(t); 7 | 8 | %% Retrieve Environment 9 | 10 | % Thrust Model 11 | cex = BodyMap.ThrustModel.ceffVac; 12 | 13 | %% 14 | B = zeros(size(X,1),size(U,1),Nodes-1); 15 | B1 = zeros(size(X,1),size(U,1),Nodes-1); 16 | switch BodyMap.ThrustModel.collocationMethod 17 | case {'zoh','floor'} 18 | switch BodyMap.DynamicModel.controlVariable 19 | case 'thrust' 20 | M = X(7,:); 21 | for ii = 1:Nodes-1 22 | B(:,:,ii) = [... 23 | dt(ii)^2/2*eye(3)/M(ii),zeros(3,1);... 24 | dt(ii)*eye(3)/M(ii),zeros(3,1);... 25 | zeros(1,3),-dt(ii)/cex;... 26 | ]; 27 | end 28 | case 'acceleration' 29 | for ii = 1:Nodes-1 30 | B(:,:,ii) = [... 31 | dt(ii)^2/2*eye(3),zeros(3,1);... 32 | dt(ii)*eye(3),zeros(3,1);... 33 | zeros(1,3),-dt(ii)/cex;... 34 | ]; 35 | end 36 | end 37 | case {'foh','linear'} 38 | switch BodyMap.DynamicModel.controlVariable 39 | case 'thrust' 40 | M = X(7,:); 41 | for ii = 1:Nodes-1 42 | B(:,:,ii) = [... 43 | dt(ii)^2/3*eye(3)/M(ii),zeros(3,1);... 44 | dt(ii)/2*eye(3)/M(ii),zeros(3,1);... 45 | zeros(1,3),-dt(ii)/2/cex;... 46 | ]; 47 | B1(:,:,ii) = [... 48 | dt(ii)^2/6*eye(3)/M(ii+1),zeros(3,1);... 49 | dt(ii)/2*eye(3)/M(ii+1),zeros(3,1);... 50 | zeros(1,3),-dt(ii)/2/cex;... 51 | ]; 52 | end 53 | case 'acceleration' 54 | for ii = 1:Nodes-1 55 | B(:,:,ii) = [... 56 | dt(ii)^2/3*eye(3),zeros(3,1);... 57 | dt(ii)/2*eye(3),zeros(3,1);... 58 | zeros(1,3),-dt(ii)/2/cex;... 59 | ]; 60 | B1(:,:,ii) = [... 61 | dt(ii)^2/6*eye(3),zeros(3,1);... 62 | dt(ii)/2*eye(3),zeros(3,1);... 63 | zeros(1,3),-dt(ii)/2/cex;... 64 | ]; 65 | end 66 | end 67 | end 68 | end 69 | 70 | -------------------------------------------------------------------------------- /test/rocketLanding/dynamics/stateDerivativeRocketLanding.m: -------------------------------------------------------------------------------- 1 | function [Xdot] = stateDerivativeRocketLanding(t,X,U,BodyMap) 2 | %UNTITLED4 Summary of this function goes here 3 | % Detailed explanation goes here 4 | Xdot = zeros(size(X)); 5 | %% Retrieve State variables 6 | 7 | R = X(1:3,:); 8 | V = X(4:6,:); 9 | switch BodyMap.DynamicModel.controlVariable 10 | case 'thrust' 11 | M = X(7,:); 12 | Minv = 1./M; 13 | T = U(1:3,:); 14 | Tmag = U(4,:); 15 | a = T.*Minv; 16 | case 'acceleration' 17 | Z = X(7,:); 18 | Minv = exp(-Z); 19 | a = U(1:3,:); 20 | amag = U(4,:); 21 | end 22 | 23 | %% Retrieve Environment 24 | 25 | % Orientation Model 26 | aR = BodyMap.OrientationModel.computeRotatingAcceleration(R,V); 27 | siteNormal = [1;0;0]; 28 | 29 | % Earth Shape Model 30 | h = R(1,:); 31 | 32 | % Earth Gravity 33 | g = BodyMap.GravityModel.g; 34 | gVec = repmat(-siteNormal*g,1,length(t)); 35 | 36 | % Atmosphere Model 37 | if BodyMap.ThrustModel.includeBackPressure || BodyMap.AerodynamicModel.includeDrag 38 | [rho , pamb, ~ , soundSpeed] = BodyMap.AtmosphereModel.get_properties(h,[],[],[]); 39 | end 40 | 41 | % Thrust Model 42 | cex = BodyMap.ThrustModel.ceffVac; 43 | if BodyMap.ThrustModel.includeBackPressure 44 | mdotbp = -pamb*BodyMap.ThrustModel.Ae./cex; 45 | else 46 | mdotbp = 0; 47 | end 48 | 49 | % Aerodynamic 50 | if BodyMap.AerodynamicModel.includeDrag 51 | Area = BodyMap.AerodynamicModel.area; 52 | Vmag = sqrt(V(1,:).^2+V(2,:).^2+V(3,:).^2); 53 | mach = Vmag./soundSpeed; 54 | cd = BodyMap.AerodynamicModel.computeCd(mach); 55 | Drag = - 0.5*Area*repmat(rho.*cd.*Vmag,3,1).*V; 56 | aD = Drag.*repmat(Minv,3,1); 57 | else 58 | aD = zeros(3,length(t)); 59 | end 60 | 61 | % Sum external accelerations 62 | aE = aD + gVec + aR; 63 | %% Compute State Derivatives 64 | Xdot(1:3,:) = V; 65 | Xdot(4:6,:) = a + aE; 66 | switch BodyMap.DynamicModel.controlVariable 67 | case 'thrust' 68 | Xdot(7,:) = -Tmag./cex + mdotbp; % mdot = -(Teff + Ae*(Pa))/c, z = log(m), dz = 1/m*dm = -T/m/c - Ae*Pa/c/m = -a/c - Ae*Pa/c*exp(-z) 69 | case 'acceleration' 70 | Xdot(7,:) = -amag./cex + mdotbp.*Minv; % mdot = -(Teff + Ae*(Pa))/c, z = log(m), dz = 1/m*dm = -T/m/c - Ae*Pa/c/m = -a/c - Ae*Pa/c*exp(-z) 71 | end 72 | end 73 | 74 | -------------------------------------------------------------------------------- /test/rocketLanding/dynamics/stateDerivativeSolutionRocketLanding.m: -------------------------------------------------------------------------------- 1 | function [f] = stateDerivativeSolutionRocketLanding(t,X,U,BodyMap) 2 | %UNTITLED4 Summary of this function goes here 3 | % Detailed explanation goes here 4 | f = zeros(size(X,1),length(t)-1); 5 | dt = diff(t); 6 | %% Retrieve State variables 7 | 8 | R = X(1:3,:); 9 | V = X(4:6,:); 10 | 11 | switch BodyMap.DynamicModel.controlVariable 12 | case 'thrust' 13 | M = X(7,:); 14 | Minv = 1./M; 15 | T = U(1:3,:); 16 | Tmag = U(4,:); 17 | a = T.*Minv; 18 | case 'acceleration' 19 | Z = X(7,:); 20 | Minv = exp(-Z); 21 | a = U(1:3,:); 22 | amag = U(4,:); 23 | end 24 | 25 | %% Retrieve Environment 26 | 27 | % Orientation Model 28 | aR = BodyMap.OrientationModel.computeRotatingAcceleration(R,V); 29 | siteNormal = [1;0;0]; 30 | 31 | % Earth Shape Model 32 | h = R(1,:); 33 | 34 | % Earth Gravity 35 | g = BodyMap.GravityModel.g; 36 | gVec = repmat(-siteNormal*g,1,length(t)); 37 | 38 | % Atmosphere Model 39 | if BodyMap.ThrustModel.includeBackPressure || BodyMap.AerodynamicModel.includeDrag 40 | [rho , pamb, ~ , soundSpeed] = BodyMap.AtmosphereModel.get_properties(h,[],[],[]); 41 | end 42 | 43 | % Thrust Model 44 | cex = BodyMap.ThrustModel.ceffVac; 45 | 46 | if BodyMap.ThrustModel.includeBackPressure 47 | mdotbp = -pamb.*BodyMap.ThrustModel.Ae./cex; 48 | else 49 | mdotbp = zeros(1,length(t)); 50 | end 51 | 52 | 53 | % Aerodynamic 54 | if BodyMap.AerodynamicModel.includeDrag 55 | Area = BodyMap.AerodynamicModel.area; 56 | Vmag = sqrt(V(1,:).^2+V(2,:).^2+V(3,:).^2); 57 | mach = Vmag./soundSpeed; 58 | cd = BodyMap.AerodynamicModel.computeCd(mach); 59 | Drag = - 0.5*Area*repmat(rho.*cd.*Vmag,3,1).*V; 60 | aD = Drag.*repmat(Minv,3,1); 61 | else 62 | aD = zeros(3,length(t)); 63 | end 64 | 65 | % Sum external accelerations 66 | aE = aD + gVec + aR; 67 | %% Compute State Derivatives 68 | if BodyMap.DynamicModel.exactOde==0 69 | f(1:3,:) = repmat(dt,3,1).*V(:,1:end-1)+ repmat(dt.^2,3,1).*aE(:,1:end-1)/2; 70 | f(4:6,:) = repmat(dt,3,1).*(aE(:,1:end-1)); 71 | switch BodyMap.DynamicModel.controlVariable 72 | case 'thrust' 73 | f(7,:) = dt.*mdotbp(1:end-1); 74 | case 'acceleration' 75 | f(7,:) = dt.*mdotbp(1:end-1).*Minv(1:end-1); 76 | end 77 | elseif BodyMap.DynamicModel.exactOde==1 78 | f(1:3,:) = repmat(dt,3,1).*V(:,1:end-1)+ repmat(dt.^2,3,1).*(aE(:,1:end-1)/3+aE(:,2:end)/6); 79 | f(4:6,:) = repmat(dt,3,1).*(aE(:,1:end-1)/2+aE(:,2:end)/2); 80 | switch BodyMap.DynamicModel.controlVariable 81 | case 'thrust' 82 | f(7,:) = dt*1/2.*(mdotbp(1:end-1)+mdotbp(2:end)); 83 | case 'acceleration' 84 | f(7,:) = dt*1/2.*(mdotbp(1:end-1).*Minv(1:end-1)+mdotbp(2:end).*Minv(2:end)); 85 | end 86 | else 87 | error('Dynamics Error: exactOde type not defined \n') 88 | end 89 | 90 | switch BodyMap.ThrustModel.collocationMethod 91 | case {'zoh','floor'} 92 | f(1:3,:) = f(1:3,:) + repmat(dt.^2,3,1).*a(:,1:end-1)/2; 93 | f(4:6,:) = f(4:6,:) + repmat(dt,3,1).*a(:,1:end-1); 94 | switch BodyMap.DynamicModel.controlVariable 95 | case 'thrust' 96 | f(7,:) = f(7,:) - dt.*Tmag(1:end-1)./cex; 97 | case 'acceleration' 98 | f(7,:) = f(7,:) - dt.*amag(1:end-1)./cex; 99 | end 100 | case {'foh','linear'} 101 | f(1:3,:) = f(1:3,:) + repmat(dt.^2,3,1).*(a(:,1:end-1)/3+a(:,2:end)/6); 102 | f(4:6,:) = f(4:6,:) + repmat(dt,3,1).*(a(:,1:end-1)/2+a(:,2:end)/2); 103 | switch BodyMap.DynamicModel.controlVariable 104 | case 'thrust' 105 | f(7,:) = f(7,:) - dt.*(Tmag(1:end-1)./cex/2+Tmag(2:end)./cex/2); 106 | case 'acceleration' 107 | f(7,:) = f(7,:) - dt.*(amag(1:end-1)./cex/2+amag(2:end)./cex/2); 108 | end 109 | end 110 | 111 | end 112 | 113 | -------------------------------------------------------------------------------- /test/rocketLanding/dynamics/stateMatrixRocketLandingFull.m: -------------------------------------------------------------------------------- 1 | function A = stateMatrixRocketLandingFull(t,X,U,BodyMap) 2 | %UNTITLED Summary of this function goes here 3 | % Detailed explanation goes here 4 | Nodes = size(X,2); 5 | 6 | R = X(1:3,:); 7 | V = X(4:6,:); 8 | 9 | switch BodyMap.DynamicModel.controlVariable 10 | case 'thrust' 11 | M = X(7,:); 12 | Minv = 1./M; 13 | T = U(1:3,:); 14 | Tmag = U(4,:); 15 | a = T.*Minv; 16 | case 'acceleration' 17 | Z = X(7,:); 18 | Minv = exp(-Z); 19 | amag = U(4,:); 20 | end 21 | 22 | %% Retrieve Environment 23 | % Earth Shape Model 24 | h = R(1,:); 25 | 26 | % Aerodynamic Model 27 | if BodyMap.ThrustModel.includeBackPressure || BodyMap.AerodynamicModel.includeDrag 28 | [rho , pamb, ~ , soundSpeed] = BodyMap.AtmosphereModel.get_properties(h,[],[],[]); 29 | end 30 | 31 | % Orientation Model 32 | if BodyMap.OrientationModel.includeCentrifugal 33 | centrifugal_term = - BodyMap.OrientationModel.rotRateSkew*BodyMap.OrientationModel.rotRateSkew; 34 | else 35 | centrifugal_term = zeros(3,3) ; 36 | end 37 | if BodyMap.OrientationModel.includeCentripetal 38 | centripetal_term = - 2 * BodyMap.OrientationModel.rotRateSkew; 39 | else 40 | centripetal_term = zeros(3,3) ; 41 | end 42 | 43 | % Thrust Model 44 | if BodyMap.ThrustModel.includeBackPressure 45 | % get pressure derivatives with altitude 46 | cex = BodyMap.ThrustModel.ceffVac; 47 | dpdh = BodyMap.AtmosphereModel.get_dpdh(h); 48 | mdotbp = -pamb.*BodyMap.ThrustModel.Ae./cex; 49 | 50 | switch BodyMap.DynamicModel.controlVariable 51 | case 'thrust' 52 | dmdotbpdh = -dpdh.*BodyMap.ThrustModel.Ae./cex; 53 | case 'acceleration' 54 | dmdotbpdh = -dpdh.*BodyMap.ThrustModel.Ae./cex.*Minv; 55 | end 56 | else 57 | dmdotbpdh = zeros(1,Nodes); 58 | mdotbp = zeros(1,Nodes); 59 | end 60 | 61 | % Aerodynamic 62 | if BodyMap.AerodynamicModel.includeDrag 63 | Vmag = sqrt(V(1,:).^2+V(2,:).^2+V(3,:).^2); 64 | IndexZero = find(Vmag~=0); 65 | invVmag = zeros(1,Nodes); 66 | invVmag(IndexZero) = 1./Vmag(IndexZero); 67 | 68 | mach = Vmag./soundSpeed; 69 | dMda = -Vmag./soundSpeed.^2; 70 | [cD,dcDdM] = BodyMap.AerodynamicModel.computeCd(mach); 71 | Area = BodyMap.AerodynamicModel.area; 72 | Drag = - 0.5*Area*repmat(rho.*cD.*Vmag,3,1).*V; 73 | aD = Drag.*repmat(Minv,3,1); 74 | drhodh = BodyMap.AtmosphereModel.get_drhodh(h); 75 | dadh = BodyMap.AtmosphereModel.get_dadh(h); 76 | dcDdh = dcDdM.*dMda.*dadh; 77 | dDdh = - 0.5*Area*repmat((drhodh.*cD+rho.*dcDdh).*Vmag,3,1).*V; 78 | dDxdV = - 0.5*Area*( V.*repmat( rho.*cD.*V(1,:).*invVmag ,3,1) + [Vmag;zeros(2,Nodes)]) - 0.5*Area*repmat(rho.*Vmag.*V(1,:).*dcDdM,3,1).*V.*repmat(invVmag./soundSpeed ,3,1); 79 | dDydV = - 0.5*Area*( V.*repmat( rho.*cD.*V(2,:).*invVmag ,3,1) + [zeros(1,Nodes);Vmag;zeros(1,Nodes)]) - 0.5*Area*repmat(rho.*Vmag.*V(2,:).*dcDdM,3,1).*V.*repmat(invVmag./soundSpeed ,3,1); 80 | dDzdV = - 0.5*Area*( V.*repmat( rho.*cD.*V(3,:).*invVmag ,3,1) + [zeros(2,Nodes);Vmag]) - 0.5*Area*repmat(rho.*Vmag.*V(3,:).*dcDdM,3,1).*V.*repmat(invVmag./soundSpeed ,3,1); 81 | 82 | switch BodyMap.DynamicModel.controlVariable 83 | case 'thrust' 84 | daDdM = -aD.*repmat(Minv,3,1); 85 | case 'acceleration' 86 | daDdZ = -aD; 87 | end 88 | daDdh = dDdh .*Minv; 89 | daDxdV = dDxdV.*Minv; 90 | daDydV = dDydV.*Minv; 91 | daDzdV = dDzdV.*Minv; 92 | else 93 | daDdM = zeros(3,Nodes); 94 | daDdZ = zeros(3,Nodes); 95 | daDdh = zeros(3,Nodes); 96 | daDxdV = zeros(3,Nodes); 97 | daDydV = zeros(3,Nodes); 98 | daDzdV = zeros(3,Nodes); 99 | end 100 | 101 | %% Compute State Derivatives 102 | A = zeros(size(X,1),size(X,1),Nodes); 103 | switch BodyMap.DynamicModel.controlVariable 104 | case 'thrust' 105 | dadM = -a.*repmat(Minv,3,1); 106 | for ii = 1:Nodes 107 | A(:,:,ii) = [... 108 | zeros(3),eye(3),zeros(3,1);... 109 | [daDdh(:,ii),zeros(3,2) ] + centrifugal_term,centripetal_term + [daDxdV(:,ii)';daDydV(:,ii)';daDzdV(:,ii)'],daDdM(:,ii)+dadM(:,ii);... 110 | [dmdotbpdh(ii),zeros(1,2),zeros(1,4)] ;... 111 | ]; 112 | end 113 | case 'acceleration' 114 | for ii = 1:Nodes 115 | A(:,:,ii) = [... 116 | zeros(3),eye(3),zeros(3,1);... 117 | [daDdh(:,ii),zeros(3,2) ] + centrifugal_term,centripetal_term + [daDxdV(:,ii)';daDydV(:,ii)';daDzdV(:,ii)'],daDdZ(:,ii);... 118 | [dmdotbpdh(ii),zeros(1,2),zeros(1,3),-mdotbp(ii)*Minv(ii)] ;... 119 | ]; 120 | end 121 | end 122 | 123 | 124 | end 125 | 126 | -------------------------------------------------------------------------------- /test/rocketLanding/dynamics/stateMatrixSolutionRocketLandingFull.m: -------------------------------------------------------------------------------- 1 | function [A,A1] = stateMatrixSolutionRocketLandingFull(t,X,U,BodyMap) 2 | %UNTITLED Summary of this function goes here 3 | % Detailed explanation goes here 4 | Nodes = size(X,2); 5 | dt = diff(t); 6 | 7 | R = X(1:3,:); 8 | V = X(4:6,:); 9 | switch BodyMap.DynamicModel.controlVariable 10 | case 'thrust' 11 | M = X(7,:); 12 | Minv = 1./M; 13 | T = U(1:3,:); 14 | Tmag = U(4,:); 15 | a = T.*Minv; 16 | case 'acceleration' 17 | Z = X(7,:); 18 | Minv = exp(-Z); 19 | amag = U(4,:); 20 | end 21 | 22 | %% Retrieve Environment 23 | % Earth Shape Model 24 | h = R(1,:); 25 | 26 | % Atmosphere Model 27 | if BodyMap.ThrustModel.includeBackPressure || BodyMap.AerodynamicModel.includeDrag 28 | [rho , pamb, ~ , soundSpeed] = BodyMap.AtmosphereModel.get_properties(h,[],[],[]); 29 | end 30 | 31 | % Orientation Model 32 | if BodyMap.OrientationModel.includeCentrifugal 33 | centrifugal_term = - BodyMap.OrientationModel.rotRateSkew*BodyMap.OrientationModel.rotRateSkew; 34 | else 35 | centrifugal_term = zeros(3,3) ; 36 | end 37 | if BodyMap.OrientationModel.includeCentripetal 38 | centripetal_term = - 2 * BodyMap.OrientationModel.rotRateSkew; 39 | else 40 | centripetal_term = zeros(3,3) ; 41 | end 42 | 43 | % Thrust Model 44 | if BodyMap.ThrustModel.includeBackPressure 45 | % get pressure derivatives with altitude 46 | cex = BodyMap.ThrustModel.ceffVac; 47 | mdotbp = -pamb.*BodyMap.ThrustModel.Ae./cex; 48 | dpdh = BodyMap.AtmosphereModel.get_dpdh(h); 49 | dmdotbpdh = -dpdh.*BodyMap.ThrustModel.Ae./cex; 50 | else 51 | mdotbp = zeros(1,Nodes); 52 | dmdotbpdh = zeros(1,Nodes); 53 | end 54 | 55 | % Aerodynamic 56 | if BodyMap.AerodynamicModel.includeDrag 57 | Vmag = sqrt(V(1,:).^2+V(2,:).^2+V(3,:).^2); 58 | IndexZero = find(Vmag~=0); 59 | invVmag = zeros(1,Nodes); 60 | invVmag(IndexZero) = 1./Vmag(IndexZero); 61 | 62 | mach = Vmag./soundSpeed; 63 | [cD,dcDdM] = BodyMap.AerodynamicModel.computeCd(mach); 64 | Area = BodyMap.AerodynamicModel.area; 65 | Drag = - 0.5*Area*repmat(rho.*cD.*Vmag,3,1).*V; 66 | aD = Drag.*repmat(Minv,3,1); 67 | dMda = -Vmag./soundSpeed.^2; 68 | drhodh = BodyMap.AtmosphereModel.get_drhodh(h); 69 | dadh = BodyMap.AtmosphereModel.get_dadh(h); 70 | dcDdh = dcDdM.*dMda.*dadh; 71 | dDdh = - 0.5*Area*repmat((drhodh.*cD+rho.*dcDdh).*Vmag,3,1).*V; 72 | dDxdV = - 0.5*Area*( V.*repmat( rho.*cD.*V(1,:).*invVmag ,3,1) + [Vmag;zeros(2,Nodes)]) - 0.5*Area*repmat(rho.*Vmag.*V(1,:).*dcDdM,3,1).*V.*repmat(invVmag./soundSpeed ,3,1); 73 | dDydV = - 0.5*Area*( V.*repmat( rho.*cD.*V(2,:).*invVmag ,3,1) + [zeros(1,Nodes);Vmag;zeros(1,Nodes)]) - 0.5*Area*repmat(rho.*Vmag.*V(2,:).*dcDdM,3,1).*V.*repmat(invVmag./soundSpeed ,3,1); 74 | dDzdV = - 0.5*Area*( V.*repmat( rho.*cD.*V(3,:).*invVmag ,3,1) + [zeros(2,Nodes);Vmag]) - 0.5*Area*repmat(rho.*Vmag.*V(3,:).*dcDdM,3,1).*V.*repmat(invVmag./soundSpeed ,3,1); 75 | 76 | switch BodyMap.DynamicModel.controlVariable 77 | case 'thrust' 78 | daDdM = -aD.*repmat(Minv,3,1); 79 | case 'acceleration' 80 | daDdZ = -aD; 81 | end 82 | daDdh = dDdh .*Minv; 83 | daDxdV = dDxdV.*Minv; 84 | daDydV = dDydV.*Minv; 85 | daDzdV = dDzdV.*Minv; 86 | else 87 | daDdM = zeros(3,Nodes); 88 | daDdZ = zeros(3,Nodes); 89 | daDdh = zeros(3,Nodes); 90 | daDxdV = zeros(3,Nodes); 91 | daDydV = zeros(3,Nodes); 92 | daDzdV = zeros(3,Nodes); 93 | end 94 | %% Compute State Derivatives 95 | A = zeros(size(X,1),size(X,1),Nodes-1); 96 | A1 = zeros(size(X,1),size(X,1),Nodes-1); 97 | 98 | switch BodyMap.DynamicModel.controlVariable 99 | case 'thrust' 100 | dadM = -a.*repmat(Minv,3,1); 101 | if BodyMap.DynamicModel.exactOde==0 102 | for ii = 1:Nodes-1 103 | A(:,:,ii) = [... 104 | dt(ii)^2/2*[ daDdh(:,ii) , zeros(3,2) ] + dt(ii)^2/2*centrifugal_term,dt(ii)*eye(3)+dt(ii)^2/2*centripetal_term + dt(ii)^2/2*[daDxdV(:,ii)';daDydV(:,ii)';daDzdV(:,ii)'],dt(ii)^2/2*daDdM(:,ii);... 105 | dt(ii)*[ daDdh(:,ii) , zeros(3,2) ] + dt(ii)*centrifugal_term,dt(ii)*centripetal_term + dt(ii)*[daDxdV(:,ii)';daDydV(:,ii)';daDzdV(:,ii)'],dt(ii)*daDdM(:,ii);... 106 | dt(ii)*[dmdotbpdh(ii),zeros(1,2),zeros(1,4)] ;... 107 | ]; 108 | end 109 | elseif BodyMap.DynamicModel.exactOde==1 110 | for ii = 1:Nodes-1 111 | A(:,:,ii) = [... 112 | dt(ii)^2/3*[ daDdh(:,ii) , zeros(3,2) ] + dt(ii)^2/3*centrifugal_term,dt(ii)*eye(3)+dt(ii)^2/3*centripetal_term + dt(ii)^2/3*[daDxdV(:,ii)';daDydV(:,ii)';daDzdV(:,ii)'],dt(ii)^2/3*daDdM(:,ii);... 113 | dt(ii)/2*[ daDdh(:,ii) , zeros(3,2) ] + dt(ii)/2*centrifugal_term,dt(ii)/2*centripetal_term + dt(ii)/2*[daDxdV(:,ii)';daDydV(:,ii)';daDzdV(:,ii)'],dt(ii)/2*daDdM(:,ii);... 114 | dt(ii)/2*[dmdotbpdh(ii),zeros(1,2),zeros(1,4)] ;... 115 | ]; 116 | A1(:,:,ii) = [... 117 | dt(ii)^2/6*[ daDdh(:,ii+1) , zeros(3,2) ] + dt(ii)^2/6*centrifugal_term,dt(ii)^2/6*centripetal_term + dt(ii)^2/6*[daDxdV(:,ii+1)';daDydV(:,ii+1)';daDzdV(:,ii+1)'],dt(ii)^2/6*daDdM(:,ii+1);... 118 | dt(ii)/2*[ daDdh(:,ii+1) , zeros(3,2) ] + dt(ii)/2*centrifugal_term,dt(ii)/2*centripetal_term + dt(ii)/2*[daDxdV(:,ii+1)';daDydV(:,ii+1)';daDzdV(:,ii+1)'],dt(ii)/2*daDdM(:,ii+1);... 119 | dt(ii)/2*[dmdotbpdh(ii+1),zeros(1,2),zeros(1,4)] ;... 120 | ]; 121 | end 122 | else 123 | error('Dynamics Error: exactOde type not defined \n') 124 | end 125 | 126 | switch BodyMap.ThrustModel.collocationMethod 127 | case {'zoh','floor'} 128 | for ii = 1:Nodes-1 129 | A(1:3,7,ii) = A(1:3,7,ii) + dt(ii)^2*dadM(:,ii); 130 | A(4:6,7,ii) = A(4:6,7,ii) + dt(ii)*dadM(:,ii); 131 | end 132 | case {'foh','linear'} 133 | for ii = 1:Nodes-1 134 | A(1:3,7,ii) = A(1:3,7,ii) + dt(ii)^2*dadM(:,ii)/3; 135 | A(4:6,7,ii) = A(4:6,7,ii) + dt(ii)*dadM(:,ii)/2; 136 | A1(1:3,7,ii) = A1(1:3,7,ii) + dt(ii)^2*dadM(:,ii+1)/6; 137 | A1(4:6,7,ii) = A1(4:6,7,ii) + dt(ii)*dadM(:,ii+1)/2; 138 | end 139 | end 140 | 141 | case 'acceleration' 142 | if BodyMap.DynamicModel.exactOde==0 143 | for ii = 1:Nodes-1 144 | A(:,:,ii) = [... 145 | dt(ii)^2/2*[ daDdh(:,ii) , zeros(3,2) ] + dt(ii)^2/2*centrifugal_term,dt(ii)*eye(3)+dt(ii)^2/2*centripetal_term + dt(ii)^2/2*[daDxdV(:,ii)';daDydV(:,ii)';daDzdV(:,ii)'],dt(ii)^2/2*daDdZ(:,ii);... 146 | dt(ii)*[ daDdh(:,ii) , zeros(3,2) ] + dt(ii)*centrifugal_term,dt(ii)*centripetal_term + dt(ii)*[daDxdV(:,ii)';daDydV(:,ii)';daDzdV(:,ii)'],dt(ii)*daDdZ(:,ii);... 147 | dt(ii)*[dmdotbpdh(ii)*Minv(ii),zeros(1,2),zeros(1,3),-mdotbp(ii)*Minv(ii)] ;... 148 | ]; 149 | end 150 | elseif BodyMap.DynamicModel.exactOde==1 151 | for ii = 1:Nodes-1 152 | A(:,:,ii) = [... 153 | dt(ii)^2/3*[ daDdh(:,ii) , zeros(3,2) ] + dt(ii)^2/3*centrifugal_term,dt(ii)*eye(3)+dt(ii)^2/3*centripetal_term + dt(ii)^2/3*[daDxdV(:,ii)';daDydV(:,ii)';daDzdV(:,ii)'],dt(ii)^2/3*daDdZ(:,ii);... 154 | dt(ii)/2*[ daDdh(:,ii) , zeros(3,2) ] + dt(ii)/2*centrifugal_term,dt(ii)/2*centripetal_term + dt(ii)/2*[daDxdV(:,ii)';daDydV(:,ii)';daDzdV(:,ii)'],dt(ii)/2*daDdZ(:,ii);... 155 | dt(ii)/2*[dmdotbpdh(ii)*Minv(ii),zeros(1,2),zeros(1,3),-mdotbp(ii)*Minv(ii)] ;... 156 | ]; 157 | A1(:,:,ii) = [... 158 | dt(ii)^2/6*[ daDdh(:,ii+1) , zeros(3,2) ] + dt(ii)^2/6*centrifugal_term,dt(ii)^2/6*centripetal_term + dt(ii)^2/6*[daDxdV(:,ii+1)';daDydV(:,ii+1)';daDzdV(:,ii+1)'],dt(ii)^2/6*daDdZ(:,ii+1);... 159 | dt(ii)/2*[ daDdh(:,ii+1) , zeros(3,2) ] + dt(ii)/2*centrifugal_term,dt(ii)/2*centripetal_term + dt(ii)/2*[daDxdV(:,ii+1)';daDydV(:,ii+1)';daDzdV(:,ii+1)'],dt(ii)/2*daDdZ(:,ii+1);... 160 | dt(ii)/2*[dmdotbpdh(ii+1)*Minv(ii+1),zeros(1,2),zeros(1,3),-mdotbp(ii+1)*Minv(ii+1)] ;... 161 | ]; 162 | end 163 | else 164 | error('Dynamics Error: exactOde type not defined \n') 165 | end 166 | 167 | end 168 | end 169 | 170 | -------------------------------------------------------------------------------- /test/rocketLanding/dynamics/timeDerivativeSolutionRocketLanding.m: -------------------------------------------------------------------------------- 1 | function [dfdt] = timeDerivativeSolutionRocketLanding(t,X,U,BodyMap) 2 | %UNTITLED4 Summary of this function goes here 3 | % Detailed explanation goes here 4 | dfdt = zeros(size(X,1),length(t)-1); 5 | dt = diff(t); 6 | %% Retrieve State variables 7 | 8 | R = X(1:3,:); 9 | V = X(4:6,:); 10 | switch BodyMap.DynamicModel.controlVariable 11 | case 'thrust' 12 | M = X(7,:); 13 | Minv = 1./M; 14 | T = U(1:3,:); 15 | Tmag = U(4,:); 16 | a = T.*Minv; 17 | case 'acceleration' 18 | Z = X(7,:); 19 | Minv = exp(-Z); 20 | a = U(1:3,:); 21 | amag = U(4,:); 22 | end 23 | 24 | 25 | %% Retrieve Environment 26 | 27 | % Orientation Model 28 | aR = BodyMap.OrientationModel.computeRotatingAcceleration(R,V); 29 | siteNormal = [1;0;0]; 30 | 31 | % Earth Shape Model 32 | h = R(1,:); 33 | 34 | % Earth Gravity 35 | g = BodyMap.GravityModel.g; 36 | gVec = repmat(-siteNormal*g,1,length(t)); 37 | 38 | % Atmosphere Model 39 | if BodyMap.ThrustModel.includeBackPressure || BodyMap.AerodynamicModel.includeDrag 40 | [rho , pamb, ~ , soundSpeed] = BodyMap.AtmosphereModel.get_properties(h,[],[],[]); 41 | end 42 | 43 | % Thrust Model 44 | cex = BodyMap.ThrustModel.ceffVac; 45 | 46 | if BodyMap.ThrustModel.includeBackPressure 47 | mdotbp = -pamb.*BodyMap.ThrustModel.Ae./cex; 48 | else 49 | mdotbp = zeros(1,length(t)); 50 | end 51 | 52 | 53 | % Aerodynamic 54 | if BodyMap.AerodynamicModel.includeDrag 55 | Area = BodyMap.AerodynamicModel.area; 56 | Vmag = sqrt(V(1,:).^2+V(2,:).^2+V(3,:).^2); 57 | mach = Vmag./soundSpeed; 58 | cd = BodyMap.AerodynamicModel.computeCd(mach); 59 | Drag = - 0.5*Area*repmat(rho.*cd.*Vmag,3,1).*V; 60 | aD = Drag.*repmat(Minv,3,1); 61 | else 62 | aD = zeros(3,length(t)); 63 | end 64 | 65 | % Sum external accelerations 66 | aE = aD + gVec + aR; 67 | %% Compute State Derivatives 68 | if BodyMap.DynamicModel.exactOde==0 69 | dfdt(1:3,:) = V(:,1:end-1)+ repmat(dt,3,1).*aE(:,1:end-1); 70 | dfdt(4:6,:) = aE(:,1:end-1); 71 | switch BodyMap.DynamicModel.controlVariable 72 | case 'thrust' 73 | dfdt(7,:) = mdotbp(1:end-1); 74 | case 'acceleration' 75 | dfdt(7,:) = mdotbp(1:end-1).*Minv(1:end-1); 76 | end 77 | elseif BodyMap.DynamicModel.exactOde==1 78 | dfdt(1:3,:) = V(:,1:end-1)+ 2*repmat(dt,3,1)/3.*(aE(:,1:end-1)+aE(:,2:end)/2); 79 | dfdt(4:6,:) = 1/2.*(aE(:,1:end-1)+aE(:,2:end)); 80 | switch BodyMap.DynamicModel.controlVariable 81 | case 'thrust' 82 | dfdt(7,:) = 1/2.*(mdotbp(1:end-1)+mdotbp(2:end)); 83 | case 'acceleration' 84 | dfdt(7,:) = 1/2.*(mdotbp(1:end-1).*Minv(1:end-1)+mdotbp(2:end).*Minv(2:end)); 85 | end 86 | else 87 | error('Dynamics Error: exactOde type not defined \n') 88 | end 89 | 90 | switch BodyMap.ThrustModel.collocationMethod 91 | case {'zoh','floor'} 92 | dfdt(1:3,:) = dfdt(1:3,:) + repmat(dt,3,1).*a(:,1:end-1); 93 | dfdt(4:6,:) = dfdt(4:6,:) + a(:,1:end-1); 94 | switch BodyMap.DynamicModel.controlVariable 95 | case 'thrust' 96 | dfdt(7,:) = dfdt(7,:) - Tmag(1:end-1)./cex; 97 | case 'acceleration' 98 | dfdt(7,:) = dfdt(7,:) - amag(1:end-1)./cex; 99 | end 100 | case {'foh','linear'} 101 | dfdt(1:3,:) = dfdt(1:3,:) + 2*repmat(dt,3,1).*(a(:,1:end-1)/3+a(:,2:end)/6); 102 | dfdt(4:6,:) = dfdt(4:6,:) + a(:,1:end-1)/2+a(:,2:end)/2; 103 | switch BodyMap.DynamicModel.controlVariable 104 | case 'thrust' 105 | dfdt(7,:) = dfdt(7,:) - 1/2*(Tmag(1:end-1)+Tmag(2:end))/cex; 106 | case 'acceleration' 107 | dfdt(7,:) = dfdt(7,:) - 1/2*(amag(1:end-1)+amag(2:end))/cex; 108 | end 109 | end 110 | end 111 | 112 | -------------------------------------------------------------------------------- /test/spacecraftLanding/controlMatrixSpacecraft.m: -------------------------------------------------------------------------------- 1 | function [row,col,val,nEntries] = controlMatrixSpacecraft(t,X,U,BodyMap,ii) 2 | %UNTITLED Summary of this function goes here 3 | % Detailed explanation goes here 4 | planet = BodyMap.CentralBody ; 5 | vehicle = BodyMap.ControlledBody ; 6 | nodes = size(X,2); 7 | 8 | %% Retrieve Environment 9 | 10 | % Mass Model 11 | Mass = BodyMap.(vehicle).MassModel.mass ; 12 | %% 13 | row = repmat((4:6)',1,nodes); 14 | col = repmat((1:3)',1,nodes); 15 | 16 | nEntries= size(row,1); 17 | 18 | val = zeros(nEntries,nodes); 19 | 20 | val( 1:3,:) = 1/Mass*ones(3,nodes); 21 | end 22 | 23 | -------------------------------------------------------------------------------- /test/spacecraftLanding/stateDerivativeSpacecraft.m: -------------------------------------------------------------------------------- 1 | function [Xdot] = stateDerivativeSpacecraft(t,X,U,BodyMap,ii) 2 | %UNTITLED4 Summary of this function goes here 3 | % Detailed explanation goes here 4 | planet = BodyMap.CentralBody ; 5 | vehicle = BodyMap.ControlledBody ; 6 | Xdot = zeros(size(X)); 7 | %% Retrieve State variables 8 | 9 | P = X(1:3,:); 10 | V = X(4:6,:); 11 | T = U(1:3,:); 12 | 13 | %% Retrieve Environment 14 | 15 | % Earth Gravity 16 | g = BodyMap.(planet).Gravity.g; 17 | vector = BodyMap.(planet).Gravity.vector; 18 | g_vec = vector(:)*g; 19 | 20 | % Mass Model 21 | Mass = BodyMap.(vehicle).MassModel.mass ; 22 | %% Compute State Derivatives 23 | Xdot(1:3,:) = V; 24 | Xdot(4:6,:) = T/Mass + repmat(g_vec,1,size(T,2)); 25 | 26 | end 27 | 28 | -------------------------------------------------------------------------------- /test/spacecraftLanding/stateMatrixSpacecraft.m: -------------------------------------------------------------------------------- 1 | function [row,col,val,nEntries] = stateMatrixSpacecraft(t,X,U,BodyMap,ii) 2 | %UNTITLED Summary of this function goes here 3 | % Detailed explanation goes here 4 | planet = BodyMap.CentralBody ; 5 | vehicle = BodyMap.ControlledBody ; 6 | nodes = size(X,2); 7 | 8 | %% 9 | row = repmat((1:3)',1,nodes); 10 | col = repmat((4:6)',1,nodes); 11 | 12 | nEntries= size(row,1); 13 | 14 | val = zeros(nEntries,nodes); 15 | 16 | val( 1:3,:) = ones(3,nodes); 17 | 18 | end 19 | 20 | -------------------------------------------------------------------------------- /test/spacecraftLanding/unitTest_spacecraftLanding.m: -------------------------------------------------------------------------------- 1 | % Spacecraft Landing with fixed final mass unit test. problem is fully 2 | % convex and can be solved with one iteration. Solved with free final time 3 | % to check for differences 4 | % A full Description is available in TU Delft Repository: 5 | % 'Optimum On-board Abort Guidance based on Successive Convexification 6 | % for Atmospheric Re-Entry Vehicles', Master Thesis, Guillermo 7 | % Joaquin Dominguez Calabuig. 8 | % 9 | 10 | clc; clear all; close all; 11 | addpath(genpath('../../src')); 12 | 13 | 14 | vehicle = 'Spacecraft'; 15 | planet = 'Mars'; 16 | 17 | dbstop if error 18 | 19 | %% Environment And Vehicle 20 | 21 | 22 | P0 = [50,50,100]'; 23 | V0 = [-10,0,-10]'; 24 | 25 | t0_IG = 0; 26 | 27 | tanGlide = 0.5; 28 | 29 | % Gravity 30 | Gravity.type = 0; 31 | Gravity.g = 0.1; 32 | Gravity.vector = [0;0;-1]; 33 | 34 | ThrustModel.Fmax = 10; 35 | ThrustModel.gamma = 1; 36 | 37 | MassModel.type = 0; 38 | MassModel.mass = 10; 39 | 40 | Nodes = 36; 41 | %% Define Body Map 42 | BodyMap.(planet).Gravity = Gravity; 43 | BodyMap.(vehicle).ThrustModel = ThrustModel; 44 | BodyMap.(vehicle).MassModel = MassModel; 45 | BodyMap.CentralBody = planet ; 46 | BodyMap.ControlledBody = vehicle ; 47 | %% Initial Guess 48 | X_0 = [P0;V0]; 49 | X_IG = zeros(length(X_0),Nodes); 50 | for ii = 1:length(X_0) 51 | X_IG(ii,:) = linspace(X_0(ii),0,Nodes); 52 | end 53 | % 54 | unitVectorTarget = ([0;0;0]-P0)/norm(([0;0;0]-P0)); 55 | U_IG = -repmat(unitVectorTarget*ThrustModel.Fmax*3/4,1,Nodes); 56 | 57 | %% 58 | problems = [1 2 3 3]; 59 | 60 | tf_IG = [35 35 35 35]; 61 | 62 | NumberProblems = length(problems); 63 | 64 | 65 | 66 | X = zeros(6,Nodes,NumberProblems); 67 | U = zeros(3,Nodes,NumberProblems); 68 | Jreal = zeros(1,NumberProblems); 69 | timeFinal = zeros(1,NumberProblems); 70 | timeElapsed = zeros(1,NumberProblems); 71 | 72 | for ii = 1:NumberProblems 73 | 74 | ProblemType = problems(ii); 75 | 76 | if ProblemType==2 || ProblemType==3 77 | freeTime = 1; 78 | else 79 | freeTime = 0; 80 | end 81 | %% Setup SCOPT 82 | 83 | Scopt = scopt2; 84 | 85 | Scopt.problem.nphases = 1; 86 | Scopt.problem.phases(1).Nodes = Nodes; 87 | Scopt.problem.phases(1).n.states = 6; 88 | Scopt.problem.phases(1).n.controls = 3; 89 | Scopt.problem.phases(1).n.path = 2; 90 | Scopt.problem.phases(1).freeTimeFinal = freeTime; 91 | 92 | Scopt.setUpSCOPT_level1(); 93 | %% Interface Main Input 94 | index.states.position = (1:3)' ; 95 | index.states.velocity = (4:6)' ; 96 | index.controls.thrust = (1:3)' ; 97 | 98 | Scopt.problem.phases(1).initialGuess.states = X_IG; 99 | Scopt.problem.phases(1).initialGuess.controls = U_IG; 100 | Scopt.problem.phases(1).initialGuess.timeInitial = t0_IG; 101 | Scopt.problem.phases(1).initialGuess.timeFinal = tf_IG(ii); 102 | 103 | % Internal Body Model 104 | Scopt.problem.phases(1).BodyMap = BodyMap; 105 | 106 | % Dynamics 107 | Scopt.problem.phases(1).dynamics.stateDerivativeFunction = @(t,X,U,BodyMap,ii) stateDerivativeSpacecraft(t,X,U,BodyMap,ii); 108 | Scopt.problem.phases(1).dynamics.stateMatrixFunction = @(t,X,U,BodyMap,ii) stateMatrixSpacecraft(t,X,U,BodyMap,ii); 109 | Scopt.problem.phases(1).dynamics.controlMatrixFunction = @(t,X,U,BodyMap,ii) controlMatrixSpacecraft(t,X,U,BodyMap,ii); 110 | 111 | % Successive Convexification Speed Up 112 | Scopt.problem.phases(1).interpolateParameters = 0; 113 | 114 | % Bounds 115 | Scopt.problem.phases(1).bounds.timeFinal.upper = 50 ; 116 | Scopt.problem.phases(1).bounds.timeFinal.lower = 20 ; 117 | 118 | % Convex Path Constraints (usually resulting from lossless convexification) 119 | Scopt.problem.phases(1).path(1).cone = Scopt.initCone(4,Scopt.problem.phases(1).n); 120 | Scopt.problem.phases(1).path(1).type = 'upper'; 121 | Scopt.problem.phases(1).path(1).funType = 'convex'; 122 | Scopt.problem.phases(1).path(1).cone.norm.controls(1,1) = 1; 123 | Scopt.problem.phases(1).path(1).cone.norm.controls(2,2) = 1; 124 | Scopt.problem.phases(1).path(1).cone.norm.controls(3,3) = 1; 125 | Scopt.problem.phases(1).path(1).cone.right.cons = ThrustModel.Fmax; 126 | 127 | Scopt.problem.phases(1).path(2).cone = Scopt.initCone(3,Scopt.problem.phases(1).n); 128 | Scopt.problem.phases(1).path(2).type = 'upper'; 129 | Scopt.problem.phases(1).path(2).funType = 'convex'; 130 | Scopt.problem.phases(1).path(2).cone.norm.states(1,1) = 1; 131 | Scopt.problem.phases(1).path(2).cone.norm.states(2,2) = 1; 132 | Scopt.problem.phases(1).path(2).cone.right.states(3) = 1/tanGlide; 133 | 134 | % Initialing Conditions 135 | 136 | Scopt.problem.phases(1).initial.states.equal.index = ... 137 | [index.states.position;... 138 | index.states.velocity]; 139 | Scopt.problem.phases(1).initial.states.equal.value = ... 140 | [P0;... 141 | V0]; 142 | 143 | % Final Conditions 144 | 145 | Scopt.problem.phases(1).final.states.equal.index = [index.states.position;index.states.velocity]; 146 | Scopt.problem.phases(1).final.states.equal.value = zeros(6,1); 147 | 148 | switch ProblemType 149 | case 0 150 | Scopt.problem.objective.type = 'feasibility'; 151 | case 1 152 | Scopt.problem.objective.type = 'minimize'; 153 | Scopt.problem.objective.scale = 200 ; 154 | Scopt.problem.objective.lagrange.funType = 'convex'; 155 | Scopt.problem.objective.lagrange.cone = Scopt.initCone(4,Scopt.problem.phases(1).n); 156 | Scopt.problem.objective.lagrange.cone.norm.controls(1,1) = ThrustModel.gamma; 157 | Scopt.problem.objective.lagrange.cone.norm.controls(2,2) = ThrustModel.gamma; 158 | Scopt.problem.objective.lagrange.cone.norm.controls(3,3) = ThrustModel.gamma; 159 | 160 | Scopt.algorithm.sequential.activate = 0; 161 | case 2 162 | Scopt.problem.objective.type = 'minimize'; 163 | Scopt.problem.objective.scale = tf_IG(ii); 164 | Scopt.problem.objective.mayer.funType = 'linear'; % 1 for linear, 2 for convex, 3 for non-convex 165 | Scopt.problem.objective.mayer.where = 'final'; 166 | Scopt.problem.objective.mayer.timeFinal = 1; 167 | Scopt.algorithm.sequential.activate = 1; 168 | case 3 169 | Scopt.problem.objective.type = 'minimize'; 170 | Scopt.problem.objective.scale = 200 ; 171 | Scopt.problem.objective.lagrange.funType = 'convex'; 172 | Scopt.problem.objective.lagrange.cone = Scopt.initCone(4,Scopt.problem.phases(1).n); 173 | Scopt.problem.objective.lagrange.cone.norm.controls(1,1) = ThrustModel.gamma; 174 | Scopt.problem.objective.lagrange.cone.norm.controls(2,2) = ThrustModel.gamma; 175 | Scopt.problem.objective.lagrange.cone.norm.controls(3,3) = ThrustModel.gamma; 176 | Scopt.algorithm.sequential.activate = 1; 177 | end 178 | 179 | % Scopt.algorithm 180 | Scopt.algorithm.conicSolver.feasTol = eps^(1/2); 181 | Scopt.algorithm.conicSolver.relTol = eps^(1/2); 182 | Scopt.algorithm.conicSolver.absTol = eps^(1/2); 183 | Scopt.algorithm.conicSolver.maxIter = 100; 184 | 185 | Scopt.algorithm.sequential.type = 'none'; 186 | Scopt.algorithm.sequential.maxIter = 20; % trust region or line-search 187 | Scopt.algorithm.sequential.minIter = 0; 188 | % Scopt.algorithm.sequential.Jtol = 0.0001; 189 | % Scopt.algorithm.sequential.JtolChange = 0.0001; 190 | 191 | Scopt.algorithm.initialGuessType = 'user'; 192 | Scopt.algorithm.scaling.variables = 'automatic'; 193 | Scopt.algorithm.collocationMethod = 'trapezoidal'; 194 | Scopt.algorithm.scaling.events = 'automatic-limit'; % Alternative is automatic-limit automatic-jacobian 195 | Scopt.algorithm.scaling.path = 'automatic-jacobian'; % Alternative is automatic-limit automatic-jacobian 196 | Scopt.algorithm.scaling.objective = 'automatic-jacobian'; % Alternative is automatic-jacobian 197 | Scopt.algorithm.meshRefinement = 'none'; % No Mesh Refinement Implemented (YET) 198 | 199 | Scopt.algorithm.sequential.trustRegion.phases(1).timeFinal.component = 5; 200 | Scopt.algorithm.sequential.trustRegion.phases(1).timeFinal.lambda = 1.0e0; 201 | Scopt.algorithm.sequential.trustRegion.phases(1).timeFinal.type = 'hard'; 202 | 203 | if ii==4 204 | Scopt.algorithm.sequential.trustRegion.phases(1).timeFinal.type = 'soft'; 205 | Scopt.algorithm.sequential.trustRegion.phases(1).timeFinal.lambda = 3e-1; 206 | end 207 | 208 | Scopt.setUpSCOPT_level2(); 209 | %% Solve Scopt.problem 210 | Scopt.solveSCOPTProblem(); 211 | %% Retrieve Scopt.solutions 212 | 213 | Solution = Scopt.solution; 214 | 215 | X_IG = Scopt.problem.phases.initialGuess.states ; 216 | X(:,:,ii) = Solution.states; 217 | U(:,:,ii) = Solution.controls; 218 | Jreal(ii) = Solution.Jreal; 219 | timeFinal(ii) = Solution.timeFinal; 220 | timeElapsed(ii) = Solution.timeElapsed; 221 | end 222 | 223 | 224 | %% External Post Processing 225 | 226 | 227 | % figure;defaultColor = get(gca,'colororder');close(gcf) 228 | defaultColor = [1 0 0;0 1 0;0 0 1;0.5 1 1]; 229 | linestyle = {'-','-','-','-.'}; 230 | 231 | p_IG = X_IG(1:3,:); 232 | p = X(1:3,:,:); 233 | f = U(1:3,:,:); 234 | 235 | %% Plot 3D 236 | % Plot Glide Cone 237 | x = linspace(-40,55,30); y = linspace(0,55,30); 238 | [X,Y] = meshgrid(x,y); 239 | Z = tanGlide*sqrt(X.^2+Y.^2); 240 | figure(1); colormap autumn ; s=surf(X,Y,Z);s.FaceAlpha = 0.1; 241 | axis([-40,55,0,55,0,105]); 242 | grid on; hold on; 243 | 244 | % INSERT YOUR VARIABLES HERE: 245 | % ------------------------------------------------------- 246 | 247 | hplotIG=plot3(p_IG(1,:),p_IG(2,:),p_IG(3,:),'k--','linewidth',1); 248 | for kk = 1:NumberProblems 249 | hplot(kk)=plot3(p(1,:,kk),p(2,:,kk),p(3,:,kk),'linestyle',linestyle{kk},'Color',defaultColor(kk,:),'linewidth',1.5); 250 | quiver3(p(1,:,kk),p(2,:,kk),p(3,:,kk),... 251 | f(1,:,kk),f(2,:,kk),f(3,:,kk),0.3,'k','linewidth',1.5); 252 | end 253 | 254 | xlabel('$P_x$ [-]','interpreter','latex') 255 | ylabel('$P_y$ [-]','interpreter','latex') 256 | zlabel('$P_z$ [-]','interpreter','latex') 257 | legend([hplotIG,hplot(1),hplot(2),hplot(3),hplot(4)],{'Initial Guess','Problem 1','Problem 2 (hard)','Problem 3 (hard)','Problem 3 (soft)'},'interpreter','latex','location','east') 258 | 259 | 260 | % upFig(gcf,['verification_spacecraftLandingBoyd_trajectory'],'Guidance/Convex/Verification','.pdf','wide') 261 | 262 | 263 | --------------------------------------------------------------------------------