Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00034 #ifndef ACADO_TOOLKIT_SCP_METHOD_HPP
00035 #define ACADO_TOOLKIT_SCP_METHOD_HPP
00036
00037
00038 #include <acado/utils/acado_utils.hpp>
00039
00040 #include <acado/function/ocp_iterate.hpp>
00041
00042 #include <acado/nlp_solver/nlp_solver.hpp>
00043 #include <acado/conic_solver/dense_qp_solver.hpp>
00044 #include <acado/conic_solver/banded_cp_solver.hpp>
00045 #include <acado/conic_solver/condensing_based_cp_solver.hpp>
00046
00047 #include <acado/nlp_solver/scp_evaluation.hpp>
00048 #include <acado/nlp_solver/scp_step_linesearch.hpp>
00049 #include <acado/nlp_solver/scp_step_fullstep.hpp>
00050 #include <acado/nlp_derivative_approximation/nlp_derivative_approximation.hpp>
00051
00052
00053
00054 BEGIN_NAMESPACE_ACADO
00055
00056
00067 class SCPmethod : public NLPsolver
00068 {
00069
00070
00071
00072
00073 public:
00074
00076 SCPmethod( );
00077
00079 SCPmethod( UserInteraction* _userInteraction,
00080 const Objective *objective_ ,
00081 const DynamicDiscretization *dynamic_discretization_,
00082 const Constraint *constraint_,
00083 BooleanType _isCP = BT_FALSE
00084 );
00085
00087 SCPmethod( const SCPmethod& rhs );
00088
00090 virtual ~SCPmethod( );
00091
00093 SCPmethod& operator=( const SCPmethod& rhs );
00094
00095 virtual NLPsolver* clone() const;
00096
00097
00099 virtual returnValue init( VariablesGrid* x_init ,
00100 VariablesGrid* xa_init,
00101 VariablesGrid* p_init ,
00102 VariablesGrid* u_init ,
00103 VariablesGrid* w_init );
00104
00105
00107 virtual returnValue solve( const DVector &x0_ = emptyConstVector,
00108 const DVector &p_ = emptyConstVector
00109 );
00110
00112 virtual returnValue step( const DVector &x0_ = emptyConstVector,
00113 const DVector &p_ = emptyConstVector
00114 );
00115
00117 virtual returnValue feedbackStep( const DVector &x0_,
00118 const DVector &p_ = emptyConstVector
00119 );
00120
00121 virtual returnValue performCurrentStep( );
00122
00123
00125 virtual returnValue prepareNextStep( );
00126
00127
00136 virtual returnValue setReference( const VariablesGrid &ref
00137 );
00138
00139
00140
00141
00146 virtual returnValue shiftVariables( double timeShift = -1.0,
00147 DVector lastX = emptyVector,
00148 DVector lastXA = emptyVector,
00149 DVector lastP = emptyVector,
00150 DVector lastU = emptyVector,
00151 DVector lastW = emptyVector );
00152
00153
00159 virtual returnValue getVarianceCovariance( DMatrix &var );
00160
00161
00166 virtual returnValue printRuntimeProfile() const;
00167
00168
00169
00170
00171
00172
00173 protected:
00174
00175 virtual returnValue setupLogging( );
00176
00177
00178 returnValue setup( );
00179
00180
00183 returnValue printIterate( ) const;
00184
00185 returnValue printIteration( );
00186
00187 returnValue checkForConvergence( );
00188
00189
00190 returnValue computeHessianMatrix( const BlockMatrix& oldLagrangeGradient,
00191 const BlockMatrix& newLagrangeGradient
00192 );
00193
00194
00195 returnValue initializeHessianProjection( );
00196
00197
00198 returnValue checkForRealTimeMode( const DVector &x0_,
00199 const DVector &p_
00200 );
00201
00202 returnValue setupRealTimeParameters( const DVector &x0_ = emptyConstVector,
00203 const DVector &p_ = emptyConstVector
00204 );
00205
00206
00207 returnValue stopClockAndPrintRuntimeProfile( );
00208
00209
00210 virtual returnValue getDifferentialStates( VariablesGrid &xd_ ) const;
00211 virtual returnValue getAlgebraicStates ( VariablesGrid &xa_ ) const;
00212 virtual returnValue getParameters ( VariablesGrid &p_ ) const;
00213 virtual returnValue getParameters ( DVector &p_ ) const;
00214 virtual returnValue getControls ( VariablesGrid &u_ ) const;
00215 virtual returnValue getFirstControl ( DVector &u0_ ) const;
00216 virtual returnValue getDisturbances ( VariablesGrid &w_ ) const;
00217 virtual double getObjectiveValue ( ) const;
00218
00219
00220 virtual returnValue getSensitivitiesX( BlockMatrix& _sens
00221 ) const;
00222
00223 virtual returnValue getSensitivitiesXA( BlockMatrix& _sens
00224 ) const;
00225
00226 virtual returnValue getSensitivitiesP( BlockMatrix& _sens
00227 ) const;
00228
00229 virtual returnValue getSensitivitiesU( BlockMatrix& _sens
00230 ) const;
00231
00232 virtual returnValue getSensitivitiesW( BlockMatrix& _sens
00233 ) const;
00234
00235 virtual returnValue getAnySensitivities( BlockMatrix& _sens,
00236 uint idx
00237 ) const;
00238
00239
00240 inline uint getNumPoints( ) const;
00241
00242 inline uint getNX( ) const;
00243 inline uint getNXA( ) const;
00244 inline uint getNP( ) const;
00245 inline uint getNU( ) const;
00246 inline uint getNW( ) const;
00247
00248 inline uint getNC( ) const;
00249
00250
00251
00252
00253
00254 protected:
00255
00256 int timeLoggingIdx;
00257 RealClock clock;
00258 RealClock clockTotalTime;
00259
00260 OCPiterate iter;
00261 OCPiterate oldIter;
00262
00263 SCPevaluation* eval;
00264 SCPstep* scpStep;
00265 NLPderivativeApproximation* derivativeApproximation;
00266
00267 BandedCP bandedCP;
00268 BandedCPsolver* bandedCPsolver;
00269
00270 BlockStatus status;
00271 BooleanType isCP;
00272
00273 BooleanType hasPerformedStep;
00274 BooleanType isInRealTimeMode;
00275 BooleanType needToReevaluate;
00276 };
00277
00278
00279 CLOSE_NAMESPACE_ACADO
00280
00281
00282
00283 #include <acado/nlp_solver/scp_method.ipp>
00284
00285
00286 #endif // ACADO_TOOLKIT_SCP_METHOD_HPP
00287
00288
00289
00290