Added logic to fill in new cereal fields
mitchellgoffpc
parent
e797439c5f
commit
ba57a8d1ce
2
cereal
2
cereal
|
|
@ -1 +1 @@
|
|||
Subproject commit bbcc8eea73fdf3d0cbb0cc925d47379dd33ba0b8
|
||||
Subproject commit cdbc15c97214a75f80493279416d3b24e18e0a45
|
||||
|
|
@ -0,0 +1,189 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/Bounds.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of the Bounds class designed to manage working sets of
|
||||
* bounds within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_BOUNDS_HPP
|
||||
#define QPOASES_BOUNDS_HPP
|
||||
|
||||
|
||||
#include <SubjectTo.hpp>
|
||||
|
||||
|
||||
|
||||
/** This class manages working sets of bounds by storing
|
||||
* index sets and other status information.
|
||||
*
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*/
|
||||
class Bounds : public SubjectTo
|
||||
{
|
||||
/*
|
||||
* PUBLIC MEMBER FUNCTIONS
|
||||
*/
|
||||
public:
|
||||
/** Default constructor. */
|
||||
Bounds( );
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
Bounds( const Bounds& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~Bounds( );
|
||||
|
||||
/** Assignment operator (deep copy). */
|
||||
Bounds& operator=( const Bounds& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
|
||||
/** Pseudo-constructor takes the number of bounds.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue init( int n /**< Number of bounds. */
|
||||
);
|
||||
|
||||
|
||||
/** Initially adds number of a new (i.e. not yet in the list) bound to
|
||||
* given index set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SETUP_BOUND_FAILED \n
|
||||
RET_INDEX_OUT_OF_BOUNDS \n
|
||||
RET_INVALID_ARGUMENTS */
|
||||
returnValue setupBound( int _number, /**< Number of new bound. */
|
||||
SubjectToStatus _status /**< Status of new bound. */
|
||||
);
|
||||
|
||||
/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
|
||||
* to the index set of free bounds; the order depends on the SujectToType
|
||||
* of each index.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SETUP_BOUND_FAILED */
|
||||
returnValue setupAllFree( );
|
||||
|
||||
|
||||
/** Moves index of a bound from index list of fixed to that of free bounds.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_MOVING_BOUND_FAILED \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
returnValue moveFixedToFree( int _number /**< Number of bound to be freed. */
|
||||
);
|
||||
|
||||
/** Moves index of a bound from index list of free to that of fixed bounds.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_MOVING_BOUND_FAILED \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
returnValue moveFreeToFixed( int _number, /**< Number of bound to be fixed. */
|
||||
SubjectToStatus _status /**< Status of bound to be fixed. */
|
||||
);
|
||||
|
||||
/** Swaps the indices of two free bounds within the index set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SWAPINDEX_FAILED */
|
||||
returnValue swapFree( int number1, /**< Number of first constraint or bound. */
|
||||
int number2 /**< Number of second constraint or bound. */
|
||||
);
|
||||
|
||||
|
||||
/** Returns number of variables.
|
||||
* \return Number of variables. */
|
||||
inline int getNV( ) const;
|
||||
|
||||
/** Returns number of implicitly fixed variables.
|
||||
* \return Number of implicitly fixed variables. */
|
||||
inline int getNFV( ) const;
|
||||
|
||||
/** Returns number of bounded (but possibly free) variables.
|
||||
* \return Number of bounded (but possibly free) variables. */
|
||||
inline int getNBV( ) const;
|
||||
|
||||
/** Returns number of unbounded variables.
|
||||
* \return Number of unbounded variables. */
|
||||
inline int getNUV( ) const;
|
||||
|
||||
|
||||
/** Sets number of implicitly fixed variables.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setNFV( int n /**< Number of implicitly fixed variables. */
|
||||
);
|
||||
|
||||
/** Sets number of bounded (but possibly free) variables.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setNBV( int n /**< Number of bounded (but possibly free) variables. */
|
||||
);
|
||||
|
||||
/** Sets number of unbounded variables.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setNUV( int n /**< Number of unbounded variables */
|
||||
);
|
||||
|
||||
|
||||
/** Returns number of free variables.
|
||||
* \return Number of free variables. */
|
||||
inline int getNFR( );
|
||||
|
||||
/** Returns number of fixed variables.
|
||||
* \return Number of fixed variables. */
|
||||
inline int getNFX( );
|
||||
|
||||
|
||||
/** Returns a pointer to free variables index list.
|
||||
* \return Pointer to free variables index list. */
|
||||
inline Indexlist* getFree( );
|
||||
|
||||
/** Returns a pointer to fixed variables index list.
|
||||
* \return Pointer to fixed variables index list. */
|
||||
inline Indexlist* getFixed( );
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER VARIABLES
|
||||
*/
|
||||
protected:
|
||||
int nV; /**< Number of variables (nV = nFV + nBV + nUV). */
|
||||
int nFV; /**< Number of implicitly fixed variables. */
|
||||
int nBV; /**< Number of bounded (but possibly free) variables. */
|
||||
int nUV; /**< Number of unbounded variables. */
|
||||
|
||||
Indexlist free; /**< Index list of free variables. */
|
||||
Indexlist fixed; /**< Index list of fixed variables. */
|
||||
};
|
||||
|
||||
#include <Bounds.ipp>
|
||||
|
||||
#endif /* QPOASES_BOUNDS_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,108 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/Constants.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2008
|
||||
*
|
||||
* Definition of all global constants.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_CONSTANTS_HPP
|
||||
#define QPOASES_CONSTANTS_HPP
|
||||
|
||||
#ifndef QPOASES_CUSTOM_INTERFACE
|
||||
#include "acado_qpoases_interface.hpp"
|
||||
#else
|
||||
#define XSTR(x) #x
|
||||
#define STR(x) XSTR(x)
|
||||
#include STR(QPOASES_CUSTOM_INTERFACE)
|
||||
#endif
|
||||
|
||||
/** Maximum number of variables within a QP formulation.
|
||||
Note: this value has to be positive! */
|
||||
const int NVMAX = QPOASES_NVMAX;
|
||||
|
||||
/** Maximum number of constraints within a QP formulation.
|
||||
Note: this value has to be positive! */
|
||||
const int NCMAX = QPOASES_NCMAX;
|
||||
|
||||
/** Redefinition of NCMAX used for memory allocation, to avoid zero sized arrays
|
||||
and compiler errors. */
|
||||
const int NCMAX_ALLOC = (NCMAX == 0) ? 1 : NCMAX;
|
||||
|
||||
/**< Maximum number of working set recalculations.
|
||||
Note: this value has to be positive! */
|
||||
const int NWSRMAX = QPOASES_NWSRMAX;
|
||||
|
||||
/** Desired KKT tolerance of QP solution; a warning RET_INACCURATE_SOLUTION is
|
||||
* issued if this tolerance is not met.
|
||||
* Note: this value has to be positive! */
|
||||
const real_t DESIREDACCURACY = (real_t) 1.0e-3;
|
||||
|
||||
/** Critical KKT tolerance of QP solution; an error is issued if this
|
||||
* tolerance is not met.
|
||||
* Note: this value has to be positive! */
|
||||
const real_t CRITICALACCURACY = (real_t) 1.0e-2;
|
||||
|
||||
|
||||
|
||||
/** Numerical value of machine precision (min eps, s.t. 1+eps > 1).
|
||||
Note: this value has to be positive! */
|
||||
const real_t EPS = (real_t) QPOASES_EPS;
|
||||
|
||||
/** Numerical value of zero (for situations in which it would be
|
||||
* unreasonable to compare with 0.0).
|
||||
* Note: this value has to be positive! */
|
||||
const real_t ZERO = (real_t) 1.0e-50;
|
||||
|
||||
/** Numerical value of infinity (e.g. for non-existing bounds).
|
||||
* Note: this value has to be positive! */
|
||||
const real_t INFTY = (real_t) 1.0e12;
|
||||
|
||||
|
||||
/** Lower/upper (constraints') bound tolerance (an inequality constraint
|
||||
* whose lower and upper bound differ by less than BOUNDTOL is regarded
|
||||
* to be an equality constraint).
|
||||
* Note: this value has to be positive! */
|
||||
const real_t BOUNDTOL = (real_t) 1.0e-10;
|
||||
|
||||
/** Offset for relaxing (constraints') bounds at beginning of an initial homotopy.
|
||||
* Note: this value has to be positive! */
|
||||
const real_t BOUNDRELAXATION = (real_t) 1.0e3;
|
||||
|
||||
|
||||
/** Factor that determines physical lengths of index lists.
|
||||
* Note: this value has to be greater than 1! */
|
||||
const int INDEXLISTFACTOR = 5;
|
||||
|
||||
|
||||
#endif /* QPOASES_CONSTANTS_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,181 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/Constraints.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of the Constraints class designed to manage working sets of
|
||||
* constraints within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_CONSTRAINTS_HPP
|
||||
#define QPOASES_CONSTRAINTS_HPP
|
||||
|
||||
|
||||
#include <SubjectTo.hpp>
|
||||
|
||||
|
||||
|
||||
/** This class manages working sets of constraints by storing
|
||||
* index sets and other status information.
|
||||
*
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*/
|
||||
class Constraints : public SubjectTo
|
||||
{
|
||||
/*
|
||||
* PUBLIC MEMBER FUNCTIONS
|
||||
*/
|
||||
public:
|
||||
/** Default constructor. */
|
||||
Constraints( );
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
Constraints( const Constraints& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~Constraints( );
|
||||
|
||||
/** Assignment operator (deep copy). */
|
||||
Constraints& operator=( const Constraints& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
|
||||
/** Pseudo-constructor takes the number of constraints.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue init( int n /**< Number of constraints. */
|
||||
);
|
||||
|
||||
|
||||
/** Initially adds number of a new (i.e. not yet in the list) constraint to
|
||||
* a given index set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SETUP_CONSTRAINT_FAILED \n
|
||||
RET_INDEX_OUT_OF_BOUNDS \n
|
||||
RET_INVALID_ARGUMENTS */
|
||||
returnValue setupConstraint( int _number, /**< Number of new constraint. */
|
||||
SubjectToStatus _status /**< Status of new constraint. */
|
||||
);
|
||||
|
||||
/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to
|
||||
* to the index set of inactive constraints; the order depends on the SujectToType
|
||||
* of each index. Only disabled constraints are added to index set of disabled constraints!
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SETUP_CONSTRAINT_FAILED */
|
||||
returnValue setupAllInactive( );
|
||||
|
||||
|
||||
/** Moves index of a constraint from index list of active to that of inactive constraints.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_MOVING_CONSTRAINT_FAILED */
|
||||
returnValue moveActiveToInactive( int _number /**< Number of constraint to become inactive. */
|
||||
);
|
||||
|
||||
/** Moves index of a constraint from index list of inactive to that of active constraints.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_MOVING_CONSTRAINT_FAILED */
|
||||
returnValue moveInactiveToActive( int _number, /**< Number of constraint to become active. */
|
||||
SubjectToStatus _status /**< Status of constraint to become active. */
|
||||
);
|
||||
|
||||
|
||||
/** Returns the number of constraints.
|
||||
* \return Number of constraints. */
|
||||
inline int getNC( ) const;
|
||||
|
||||
/** Returns the number of implicit equality constraints.
|
||||
* \return Number of implicit equality constraints. */
|
||||
inline int getNEC( ) const;
|
||||
|
||||
/** Returns the number of "real" inequality constraints.
|
||||
* \return Number of "real" inequality constraints. */
|
||||
inline int getNIC( ) const;
|
||||
|
||||
/** Returns the number of unbounded constraints (i.e. without any bounds).
|
||||
* \return Number of unbounded constraints (i.e. without any bounds). */
|
||||
inline int getNUC( ) const;
|
||||
|
||||
|
||||
/** Sets number of implicit equality constraints.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setNEC( int n /**< Number of implicit equality constraints. */
|
||||
);
|
||||
|
||||
/** Sets number of "real" inequality constraints.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setNIC( int n /**< Number of "real" inequality constraints. */
|
||||
);
|
||||
|
||||
/** Sets number of unbounded constraints (i.e. without any bounds).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setNUC( int n /**< Number of unbounded constraints (i.e. without any bounds). */
|
||||
);
|
||||
|
||||
|
||||
/** Returns the number of active constraints.
|
||||
* \return Number of constraints. */
|
||||
inline int getNAC( );
|
||||
|
||||
/** Returns the number of inactive constraints.
|
||||
* \return Number of constraints. */
|
||||
inline int getNIAC( );
|
||||
|
||||
|
||||
/** Returns a pointer to active constraints index list.
|
||||
* \return Pointer to active constraints index list. */
|
||||
inline Indexlist* getActive( );
|
||||
|
||||
/** Returns a pointer to inactive constraints index list.
|
||||
* \return Pointer to inactive constraints index list. */
|
||||
inline Indexlist* getInactive( );
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER VARIABLES
|
||||
*/
|
||||
protected:
|
||||
int nC; /**< Number of constraints (nC = nEC + nIC + nUC). */
|
||||
int nEC; /**< Number of implicit equality constraints. */
|
||||
int nIC; /**< Number of "real" inequality constraints. */
|
||||
int nUC; /**< Number of unbounded constraints (i.e. without any bounds). */
|
||||
|
||||
Indexlist active; /**< Index list of active constraints. */
|
||||
Indexlist inactive; /**< Index list of inactive constraints. */
|
||||
};
|
||||
|
||||
|
||||
#include <Constraints.ipp>
|
||||
|
||||
#endif /* QPOASES_CONSTRAINTS_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,126 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/CyclingManager.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of the CyclingManager class designed to detect
|
||||
* and handle possible cycling during QP iterations.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_CYCLINGMANAGER_HPP
|
||||
#define QPOASES_CYCLINGMANAGER_HPP
|
||||
|
||||
|
||||
#include <Utils.hpp>
|
||||
|
||||
|
||||
|
||||
/** This class is intended to detect and handle possible cycling during QP iterations.
|
||||
* As cycling seems to occur quite rarely, this class is NOT FULLY IMPLEMENTED YET!
|
||||
*
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*/
|
||||
class CyclingManager
|
||||
{
|
||||
/*
|
||||
* PUBLIC MEMBER FUNCTIONS
|
||||
*/
|
||||
public:
|
||||
/** Default constructor. */
|
||||
CyclingManager( );
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
CyclingManager( const CyclingManager& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~CyclingManager( );
|
||||
|
||||
/** Copy asingment operator (deep copy). */
|
||||
CyclingManager& operator=( const CyclingManager& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
|
||||
/** Pseudo-constructor which takes the number of bounds/constraints.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue init( int _nV, /**< Number of bounds to be managed. */
|
||||
int _nC /**< Number of constraints to be managed. */
|
||||
);
|
||||
|
||||
|
||||
/** Stores index of a bound/constraint that might cause cycling.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
returnValue setCyclingStatus( int number, /**< Number of bound/constraint. */
|
||||
BooleanType isBound, /**< Flag that indicates if given number corresponds to a
|
||||
* bound (BT_TRUE) or a constraint (BT_FALSE). */
|
||||
CyclingStatus _status /**< Cycling status of bound/constraint. */
|
||||
);
|
||||
|
||||
/** Returns if bound/constraint might cause cycling.
|
||||
* \return BT_TRUE: bound/constraint might cause cycling \n
|
||||
BT_FALSE: otherwise */
|
||||
CyclingStatus getCyclingStatus( int number, /**< Number of bound/constraint. */
|
||||
BooleanType isBound /**< Flag that indicates if given number corresponds to
|
||||
* a bound (BT_TRUE) or a constraint (BT_FALSE). */
|
||||
) const;
|
||||
|
||||
|
||||
/** Clears all previous cycling information.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue clearCyclingData( );
|
||||
|
||||
|
||||
/** Returns if cycling was detected.
|
||||
* \return BT_TRUE iff cycling was detected. */
|
||||
inline BooleanType isCyclingDetected( ) const;
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER VARIABLES
|
||||
*/
|
||||
protected:
|
||||
int nV; /**< Number of managed bounds. */
|
||||
int nC; /**< Number of managed constraints. */
|
||||
|
||||
CyclingStatus status[NVMAX+NCMAX]; /**< Array to store cycling status of all bounds/constraints. */
|
||||
|
||||
BooleanType cyclingDetected; /**< Flag if cycling was detected. */
|
||||
};
|
||||
|
||||
|
||||
#include <CyclingManager.ipp>
|
||||
|
||||
#endif /* QPOASES_CYCLINGMANAGER_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,107 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/EXTRAS/SolutionAnalysis.hpp
|
||||
* \author Milan Vukov, Boris Houska, Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2012
|
||||
*
|
||||
* Solution analysis class, based on a class in the standard version of the qpOASES
|
||||
*/
|
||||
|
||||
|
||||
//
|
||||
|
||||
#ifndef QPOASES_SOLUTIONANALYSIS_HPP
|
||||
#define QPOASES_SOLUTIONANALYSIS_HPP
|
||||
|
||||
#include <QProblem.hpp>
|
||||
|
||||
/** Enables the computation of variance as is in the standard version of qpOASES */
|
||||
#define QPOASES_USE_OLD_VERSION 0
|
||||
|
||||
#if QPOASES_USE_OLD_VERSION
|
||||
#define KKT_DIM (2 * NVMAX + NCMAX)
|
||||
#endif
|
||||
|
||||
class SolutionAnalysis
|
||||
{
|
||||
public:
|
||||
|
||||
/** Default constructor. */
|
||||
SolutionAnalysis( );
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
SolutionAnalysis( const SolutionAnalysis& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~SolutionAnalysis( );
|
||||
|
||||
/** Copy asingment operator (deep copy). */
|
||||
SolutionAnalysis& operator=( const SolutionAnalysis& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** A routine for computation of inverse of the Hessian matrix. */
|
||||
returnValue getHessianInverse(
|
||||
QProblem* qp, /** QP */
|
||||
real_t* hessianInverse /** Inverse of the Hessian matrix*/
|
||||
);
|
||||
|
||||
/** A routine for computation of inverse of the Hessian matrix. */
|
||||
returnValue getHessianInverse( QProblemB* qp, /** QP */
|
||||
real_t* hessianInverse /** Inverse of the Hessian matrix*/
|
||||
);
|
||||
|
||||
#if QPOASES_USE_OLD_VERSION
|
||||
returnValue getVarianceCovariance(
|
||||
QProblem* qp,
|
||||
real_t* g_b_bA_VAR,
|
||||
real_t* Primal_Dual_VAR
|
||||
);
|
||||
#endif
|
||||
|
||||
private:
|
||||
|
||||
real_t delta_g_cov[ NVMAX ]; /** A covariance-vector of g */
|
||||
real_t delta_lb_cov[ NVMAX ]; /** A covariance-vector of lb */
|
||||
real_t delta_ub_cov[ NVMAX ]; /** A covariance-vector of ub */
|
||||
real_t delta_lbA_cov[ NCMAX_ALLOC ]; /** A covariance-vector of lbA */
|
||||
real_t delta_ubA_cov[ NCMAX_ALLOC ]; /** A covariance-vector of ubA */
|
||||
|
||||
#if QPOASES_USE_OLD_VERSION
|
||||
real_t K[KKT_DIM * KKT_DIM]; /** A matrix to store an intermediate result */
|
||||
#endif
|
||||
|
||||
int FR_idx[ NVMAX ]; /** Index array for free variables */
|
||||
int FX_idx[ NVMAX ]; /** Index array for fixed variables */
|
||||
int AC_idx[ NCMAX_ALLOC ]; /** Index array for active constraints */
|
||||
|
||||
real_t delta_xFR[ NVMAX ]; /** QP reaction, primal, w.r.t. free */
|
||||
real_t delta_xFX[ NVMAX ]; /** QP reaction, primal, w.r.t. fixed */
|
||||
real_t delta_yAC[ NVMAX ]; /** QP reaction, dual, w.r.t. active */
|
||||
real_t delta_yFX[ NVMAX ]; /** QP reaction, dual, w.r.t. fixed*/
|
||||
};
|
||||
|
||||
#endif // QPOASES_SOLUTIONANALYSIS_HPP
|
||||
|
|
@ -0,0 +1,154 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/Indexlist.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of the Indexlist class designed to manage index lists of
|
||||
* constraints and bounds within a SubjectTo object.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_INDEXLIST_HPP
|
||||
#define QPOASES_INDEXLIST_HPP
|
||||
|
||||
|
||||
#include <Utils.hpp>
|
||||
|
||||
|
||||
/** This class manages index lists.
|
||||
*
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*/
|
||||
class Indexlist
|
||||
{
|
||||
/*
|
||||
* PUBLIC MEMBER FUNCTIONS
|
||||
*/
|
||||
public:
|
||||
/** Default constructor. */
|
||||
Indexlist( );
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
Indexlist( const Indexlist& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~Indexlist( );
|
||||
|
||||
/** Assingment operator (deep copy). */
|
||||
Indexlist& operator=( const Indexlist& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Pseudo-constructor.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue init( );
|
||||
|
||||
|
||||
/** Creates an array of all numbers within the index set in correct order.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEXLIST_CORRUPTED */
|
||||
returnValue getNumberArray( int* const numberarray /**< Output: Array of numbers (NULL on error). */
|
||||
) const;
|
||||
|
||||
|
||||
/** Determines the index within the index list at with a given number is stored.
|
||||
* \return >= 0: Index of given number. \n
|
||||
-1: Number not found. */
|
||||
int getIndex( int givennumber /**< Number whose index shall be determined. */
|
||||
) const;
|
||||
|
||||
/** Determines the physical index within the index list at with a given number is stored.
|
||||
* \return >= 0: Index of given number. \n
|
||||
-1: Number not found. */
|
||||
int getPhysicalIndex( int givennumber /**< Number whose physical index shall be determined. */
|
||||
) const;
|
||||
|
||||
/** Returns the number stored at a given physical index.
|
||||
* \return >= 0: Number stored at given physical index. \n
|
||||
-RET_INDEXLIST_OUTOFBOUNDS */
|
||||
int getNumber( int physicalindex /**< Physical index of the number to be returned. */
|
||||
) const;
|
||||
|
||||
|
||||
/** Returns the current length of the index list.
|
||||
* \return Current length of the index list. */
|
||||
inline int getLength( );
|
||||
|
||||
/** Returns last number within the index list.
|
||||
* \return Last number within the index list. */
|
||||
inline int getLastNumber( ) const;
|
||||
|
||||
|
||||
/** Adds number to index list.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEXLIST_MUST_BE_REORDERD \n
|
||||
RET_INDEXLIST_EXCEEDS_MAX_LENGTH */
|
||||
returnValue addNumber( int addnumber /**< Number to be added. */
|
||||
);
|
||||
|
||||
/** Removes number from index list.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue removeNumber( int removenumber /**< Number to be removed. */
|
||||
);
|
||||
|
||||
/** Swaps two numbers within index list.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue swapNumbers( int number1,/**< First number for swapping. */
|
||||
int number2 /**< Second number for swapping. */
|
||||
);
|
||||
|
||||
/** Determines if a given number is contained in the index set.
|
||||
* \return BT_TRUE iff number is contain in the index set */
|
||||
inline BooleanType isMember( int _number /**< Number to be tested for membership. */
|
||||
) const;
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER VARIABLES
|
||||
*/
|
||||
protected:
|
||||
int number[INDEXLISTFACTOR*(NVMAX+NCMAX)]; /**< Array to store numbers of constraints or bounds. */
|
||||
int next[INDEXLISTFACTOR*(NVMAX+NCMAX)]; /**< Array to store physical index of successor. */
|
||||
int previous[INDEXLISTFACTOR*(NVMAX+NCMAX)]; /**< Array to store physical index of predecossor. */
|
||||
int length; /**< Length of index list. */
|
||||
int first; /**< Physical index of first element. */
|
||||
int last; /**< Physical index of last element. */
|
||||
int lastusedindex; /**< Physical index of last entry in index list. */
|
||||
int physicallength; /**< Physical length of index list. */
|
||||
};
|
||||
|
||||
|
||||
#include <Indexlist.ipp>
|
||||
|
||||
#endif /* QPOASES_INDEXLIST_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,415 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/MessageHandling.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of the MessageHandling class including global return values.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_MESSAGEHANDLING_HPP
|
||||
#define QPOASES_MESSAGEHANDLING_HPP
|
||||
|
||||
// #define PC_DEBUG
|
||||
|
||||
#ifdef PC_DEBUG
|
||||
#include <stdio.h>
|
||||
|
||||
/** Defines an alias for FILE from stdio.h. */
|
||||
#define myFILE FILE
|
||||
/** Defines an alias for stderr from stdio.h. */
|
||||
#define myStderr stderr
|
||||
/** Defines an alias for stdout from stdio.h. */
|
||||
#define myStdout stdout
|
||||
#else
|
||||
/** Defines an alias for FILE from stdio.h. */
|
||||
#define myFILE int
|
||||
/** Defines an alias for stderr from stdio.h. */
|
||||
#define myStderr 0
|
||||
/** Defines an alias for stdout from stdio.h. */
|
||||
#define myStdout 0
|
||||
#endif
|
||||
|
||||
|
||||
#include <Types.hpp>
|
||||
#include <Constants.hpp>
|
||||
|
||||
|
||||
/** Defines symbols for global return values. \n
|
||||
* Important: All return values are assumed to be nonnegative! */
|
||||
enum returnValue
|
||||
{
|
||||
TERMINAL_LIST_ELEMENT = -1, /**< Terminal list element, internal usage only! */
|
||||
/* miscellaneous */
|
||||
SUCCESSFUL_RETURN = 0, /**< Successful return. */
|
||||
RET_DIV_BY_ZERO, /**< Division by zero. */
|
||||
RET_INDEX_OUT_OF_BOUNDS, /**< Index out of bounds. */
|
||||
RET_INVALID_ARGUMENTS, /**< At least one of the arguments is invalid. */
|
||||
RET_ERROR_UNDEFINED, /**< Error number undefined. */
|
||||
RET_WARNING_UNDEFINED, /**< Warning number undefined. */
|
||||
RET_INFO_UNDEFINED, /**< Info number undefined. */
|
||||
RET_EWI_UNDEFINED, /**< Error/warning/info number undefined. */
|
||||
RET_AVAILABLE_WITH_LINUX_ONLY, /**< This function is available under Linux only. */
|
||||
RET_UNKNOWN_BUG, /**< The error occured is not yet known. */
|
||||
RET_PRINTLEVEL_CHANGED, /**< 10 Print level changed. */
|
||||
RET_NOT_YET_IMPLEMENTED, /**< Requested function is not yet implemented in this version of qpOASES. */
|
||||
/* Indexlist */
|
||||
RET_INDEXLIST_MUST_BE_REORDERD, /**< Index list has to be reordered. */
|
||||
RET_INDEXLIST_EXCEEDS_MAX_LENGTH, /**< Index list exceeds its maximal physical length. */
|
||||
RET_INDEXLIST_CORRUPTED, /**< Index list corrupted. */
|
||||
RET_INDEXLIST_OUTOFBOUNDS, /**< Physical index is out of bounds. */
|
||||
RET_INDEXLIST_ADD_FAILED, /**< Adding indices from another index set failed. */
|
||||
RET_INDEXLIST_INTERSECT_FAILED, /**< Intersection with another index set failed. */
|
||||
/* SubjectTo / Bounds / Constraints */
|
||||
RET_INDEX_ALREADY_OF_DESIRED_STATUS, /**< Index is already of desired status. */
|
||||
RET_ADDINDEX_FAILED, /**< Cannot swap between different indexsets. */
|
||||
RET_SWAPINDEX_FAILED, /**< 20 Adding index to index set failed. */
|
||||
RET_NOTHING_TO_DO, /**< Nothing to do. */
|
||||
RET_SETUP_BOUND_FAILED, /**< Setting up bound index failed. */
|
||||
RET_SETUP_CONSTRAINT_FAILED, /**< Setting up constraint index failed. */
|
||||
RET_MOVING_BOUND_FAILED, /**< Moving bound between index sets failed. */
|
||||
RET_MOVING_CONSTRAINT_FAILED, /**< Moving constraint between index sets failed. */
|
||||
/* QProblem */
|
||||
RET_QP_ALREADY_INITIALISED, /**< QProblem has already been initialised. */
|
||||
RET_NO_INIT_WITH_STANDARD_SOLVER, /**< Initialisation via extern QP solver is not yet implemented. */
|
||||
RET_RESET_FAILED, /**< Reset failed. */
|
||||
RET_INIT_FAILED, /**< Initialisation failed. */
|
||||
RET_INIT_FAILED_TQ, /**< 30 Initialisation failed due to TQ factorisation. */
|
||||
RET_INIT_FAILED_CHOLESKY, /**< Initialisation failed due to Cholesky decomposition. */
|
||||
RET_INIT_FAILED_HOTSTART, /**< Initialisation failed! QP could not be solved! */
|
||||
RET_INIT_FAILED_INFEASIBILITY, /**< Initial QP could not be solved due to infeasibility! */
|
||||
RET_INIT_FAILED_UNBOUNDEDNESS, /**< Initial QP could not be solved due to unboundedness! */
|
||||
RET_INIT_SUCCESSFUL, /**< Initialisation done. */
|
||||
RET_OBTAINING_WORKINGSET_FAILED, /**< Failed to obtain working set for auxiliary QP. */
|
||||
RET_SETUP_WORKINGSET_FAILED, /**< Failed to setup working set for auxiliary QP. */
|
||||
RET_SETUP_AUXILIARYQP_FAILED, /**< Failed to setup auxiliary QP for initialised homotopy. */
|
||||
RET_NO_EXTERN_SOLVER, /**< No extern QP solver available. */
|
||||
RET_QP_UNBOUNDED, /**< 40 QP is unbounded. */
|
||||
RET_QP_INFEASIBLE, /**< QP is infeasible. */
|
||||
RET_QP_NOT_SOLVED, /**< Problems occured while solving QP with standard solver. */
|
||||
RET_QP_SOLVED, /**< QP successfully solved. */
|
||||
RET_UNABLE_TO_SOLVE_QP, /**< Problems occured while solving QP. */
|
||||
RET_INITIALISATION_STARTED, /**< Starting problem initialisation. */
|
||||
RET_HOTSTART_FAILED, /**< Unable to perform homotopy due to internal error. */
|
||||
RET_HOTSTART_FAILED_TO_INIT, /**< Unable to initialise problem. */
|
||||
RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED, /**< Unable to perform homotopy as previous QP is not solved. */
|
||||
RET_ITERATION_STARTED, /**< Iteration... */
|
||||
RET_SHIFT_DETERMINATION_FAILED, /**< 50 Determination of shift of the QP data failed. */
|
||||
RET_STEPDIRECTION_DETERMINATION_FAILED, /**< Determination of step direction failed. */
|
||||
RET_STEPLENGTH_DETERMINATION_FAILED, /**< Determination of step direction failed. */
|
||||
RET_OPTIMAL_SOLUTION_FOUND, /**< Optimal solution of neighbouring QP found. */
|
||||
RET_HOMOTOPY_STEP_FAILED, /**< Unable to perform homotopy step. */
|
||||
RET_HOTSTART_STOPPED_INFEASIBILITY, /**< Premature homotopy termination because QP is infeasible. */
|
||||
RET_HOTSTART_STOPPED_UNBOUNDEDNESS, /**< Premature homotopy termination because QP is unbounded. */
|
||||
RET_WORKINGSET_UPDATE_FAILED, /**< Unable to update working sets according to initial guesses. */
|
||||
RET_MAX_NWSR_REACHED, /**< Maximum number of working set recalculations performed. */
|
||||
RET_CONSTRAINTS_NOT_SPECIFIED, /**< Problem does comprise constraints! You also have to specify new constraints' bounds. */
|
||||
RET_INVALID_FACTORISATION_FLAG, /**< 60 Invalid factorisation flag. */
|
||||
RET_UNABLE_TO_SAVE_QPDATA, /**< Unable to save QP data. */
|
||||
RET_STEPDIRECTION_FAILED_TQ, /**< Abnormal termination due to TQ factorisation. */
|
||||
RET_STEPDIRECTION_FAILED_CHOLESKY, /**< Abnormal termination due to Cholesky factorisation. */
|
||||
RET_CYCLING_DETECTED, /**< Cycling detected. */
|
||||
RET_CYCLING_NOT_RESOLVED, /**< Cycling cannot be resolved, QP probably infeasible. */
|
||||
RET_CYCLING_RESOLVED, /**< Cycling probably resolved. */
|
||||
RET_STEPSIZE, /**< For displaying performed stepsize. */
|
||||
RET_STEPSIZE_NONPOSITIVE, /**< For displaying non-positive stepsize. */
|
||||
RET_SETUPSUBJECTTOTYPE_FAILED, /**< Setup of SubjectToTypes failed. */
|
||||
RET_ADDCONSTRAINT_FAILED, /**< 70 Addition of constraint to working set failed. */
|
||||
RET_ADDCONSTRAINT_FAILED_INFEASIBILITY, /**< Addition of constraint to working set failed (due to QP infeasibility). */
|
||||
RET_ADDBOUND_FAILED, /**< Addition of bound to working set failed. */
|
||||
RET_ADDBOUND_FAILED_INFEASIBILITY, /**< Addition of bound to working set failed (due to QP infeasibility). */
|
||||
RET_REMOVECONSTRAINT_FAILED, /**< Removal of constraint from working set failed. */
|
||||
RET_REMOVEBOUND_FAILED, /**< Removal of bound from working set failed. */
|
||||
RET_REMOVE_FROM_ACTIVESET, /**< Removing from active set... */
|
||||
RET_ADD_TO_ACTIVESET, /**< Adding to active set... */
|
||||
RET_REMOVE_FROM_ACTIVESET_FAILED, /**< Removing from active set failed. */
|
||||
RET_ADD_TO_ACTIVESET_FAILED, /**< Adding to active set failed. */
|
||||
RET_CONSTRAINT_ALREADY_ACTIVE, /**< 80 Constraint is already active. */
|
||||
RET_ALL_CONSTRAINTS_ACTIVE, /**< All constraints are active, no further constraint can be added. */
|
||||
RET_LINEARLY_DEPENDENT, /**< New bound/constraint is linearly dependent. */
|
||||
RET_LINEARLY_INDEPENDENT, /**< New bound/constraint is linearly independent. */
|
||||
RET_LI_RESOLVED, /**< Linear independence of active contraint matrix successfully resolved. */
|
||||
RET_ENSURELI_FAILED, /**< Failed to ensure linear indepence of active contraint matrix. */
|
||||
RET_ENSURELI_FAILED_TQ, /**< Abnormal termination due to TQ factorisation. */
|
||||
RET_ENSURELI_FAILED_NOINDEX, /**< No index found, QP probably infeasible. */
|
||||
RET_ENSURELI_FAILED_CYCLING, /**< Cycling detected, QP probably infeasible. */
|
||||
RET_BOUND_ALREADY_ACTIVE, /**< Bound is already active. */
|
||||
RET_ALL_BOUNDS_ACTIVE, /**< 90 All bounds are active, no further bound can be added. */
|
||||
RET_CONSTRAINT_NOT_ACTIVE, /**< Constraint is not active. */
|
||||
RET_BOUND_NOT_ACTIVE, /**< Bound is not active. */
|
||||
RET_HESSIAN_NOT_SPD, /**< Projected Hessian matrix not positive definite. */
|
||||
RET_MATRIX_SHIFT_FAILED, /**< Unable to update matrices or to transform vectors. */
|
||||
RET_MATRIX_FACTORISATION_FAILED, /**< Unable to calculate new matrix factorisations. */
|
||||
RET_PRINT_ITERATION_FAILED, /**< Unable to print information on current iteration. */
|
||||
RET_NO_GLOBAL_MESSAGE_OUTPUTFILE, /**< No global message output file initialised. */
|
||||
/* Utils */
|
||||
RET_UNABLE_TO_OPEN_FILE, /**< Unable to open file. */
|
||||
RET_UNABLE_TO_WRITE_FILE, /**< Unable to write into file. */
|
||||
RET_UNABLE_TO_READ_FILE, /**< 100 Unable to read from file. */
|
||||
RET_FILEDATA_INCONSISTENT, /**< File contains inconsistent data. */
|
||||
/* SolutionAnalysis */
|
||||
RET_NO_SOLUTION, /**< QP solution does not satisfy KKT optimality conditions. */
|
||||
RET_INACCURATE_SOLUTION /**< KKT optimality conditions not satisfied to sufficient accuracy. */
|
||||
};
|
||||
|
||||
|
||||
|
||||
/** This class handles all kinds of messages (errors, warnings, infos) initiated
|
||||
* by qpOASES modules and stores the correspoding global preferences.
|
||||
*
|
||||
* \author Hans Joachim Ferreau (special thanks to Leonard Wirsching)
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*/
|
||||
class MessageHandling
|
||||
{
|
||||
/*
|
||||
* INTERNAL DATA STRUCTURES
|
||||
*/
|
||||
public:
|
||||
/** Data structure for entries in global message list. */
|
||||
typedef struct {
|
||||
returnValue key; /**< Global return value. */
|
||||
const char* data; /**< Corresponding message. */
|
||||
VisibilityStatus globalVisibilityStatus; /**< Determines if message can be printed.
|
||||
* If this value is set to VS_HIDDEN, no message is printed! */
|
||||
} ReturnValueList;
|
||||
|
||||
|
||||
/*
|
||||
* PUBLIC MEMBER FUNCTIONS
|
||||
*/
|
||||
public:
|
||||
/** Default constructor. */
|
||||
MessageHandling( );
|
||||
|
||||
/** Constructor which takes the desired output file. */
|
||||
MessageHandling( myFILE* _outputFile /**< Output file. */
|
||||
);
|
||||
|
||||
/** Constructor which takes the desired visibility states. */
|
||||
MessageHandling( VisibilityStatus _errorVisibility, /**< Visibility status for error messages. */
|
||||
VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */
|
||||
VisibilityStatus _infoVisibility /**< Visibility status for info messages. */
|
||||
);
|
||||
|
||||
/** Constructor which takes the desired output file and desired visibility states. */
|
||||
MessageHandling( myFILE* _outputFile, /**< Output file. */
|
||||
VisibilityStatus _errorVisibility, /**< Visibility status for error messages. */
|
||||
VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */
|
||||
VisibilityStatus _infoVisibility /**< Visibility status for info messages. */
|
||||
);
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
MessageHandling( const MessageHandling& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~MessageHandling( );
|
||||
|
||||
/** Assignment operator (deep copy). */
|
||||
MessageHandling& operator=( const MessageHandling& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
|
||||
/** Prints an error message(a simplified macro THROWERROR is also provided). \n
|
||||
* Errors are definied as abnormal events which cause an immediate termination of the current (sub) function.
|
||||
* Errors of a sub function should be commented by the calling function by means of a warning message
|
||||
* (if this error does not cause an error of the calling function, either)!
|
||||
* \return Error number returned by sub function call
|
||||
*/
|
||||
returnValue throwError(
|
||||
returnValue Enumber, /**< Error number returned by sub function call. */
|
||||
const char* additionaltext, /**< Additional error text (0, if none). */
|
||||
const char* functionname, /**< Name of function which caused the error. */
|
||||
const char* filename, /**< Name of file which caused the error. */
|
||||
const unsigned long linenumber, /**< Number of line which caused the error.incompatible binary file */
|
||||
VisibilityStatus localVisibilityStatus /**< Determines (locally) if error message can be printed to myStderr.
|
||||
* If GLOBAL visibility status of the message is set to VS_HIDDEN,
|
||||
* no message is printed, anyway! */
|
||||
);
|
||||
|
||||
/** Prints a warning message (a simplified macro THROWWARNING is also provided).
|
||||
* Warnings are definied as abnormal events which does NOT cause an immediate termination of the current (sub) function.
|
||||
* \return Warning number returned by sub function call
|
||||
*/
|
||||
returnValue throwWarning(
|
||||
returnValue Wnumber, /**< Warning number returned by sub function call. */
|
||||
const char* additionaltext, /**< Additional warning text (0, if none). */
|
||||
const char* functionname, /**< Name of function which caused the warning. */
|
||||
const char* filename, /**< Name of file which caused the warning. */
|
||||
const unsigned long linenumber, /**< Number of line which caused the warning. */
|
||||
VisibilityStatus localVisibilityStatus /**< Determines (locally) if warning message can be printed to myStderr.
|
||||
* If GLOBAL visibility status of the message is set to VS_HIDDEN,
|
||||
* no message is printed, anyway! */
|
||||
);
|
||||
|
||||
/** Prints a info message (a simplified macro THROWINFO is also provided).
|
||||
* \return Info number returned by sub function call
|
||||
*/
|
||||
returnValue throwInfo(
|
||||
returnValue Inumber, /**< Info number returned by sub function call. */
|
||||
const char* additionaltext, /**< Additional warning text (0, if none). */
|
||||
const char* functionname, /**< Name of function which submitted the info. */
|
||||
const char* filename, /**< Name of file which submitted the info. */
|
||||
const unsigned long linenumber, /**< Number of line which submitted the info. */
|
||||
VisibilityStatus localVisibilityStatus /**< Determines (locally) if info message can be printed to myStderr.
|
||||
* If GLOBAL visibility status of the message is set to VS_HIDDEN,
|
||||
* no message is printed, anyway! */
|
||||
);
|
||||
|
||||
|
||||
/** Resets all preferences to default values.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue reset( );
|
||||
|
||||
|
||||
/** Prints a complete list of all messages to output file.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue listAllMessages( );
|
||||
|
||||
|
||||
/** Returns visibility status for error messages.
|
||||
* \return Visibility status for error messages. */
|
||||
inline VisibilityStatus getErrorVisibilityStatus( ) const;
|
||||
|
||||
/** Returns visibility status for warning messages.
|
||||
* \return Visibility status for warning messages. */
|
||||
inline VisibilityStatus getWarningVisibilityStatus( ) const;
|
||||
|
||||
/** Returns visibility status for info messages.
|
||||
* \return Visibility status for info messages. */
|
||||
inline VisibilityStatus getInfoVisibilityStatus( ) const;
|
||||
|
||||
/** Returns pointer to output file.
|
||||
* \return Pointer to output file. */
|
||||
inline myFILE* getOutputFile( ) const;
|
||||
|
||||
/** Returns error count value.
|
||||
* \return Error count value. */
|
||||
inline int getErrorCount( ) const;
|
||||
|
||||
|
||||
/** Changes visibility status for error messages. */
|
||||
inline void setErrorVisibilityStatus( VisibilityStatus _errorVisibility /**< New visibility status for error messages. */
|
||||
);
|
||||
|
||||
/** Changes visibility status for warning messages. */
|
||||
inline void setWarningVisibilityStatus( VisibilityStatus _warningVisibility /**< New visibility status for warning messages. */
|
||||
);
|
||||
|
||||
/** Changes visibility status for info messages. */
|
||||
inline void setInfoVisibilityStatus( VisibilityStatus _infoVisibility /**< New visibility status for info messages. */
|
||||
);
|
||||
|
||||
/** Changes output file for messages. */
|
||||
inline void setOutputFile( myFILE* _outputFile /**< New output file for messages. */
|
||||
);
|
||||
|
||||
/** Changes error count.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
* RET_INVALID_ARGUMENT */
|
||||
inline returnValue setErrorCount( int _errorCount /**< New error count value. */
|
||||
);
|
||||
|
||||
/** Return the error code string. */
|
||||
static const char* getErrorString(int error);
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER FUNCTIONS
|
||||
*/
|
||||
protected:
|
||||
/** Prints a info message to myStderr (auxiliary function).
|
||||
* \return Error/warning/info number returned by sub function call
|
||||
*/
|
||||
returnValue throwMessage(
|
||||
returnValue RETnumber, /**< Error/warning/info number returned by sub function call. */
|
||||
const char* additionaltext, /**< Additional warning text (0, if none). */
|
||||
const char* functionname, /**< Name of function which caused the error/warning/info. */
|
||||
const char* filename, /**< Name of file which caused the error/warning/info. */
|
||||
const unsigned long linenumber, /**< Number of line which caused the error/warning/info. */
|
||||
VisibilityStatus localVisibilityStatus, /**< Determines (locally) if info message can be printed to myStderr.
|
||||
* If GLOBAL visibility status of the message is set to VS_HIDDEN,
|
||||
* no message is printed, anyway! */
|
||||
const char* RETstring /**< Leading string of error/warning/info message. */
|
||||
);
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER VARIABLES
|
||||
*/
|
||||
protected:
|
||||
VisibilityStatus errorVisibility; /**< Error messages visible? */
|
||||
VisibilityStatus warningVisibility; /**< Warning messages visible? */
|
||||
VisibilityStatus infoVisibility; /**< Info messages visible? */
|
||||
|
||||
myFILE* outputFile; /**< Output file for messages. */
|
||||
|
||||
int errorCount; /**< Counts number of errors (for nicer output only). */
|
||||
};
|
||||
|
||||
|
||||
#ifndef __FUNCTION__
|
||||
/** Ensures that __FUNCTION__ macro is defined. */
|
||||
#define __FUNCTION__ 0
|
||||
#endif
|
||||
|
||||
#ifndef __FILE__
|
||||
/** Ensures that __FILE__ macro is defined. */
|
||||
#define __FILE__ 0
|
||||
#endif
|
||||
|
||||
#ifndef __LINE__
|
||||
/** Ensures that __LINE__ macro is defined. */
|
||||
#define __LINE__ 0
|
||||
#endif
|
||||
|
||||
|
||||
/** Short version of throwError with default values, only returnValue is needed */
|
||||
#define THROWERROR(retval) ( getGlobalMessageHandler( )->throwError((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
|
||||
|
||||
/** Short version of throwWarning with default values, only returnValue is needed */
|
||||
#define THROWWARNING(retval) ( getGlobalMessageHandler( )->throwWarning((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
|
||||
|
||||
/** Short version of throwInfo with default values, only returnValue is needed */
|
||||
#define THROWINFO(retval) ( getGlobalMessageHandler( )->throwInfo((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
|
||||
|
||||
|
||||
/** Returns a pointer to global message handler.
|
||||
* \return Pointer to global message handler.
|
||||
*/
|
||||
MessageHandling* getGlobalMessageHandler( );
|
||||
|
||||
|
||||
#include <MessageHandling.ipp>
|
||||
|
||||
#endif /* QPOASES_MESSAGEHANDLING_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,666 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/QProblem.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of the QProblem class which is able to use the newly
|
||||
* developed online active set strategy for parametric quadratic programming.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifndef QPOASES_QPROBLEM_HPP
|
||||
#define QPOASES_QPROBLEM_HPP
|
||||
|
||||
|
||||
#include <QProblemB.hpp>
|
||||
#include <Constraints.hpp>
|
||||
#include <CyclingManager.hpp>
|
||||
|
||||
|
||||
/** A class for setting up and solving quadratic programs. The main feature is
|
||||
* the possibily to use the newly developed online active set strategy for
|
||||
* parametric quadratic programming.
|
||||
*
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*/
|
||||
class QProblem : public QProblemB
|
||||
{
|
||||
/* allow SolutionAnalysis class to access private members */
|
||||
friend class SolutionAnalysis;
|
||||
|
||||
/*
|
||||
* PUBLIC MEMBER FUNCTIONS
|
||||
*/
|
||||
public:
|
||||
/** Default constructor. */
|
||||
QProblem( );
|
||||
|
||||
/** Constructor which takes the QP dimensions only. */
|
||||
QProblem( int _nV, /**< Number of variables. */
|
||||
int _nC /**< Number of constraints. */
|
||||
);
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
QProblem( const QProblem& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~QProblem( );
|
||||
|
||||
/** Assignment operator (deep copy). */
|
||||
QProblem& operator=( const QProblem& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
|
||||
/** Clears all data structures of QProblemB except for QP data.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_RESET_FAILED */
|
||||
returnValue reset( );
|
||||
|
||||
|
||||
/** Initialises a QProblem with given QP data and solves it
|
||||
* using an initial homotopy with empty working set (at most nWSR iterations).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INIT_FAILED \n
|
||||
RET_INIT_FAILED_CHOLESKY \n
|
||||
RET_INIT_FAILED_TQ \n
|
||||
RET_INIT_FAILED_HOTSTART \n
|
||||
RET_INIT_FAILED_INFEASIBILITY \n
|
||||
RET_INIT_FAILED_UNBOUNDEDNESS \n
|
||||
RET_MAX_NWSR_REACHED \n
|
||||
RET_INVALID_ARGUMENTS \n
|
||||
RET_INACCURATE_SOLUTION \n
|
||||
RET_NO_SOLUTION */
|
||||
returnValue init( const real_t* const _H, /**< Hessian matrix. */
|
||||
const real_t* const _g, /**< Gradient vector. */
|
||||
const real_t* const _A, /**< Constraint matrix. */
|
||||
const real_t* const _lb, /**< Lower bound vector (on variables). \n
|
||||
If no lower bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ub, /**< Upper bound vector (on variables). \n
|
||||
If no upper bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _lbA, /**< Lower constraints' bound vector. \n
|
||||
If no lower constraints' bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ubA, /**< Upper constraints' bound vector. \n
|
||||
If no lower constraints' bounds exist, a NULL pointer can be passed. */
|
||||
int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy.
|
||||
Output: Number of performed working set recalculations. */
|
||||
const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */
|
||||
real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */
|
||||
);
|
||||
|
||||
|
||||
/** Initialises a QProblem with given QP data and solves it
|
||||
* using an initial homotopy with empty working set (at most nWSR iterations).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INIT_FAILED \n
|
||||
RET_INIT_FAILED_CHOLESKY \n
|
||||
RET_INIT_FAILED_TQ \n
|
||||
RET_INIT_FAILED_HOTSTART \n
|
||||
RET_INIT_FAILED_INFEASIBILITY \n
|
||||
RET_INIT_FAILED_UNBOUNDEDNESS \n
|
||||
RET_MAX_NWSR_REACHED \n
|
||||
RET_INVALID_ARGUMENTS \n
|
||||
RET_INACCURATE_SOLUTION \n
|
||||
RET_NO_SOLUTION */
|
||||
returnValue init( const real_t* const _H, /**< Hessian matrix. */
|
||||
const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */
|
||||
const real_t* const _g, /**< Gradient vector. */
|
||||
const real_t* const _A, /**< Constraint matrix. */
|
||||
const real_t* const _lb, /**< Lower bound vector (on variables). \n
|
||||
If no lower bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ub, /**< Upper bound vector (on variables). \n
|
||||
If no upper bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _lbA, /**< Lower constraints' bound vector. \n
|
||||
If no lower constraints' bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ubA, /**< Upper constraints' bound vector. \n
|
||||
If no lower constraints' bounds exist, a NULL pointer can be passed. */
|
||||
int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy.
|
||||
Output: Number of performed working set recalculations. */
|
||||
const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */
|
||||
real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */
|
||||
);
|
||||
|
||||
|
||||
/** Solves QProblem using online active set strategy.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_MAX_NWSR_REACHED \n
|
||||
RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
|
||||
RET_HOTSTART_FAILED \n
|
||||
RET_SHIFT_DETERMINATION_FAILED \n
|
||||
RET_STEPDIRECTION_DETERMINATION_FAILED \n
|
||||
RET_STEPLENGTH_DETERMINATION_FAILED \n
|
||||
RET_HOMOTOPY_STEP_FAILED \n
|
||||
RET_HOTSTART_STOPPED_INFEASIBILITY \n
|
||||
RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
|
||||
RET_INACCURATE_SOLUTION \n
|
||||
RET_NO_SOLUTION */
|
||||
returnValue hotstart( const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */
|
||||
const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n
|
||||
If no lower bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n
|
||||
If no upper bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n
|
||||
If no lower constraints' bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n
|
||||
If no upper constraints' bounds exist, a NULL pointer can be passed. */
|
||||
int& nWSR, /**< Input: Maximum number of working set recalculations; \n
|
||||
Output: Number of performed working set recalculations. */
|
||||
real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
|
||||
);
|
||||
|
||||
|
||||
/** Returns constraint matrix of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getA( real_t* const _A /**< Array of appropriate dimension for copying constraint matrix.*/
|
||||
) const;
|
||||
|
||||
/** Returns a single row of constraint matrix of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue getA( int number, /**< Number of entry to be returned. */
|
||||
real_t* const row /**< Array of appropriate dimension for copying (number)th constraint. */
|
||||
) const;
|
||||
|
||||
/** Returns lower constraints' bound vector of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getLBA( real_t* const _lbA /**< Array of appropriate dimension for copying lower constraints' bound vector.*/
|
||||
) const;
|
||||
|
||||
/** Returns single entry of lower constraints' bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue getLBA( int number, /**< Number of entry to be returned. */
|
||||
real_t& value /**< Output: lbA[number].*/
|
||||
) const;
|
||||
|
||||
/** Returns upper constraints' bound vector of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getUBA( real_t* const _ubA /**< Array of appropriate dimension for copying upper constraints' bound vector.*/
|
||||
) const;
|
||||
|
||||
/** Returns single entry of upper constraints' bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue getUBA( int number, /**< Number of entry to be returned. */
|
||||
real_t& value /**< Output: ubA[number].*/
|
||||
) const;
|
||||
|
||||
|
||||
/** Returns current constraints object of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getConstraints( Constraints* const _constraints /** Output: Constraints object. */
|
||||
) const;
|
||||
|
||||
|
||||
/** Returns the number of constraints.
|
||||
* \return Number of constraints. */
|
||||
inline int getNC( ) const;
|
||||
|
||||
/** Returns the number of (implicitly defined) equality constraints.
|
||||
* \return Number of (implicitly defined) equality constraints. */
|
||||
inline int getNEC( ) const;
|
||||
|
||||
/** Returns the number of active constraints.
|
||||
* \return Number of active constraints. */
|
||||
inline int getNAC( );
|
||||
|
||||
/** Returns the number of inactive constraints.
|
||||
* \return Number of inactive constraints. */
|
||||
inline int getNIAC( );
|
||||
|
||||
/** Returns the dimension of null space.
|
||||
* \return Dimension of null space. */
|
||||
int getNZ( );
|
||||
|
||||
|
||||
/** Returns the dual solution vector (deep copy).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_QP_NOT_SOLVED */
|
||||
returnValue getDualSolution( real_t* const yOpt /**< Output: Dual solution vector (if QP has been solved). */
|
||||
) const;
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER FUNCTIONS
|
||||
*/
|
||||
protected:
|
||||
/** Determines type of constraints and bounds (i.e. implicitly fixed, unbounded etc.).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SETUPSUBJECTTOTYPE_FAILED */
|
||||
returnValue setupSubjectToType( );
|
||||
|
||||
/** Computes the Cholesky decomposition R of the projected Hessian (i.e. R^T*R = Z^T*H*Z).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
* RET_INDEXLIST_CORRUPTED */
|
||||
returnValue setupCholeskyDecompositionProjected( );
|
||||
|
||||
/** Initialises TQ factorisation of A (i.e. A*Q = [0 T]) if NO constraint is active.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEXLIST_CORRUPTED */
|
||||
returnValue setupTQfactorisation( );
|
||||
|
||||
|
||||
/** Solves a QProblem whose QP data is assumed to be stored in the member variables.
|
||||
* A guess for its primal/dual optimal solution vectors and the corresponding
|
||||
* working sets of bounds and constraints can be provided.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INIT_FAILED \n
|
||||
RET_INIT_FAILED_CHOLESKY \n
|
||||
RET_INIT_FAILED_TQ \n
|
||||
RET_INIT_FAILED_HOTSTART \n
|
||||
RET_INIT_FAILED_INFEASIBILITY \n
|
||||
RET_INIT_FAILED_UNBOUNDEDNESS \n
|
||||
RET_MAX_NWSR_REACHED */
|
||||
returnValue solveInitialQP( const real_t* const xOpt, /**< Optimal primal solution vector.
|
||||
* A NULL pointer can be passed. */
|
||||
const real_t* const yOpt, /**< Optimal dual solution vector.
|
||||
* A NULL pointer can be passed. */
|
||||
const Bounds* const guessedBounds, /**< Guessed working set of bounds for solution (xOpt,yOpt).
|
||||
* A NULL pointer can be passed. */
|
||||
const Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt).
|
||||
* A NULL pointer can be passed. */
|
||||
int& nWSR, /**< Input: Maximum number of working set recalculations; \n
|
||||
* Output: Number of performed working set recalculations. */
|
||||
real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
|
||||
);
|
||||
|
||||
/** Obtains the desired working set for the auxiliary initial QP in
|
||||
* accordance with the user specifications
|
||||
* (assumes that member AX has already been initialised!)
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_OBTAINING_WORKINGSET_FAILED \n
|
||||
RET_INVALID_ARGUMENTS */
|
||||
returnValue obtainAuxiliaryWorkingSet( const real_t* const xOpt, /**< Optimal primal solution vector.
|
||||
* If a NULL pointer is passed, all entries are assumed to be zero. */
|
||||
const real_t* const yOpt, /**< Optimal dual solution vector.
|
||||
* If a NULL pointer is passed, all entries are assumed to be zero. */
|
||||
const Bounds* const guessedBounds, /**< Guessed working set of bounds for solution (xOpt,yOpt). */
|
||||
const Constraints* const guessedConstraints, /**< Guessed working set for solution (xOpt,yOpt). */
|
||||
Bounds* auxiliaryBounds, /**< Input: Allocated bound object. \n
|
||||
* Ouput: Working set of constraints for auxiliary QP. */
|
||||
Constraints* auxiliaryConstraints /**< Input: Allocated bound object. \n
|
||||
* Ouput: Working set for auxiliary QP. */
|
||||
) const;
|
||||
|
||||
/** Setups bound and constraints data structures according to auxiliaryBounds/Constraints.
|
||||
* (If the working set shall be setup afresh, make sure that
|
||||
* bounds and constraints data structure have been resetted
|
||||
* and the TQ factorisation has been initialised!)
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SETUP_WORKINGSET_FAILED \n
|
||||
RET_INVALID_ARGUMENTS \n
|
||||
RET_UNKNOWN BUG */
|
||||
returnValue setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */
|
||||
const Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */
|
||||
BooleanType setupAfresh /**< Flag indicating if given working set shall be
|
||||
* setup afresh or by updating the current one. */
|
||||
);
|
||||
|
||||
/** Setups the optimal primal/dual solution of the auxiliary initial QP.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue setupAuxiliaryQPsolution( const real_t* const xOpt, /**< Optimal primal solution vector.
|
||||
* If a NULL pointer is passed, all entries are set to zero. */
|
||||
const real_t* const yOpt /**< Optimal dual solution vector.
|
||||
* If a NULL pointer is passed, all entries are set to zero. */
|
||||
);
|
||||
|
||||
/** Setups gradient of the auxiliary initial QP for given
|
||||
* optimal primal/dual solution and given initial working set
|
||||
* (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue setupAuxiliaryQPgradient( );
|
||||
|
||||
/** Setups (constraints') bounds of the auxiliary initial QP for given
|
||||
* optimal primal/dual solution and given initial working set
|
||||
* (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNKNOWN BUG */
|
||||
returnValue setupAuxiliaryQPbounds( const Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */
|
||||
const Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */
|
||||
BooleanType useRelaxation /**< Flag indicating if inactive (constraints') bounds shall be relaxed. */
|
||||
);
|
||||
|
||||
|
||||
/** Adds a constraint to active set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_ADDCONSTRAINT_FAILED \n
|
||||
RET_ADDCONSTRAINT_FAILED_INFEASIBILITY \n
|
||||
RET_ENSURELI_FAILED */
|
||||
returnValue addConstraint( int number, /**< Number of constraint to be added to active set. */
|
||||
SubjectToStatus C_status, /**< Status of new active constraint. */
|
||||
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
|
||||
);
|
||||
|
||||
/** Checks if new active constraint to be added is linearly dependent from
|
||||
* from row of the active constraints matrix.
|
||||
* \return RET_LINEARLY_DEPENDENT \n
|
||||
RET_LINEARLY_INDEPENDENT \n
|
||||
RET_INDEXLIST_CORRUPTED */
|
||||
returnValue addConstraint_checkLI( int number /**< Number of constraint to be added to active set. */
|
||||
);
|
||||
|
||||
/** Ensures linear independence of constraint matrix when a new constraint is added.
|
||||
* To this end a bound or constraint is removed simultaneously if necessary.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_LI_RESOLVED \n
|
||||
RET_ENSURELI_FAILED \n
|
||||
RET_ENSURELI_FAILED_TQ \n
|
||||
RET_ENSURELI_FAILED_NOINDEX \n
|
||||
RET_REMOVE_FROM_ACTIVESET */
|
||||
returnValue addConstraint_ensureLI( int number, /**< Number of constraint to be added to active set. */
|
||||
SubjectToStatus C_status /**< Status of new active bound. */
|
||||
);
|
||||
|
||||
/** Adds a bound to active set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_ADDBOUND_FAILED \n
|
||||
RET_ADDBOUND_FAILED_INFEASIBILITY \n
|
||||
RET_ENSURELI_FAILED */
|
||||
returnValue addBound( int number, /**< Number of bound to be added to active set. */
|
||||
SubjectToStatus B_status, /**< Status of new active bound. */
|
||||
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
|
||||
);
|
||||
|
||||
/** Checks if new active bound to be added is linearly dependent from
|
||||
* from row of the active constraints matrix.
|
||||
* \return RET_LINEARLY_DEPENDENT \n
|
||||
RET_LINEARLY_INDEPENDENT */
|
||||
returnValue addBound_checkLI( int number /**< Number of bound to be added to active set. */
|
||||
);
|
||||
|
||||
/** Ensures linear independence of constraint matrix when a new bound is added.
|
||||
* To this end a bound or constraint is removed simultaneously if necessary.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_LI_RESOLVED \n
|
||||
RET_ENSURELI_FAILED \n
|
||||
RET_ENSURELI_FAILED_TQ \n
|
||||
RET_ENSURELI_FAILED_NOINDEX \n
|
||||
RET_REMOVE_FROM_ACTIVESET */
|
||||
returnValue addBound_ensureLI( int number, /**< Number of bound to be added to active set. */
|
||||
SubjectToStatus B_status /**< Status of new active bound. */
|
||||
);
|
||||
|
||||
/** Removes a constraint from active set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_CONSTRAINT_NOT_ACTIVE \n
|
||||
RET_REMOVECONSTRAINT_FAILED \n
|
||||
RET_HESSIAN_NOT_SPD */
|
||||
returnValue removeConstraint( int number, /**< Number of constraint to be removed from active set. */
|
||||
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
|
||||
);
|
||||
|
||||
/** Removes a bounds from active set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_BOUND_NOT_ACTIVE \n
|
||||
RET_HESSIAN_NOT_SPD \n
|
||||
RET_REMOVEBOUND_FAILED */
|
||||
returnValue removeBound( int number, /**< Number of bound to be removed from active set. */
|
||||
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
|
||||
);
|
||||
|
||||
|
||||
/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_DIV_BY_ZERO */
|
||||
returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */
|
||||
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
|
||||
real_t* const a /**< Output: Solution vector */
|
||||
);
|
||||
|
||||
/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n
|
||||
* Special variant for the case that this function is called from within "removeBound()".
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_DIV_BY_ZERO */
|
||||
returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */
|
||||
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
|
||||
BooleanType removingBound, /**< Indicates if function is called from "removeBound()". */
|
||||
real_t* const a /**< Output: Solution vector */
|
||||
);
|
||||
|
||||
|
||||
/** Solves the system Ta = b or T^Ta = b where T is a reverse upper triangular matrix.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_DIV_BY_ZERO */
|
||||
returnValue backsolveT( const real_t* const b, /**< Right hand side vector. */
|
||||
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
|
||||
real_t* const a /**< Output: Solution vector */
|
||||
);
|
||||
|
||||
|
||||
/** Determines step direction of the shift of the QP data.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue hotstart_determineDataShift(const int* const FX_idx, /**< Index array of fixed variables. */
|
||||
const int* const AC_idx, /**< Index array of active constraints. */
|
||||
const real_t* const g_new, /**< New gradient vector. */
|
||||
const real_t* const lbA_new,/**< New lower constraints' bounds. */
|
||||
const real_t* const ubA_new,/**< New upper constraints' bounds. */
|
||||
const real_t* const lb_new, /**< New lower bounds. */
|
||||
const real_t* const ub_new, /**< New upper bounds. */
|
||||
real_t* const delta_g, /**< Output: Step direction of gradient vector. */
|
||||
real_t* const delta_lbA, /**< Output: Step direction of lower constraints' bounds. */
|
||||
real_t* const delta_ubA, /**< Output: Step direction of upper constraints' bounds. */
|
||||
real_t* const delta_lb, /**< Output: Step direction of lower bounds. */
|
||||
real_t* const delta_ub, /**< Output: Step direction of upper bounds. */
|
||||
BooleanType& Delta_bC_isZero,/**< Output: Indicates if active constraints' bounds are to be shifted. */
|
||||
BooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */
|
||||
);
|
||||
|
||||
/** Determines step direction of the homotopy path.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_STEPDIRECTION_FAILED_TQ \n
|
||||
RET_STEPDIRECTION_FAILED_CHOLESKY */
|
||||
returnValue hotstart_determineStepDirection(const int* const FR_idx, /**< Index array of free variables. */
|
||||
const int* const FX_idx, /**< Index array of fixed variables. */
|
||||
const int* const AC_idx, /**< Index array of active constraints. */
|
||||
const real_t* const delta_g, /**< Step direction of gradient vector. */
|
||||
const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */
|
||||
const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */
|
||||
const real_t* const delta_lb, /**< Step direction of lower bounds. */
|
||||
const real_t* const delta_ub, /**< Step direction of upper bounds. */
|
||||
BooleanType Delta_bC_isZero, /**< Indicates if active constraints' bounds are to be shifted. */
|
||||
BooleanType Delta_bB_isZero, /**< Indicates if active bounds are to be shifted. */
|
||||
real_t* const delta_xFX, /**< Output: Primal homotopy step direction of fixed variables. */
|
||||
real_t* const delta_xFR, /**< Output: Primal homotopy step direction of free variables. */
|
||||
real_t* const delta_yAC, /**< Output: Dual homotopy step direction of active constraints' multiplier. */
|
||||
real_t* const delta_yFX /**< Output: Dual homotopy step direction of fixed variables' multiplier. */
|
||||
);
|
||||
|
||||
/** Determines the maximum possible step length along the homotopy path.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue hotstart_determineStepLength( const int* const FR_idx, /**< Index array of free variables. */
|
||||
const int* const FX_idx, /**< Index array of fixed variables. */
|
||||
const int* const AC_idx, /**< Index array of active constraints. */
|
||||
const int* const IAC_idx, /**< Index array of inactive constraints. */
|
||||
const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */
|
||||
const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */
|
||||
const real_t* const delta_lb, /**< Step direction of lower bounds. */
|
||||
const real_t* const delta_ub, /**< Step direction of upper bounds. */
|
||||
const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */
|
||||
const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */
|
||||
const real_t* const delta_yAC, /**< Dual homotopy step direction of active constraints' multiplier. */
|
||||
const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */
|
||||
real_t* const delta_Ax, /**< Output: Step in vector Ax. */
|
||||
int& BC_idx, /**< Output: Index of blocking constraint. */
|
||||
SubjectToStatus& BC_status, /**< Output: Status of blocking constraint. */
|
||||
BooleanType& BC_isBound /**< Output: Indicates if blocking constraint is a bound. */
|
||||
);
|
||||
|
||||
/** Performs a step along the homotopy path (and updates active set).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_OPTIMAL_SOLUTION_FOUND \n
|
||||
RET_REMOVE_FROM_ACTIVESET_FAILED \n
|
||||
RET_ADD_TO_ACTIVESET_FAILED \n
|
||||
RET_QP_INFEASIBLE */
|
||||
returnValue hotstart_performStep( const int* const FR_idx, /**< Index array of free variables. */
|
||||
const int* const FX_idx, /**< Index array of fixed variables. */
|
||||
const int* const AC_idx, /**< Index array of active constraints. */
|
||||
const int* const IAC_idx, /**< Index array of inactive constraints. */
|
||||
const real_t* const delta_g, /**< Step direction of gradient vector. */
|
||||
const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */
|
||||
const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */
|
||||
const real_t* const delta_lb, /**< Step direction of lower bounds. */
|
||||
const real_t* const delta_ub, /**< Step direction of upper bounds. */
|
||||
const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */
|
||||
const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */
|
||||
const real_t* const delta_yAC, /**< Dual homotopy step direction of active constraints' multiplier. */
|
||||
const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */
|
||||
const real_t* const delta_Ax, /**< Step in vector Ax. */
|
||||
int BC_idx, /**< Index of blocking constraint. */
|
||||
SubjectToStatus BC_status, /**< Status of blocking constraint. */
|
||||
BooleanType BC_isBound /**< Indicates if blocking constraint is a bound. */
|
||||
);
|
||||
|
||||
|
||||
/** Checks if lower/upper (constraints') bounds remain consistent
|
||||
* (i.e. if lb <= ub and lbA <= ubA ) during the current step.
|
||||
* \return BT_TRUE iff (constraints") bounds remain consistent
|
||||
*/
|
||||
BooleanType areBoundsConsistent( const real_t* const delta_lb, /**< Step direction of lower bounds. */
|
||||
const real_t* const delta_ub, /**< Step direction of upper bounds. */
|
||||
const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */
|
||||
const real_t* const delta_ubA /**< Step direction of upper constraints' bounds. */
|
||||
) const;
|
||||
|
||||
|
||||
/** Setups internal QP data.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INVALID_ARGUMENTS */
|
||||
returnValue setupQPdata( const real_t* const _H, /**< Hessian matrix. */
|
||||
const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */
|
||||
const real_t* const _g, /**< Gradient vector. */
|
||||
const real_t* const _A, /**< Constraint matrix. */
|
||||
const real_t* const _lb, /**< Lower bound vector (on variables). \n
|
||||
If no lower bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ub, /**< Upper bound vector (on variables). \n
|
||||
If no upper bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _lbA, /**< Lower constraints' bound vector. \n
|
||||
If no lower constraints' bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ubA /**< Upper constraints' bound vector. \n
|
||||
If no lower constraints' bounds exist, a NULL pointer can be passed. */
|
||||
);
|
||||
|
||||
|
||||
#ifdef PC_DEBUG /* Define print functions only for debugging! */
|
||||
|
||||
/** Prints concise information on the current iteration.
|
||||
* \return SUCCESSFUL_RETURN \n */
|
||||
returnValue printIteration( int iteration, /**< Number of current iteration. */
|
||||
int BC_idx, /**< Index of blocking constraint. */
|
||||
SubjectToStatus BC_status, /**< Status of blocking constraint. */
|
||||
BooleanType BC_isBound /**< Indicates if blocking constraint is a bound. */
|
||||
);
|
||||
|
||||
/** Prints concise information on the current iteration.
|
||||
* NOTE: ONLY DEFINED FOR SUPPRESSING A COMPILER WARNING!!
|
||||
* \return SUCCESSFUL_RETURN \n */
|
||||
returnValue printIteration( int iteration, /**< Number of current iteration. */
|
||||
int BC_idx, /**< Index of blocking bound. */
|
||||
SubjectToStatus BC_status /**< Status of blocking bound. */
|
||||
);
|
||||
|
||||
#endif /* PC_DEBUG */
|
||||
|
||||
|
||||
/** Determines the maximum violation of the KKT optimality conditions
|
||||
* of the current iterate within the QProblem object.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
* RET_INACCURATE_SOLUTION \n
|
||||
* RET_NO_SOLUTION */
|
||||
returnValue checkKKTconditions( );
|
||||
|
||||
|
||||
/** Sets constraint matrix of the QP. \n
|
||||
(Remark: Also internal vector Ax is recomputed!)
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setA( const real_t* const A_new /**< New constraint matrix (with correct dimension!). */
|
||||
);
|
||||
|
||||
/** Changes single row of constraint matrix of the QP. \n
|
||||
(Remark: Also correponding component of internal vector Ax is recomputed!)
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue setA( int number, /**< Number of row to be changed. */
|
||||
const real_t* const value /**< New (number)th constraint (with correct dimension!). */
|
||||
);
|
||||
|
||||
|
||||
/** Sets constraints' lower bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setLBA( const real_t* const lbA_new /**< New constraints' lower bound vector (with correct dimension!). */
|
||||
);
|
||||
|
||||
/** Changes single entry of lower constraints' bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue setLBA( int number, /**< Number of entry to be changed. */
|
||||
real_t value /**< New value for entry of lower constraints' bound vector (with correct dimension!). */
|
||||
);
|
||||
|
||||
/** Sets constraints' upper bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setUBA( const real_t* const ubA_new /**< New constraints' upper bound vector (with correct dimension!). */
|
||||
);
|
||||
|
||||
/** Changes single entry of upper constraints' bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue setUBA( int number, /**< Number of entry to be changed. */
|
||||
real_t value /**< New value for entry of upper constraints' bound vector (with correct dimension!). */
|
||||
);
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER VARIABLES
|
||||
*/
|
||||
protected:
|
||||
real_t A[NCMAX_ALLOC*NVMAX]; /**< Constraint matrix. */
|
||||
real_t lbA[NCMAX_ALLOC]; /**< Lower constraints' bound vector. */
|
||||
real_t ubA[NCMAX_ALLOC]; /**< Upper constraints' bound vector. */
|
||||
|
||||
Constraints constraints; /**< Data structure for problem's constraints. */
|
||||
|
||||
real_t T[NVMAX*NVMAX]; /**< Reverse triangular matrix, A = [0 T]*Q'. */
|
||||
real_t Q[NVMAX*NVMAX]; /**< Orthonormal quadratic matrix, A = [0 T]*Q'. */
|
||||
int sizeT; /**< Matrix T is stored in a (sizeT x sizeT) array. */
|
||||
|
||||
real_t Ax[NCMAX_ALLOC]; /**< Stores the current product A*x (for increased efficiency only). */
|
||||
|
||||
CyclingManager cyclingManager; /**< Data structure for storing (possible) cycling information (NOT YET IMPLEMENTED!). */
|
||||
};
|
||||
|
||||
|
||||
#include <QProblem.ipp>
|
||||
|
||||
#endif /* QPOASES_QPROBLEM_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,628 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/QProblemB.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of the QProblemB class which is able to use the newly
|
||||
* developed online active set strategy for parametric quadratic programming
|
||||
* for problems with (simple) bounds only.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifndef QPOASES_QPROBLEMB_HPP
|
||||
#define QPOASES_QPROBLEMB_HPP
|
||||
|
||||
|
||||
#include <Bounds.hpp>
|
||||
|
||||
|
||||
|
||||
class SolutionAnalysis;
|
||||
|
||||
/** Class for setting up and solving quadratic programs with (simple) bounds only.
|
||||
* The main feature is the possibily to use the newly developed online active set strategy
|
||||
* for parametric quadratic programming.
|
||||
*
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*/
|
||||
class QProblemB
|
||||
{
|
||||
/* allow SolutionAnalysis class to access private members */
|
||||
friend class SolutionAnalysis;
|
||||
|
||||
/*
|
||||
* PUBLIC MEMBER FUNCTIONS
|
||||
*/
|
||||
public:
|
||||
/** Default constructor. */
|
||||
QProblemB( );
|
||||
|
||||
/** Constructor which takes the QP dimension only. */
|
||||
QProblemB( int _nV /**< Number of variables. */
|
||||
);
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
QProblemB( const QProblemB& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~QProblemB( );
|
||||
|
||||
/** Assignment operator (deep copy). */
|
||||
QProblemB& operator=( const QProblemB& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
|
||||
/** Clears all data structures of QProblemB except for QP data.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_RESET_FAILED */
|
||||
returnValue reset( );
|
||||
|
||||
|
||||
/** Initialises a QProblemB with given QP data and solves it
|
||||
* using an initial homotopy with empty working set (at most nWSR iterations).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INIT_FAILED \n
|
||||
RET_INIT_FAILED_CHOLESKY \n
|
||||
RET_INIT_FAILED_HOTSTART \n
|
||||
RET_INIT_FAILED_INFEASIBILITY \n
|
||||
RET_INIT_FAILED_UNBOUNDEDNESS \n
|
||||
RET_MAX_NWSR_REACHED \n
|
||||
RET_INVALID_ARGUMENTS \n
|
||||
RET_INACCURATE_SOLUTION \n
|
||||
RET_NO_SOLUTION */
|
||||
returnValue init( const real_t* const _H, /**< Hessian matrix. */
|
||||
const real_t* const _g, /**< Gradient vector. */
|
||||
const real_t* const _lb, /**< Lower bounds (on variables). \n
|
||||
If no lower bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ub, /**< Upper bounds (on variables). \n
|
||||
If no upper bounds exist, a NULL pointer can be passed. */
|
||||
int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n
|
||||
Output: Number of performed working set recalculations. */
|
||||
const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */
|
||||
real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */
|
||||
);
|
||||
|
||||
|
||||
/** Initialises a QProblemB with given QP data and solves it
|
||||
* using an initial homotopy with empty working set (at most nWSR iterations).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INIT_FAILED \n
|
||||
RET_INIT_FAILED_CHOLESKY \n
|
||||
RET_INIT_FAILED_HOTSTART \n
|
||||
RET_INIT_FAILED_INFEASIBILITY \n
|
||||
RET_INIT_FAILED_UNBOUNDEDNESS \n
|
||||
RET_MAX_NWSR_REACHED \n
|
||||
RET_INVALID_ARGUMENTS \n
|
||||
RET_INACCURATE_SOLUTION \n
|
||||
RET_NO_SOLUTION */
|
||||
returnValue init( const real_t* const _H, /**< Hessian matrix. */
|
||||
const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */
|
||||
const real_t* const _g, /**< Gradient vector. */
|
||||
const real_t* const _lb, /**< Lower bounds (on variables). \n
|
||||
If no lower bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ub, /**< Upper bounds (on variables). \n
|
||||
If no upper bounds exist, a NULL pointer can be passed. */
|
||||
int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n
|
||||
Output: Number of performed working set recalculations. */
|
||||
const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */
|
||||
real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */
|
||||
);
|
||||
|
||||
|
||||
/** Solves an initialised QProblemB using online active set strategy.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_MAX_NWSR_REACHED \n
|
||||
RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
|
||||
RET_HOTSTART_FAILED \n
|
||||
RET_SHIFT_DETERMINATION_FAILED \n
|
||||
RET_STEPDIRECTION_DETERMINATION_FAILED \n
|
||||
RET_STEPLENGTH_DETERMINATION_FAILED \n
|
||||
RET_HOMOTOPY_STEP_FAILED \n
|
||||
RET_HOTSTART_STOPPED_INFEASIBILITY \n
|
||||
RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
|
||||
RET_INACCURATE_SOLUTION \n
|
||||
RET_NO_SOLUTION */
|
||||
returnValue hotstart( const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */
|
||||
const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n
|
||||
If no lower bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n
|
||||
If no upper bounds exist, a NULL pointer can be passed. */
|
||||
int& nWSR, /**< Input: Maximum number of working set recalculations; \n
|
||||
Output: Number of performed working set recalculations. */
|
||||
real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
|
||||
);
|
||||
|
||||
|
||||
/** Returns Hessian matrix of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getH( real_t* const _H /**< Array of appropriate dimension for copying Hessian matrix.*/
|
||||
) const;
|
||||
|
||||
/** Returns gradient vector of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getG( real_t* const _g /**< Array of appropriate dimension for copying gradient vector.*/
|
||||
) const;
|
||||
|
||||
/** Returns lower bound vector of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getLB( real_t* const _lb /**< Array of appropriate dimension for copying lower bound vector.*/
|
||||
) const;
|
||||
|
||||
/** Returns single entry of lower bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue getLB( int number, /**< Number of entry to be returned. */
|
||||
real_t& value /**< Output: lb[number].*/
|
||||
) const;
|
||||
|
||||
/** Returns upper bound vector of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getUB( real_t* const _ub /**< Array of appropriate dimension for copying upper bound vector.*/
|
||||
) const;
|
||||
|
||||
/** Returns single entry of upper bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue getUB( int number, /**< Number of entry to be returned. */
|
||||
real_t& value /**< Output: ub[number].*/
|
||||
) const;
|
||||
|
||||
|
||||
/** Returns current bounds object of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getBounds( Bounds* const _bounds /** Output: Bounds object. */
|
||||
) const;
|
||||
|
||||
|
||||
/** Returns the number of variables.
|
||||
* \return Number of variables. */
|
||||
inline int getNV( ) const;
|
||||
|
||||
/** Returns the number of free variables.
|
||||
* \return Number of free variables. */
|
||||
inline int getNFR( );
|
||||
|
||||
/** Returns the number of fixed variables.
|
||||
* \return Number of fixed variables. */
|
||||
inline int getNFX( );
|
||||
|
||||
/** Returns the number of implicitly fixed variables.
|
||||
* \return Number of implicitly fixed variables. */
|
||||
inline int getNFV( ) const;
|
||||
|
||||
/** Returns the dimension of null space.
|
||||
* \return Dimension of null space. */
|
||||
int getNZ( );
|
||||
|
||||
|
||||
/** Returns the optimal objective function value.
|
||||
* \return finite value: Optimal objective function value (QP was solved) \n
|
||||
+infinity: QP was not yet solved */
|
||||
real_t getObjVal( ) const;
|
||||
|
||||
/** Returns the objective function value at an arbitrary point x.
|
||||
* \return Objective function value at point x */
|
||||
real_t getObjVal( const real_t* const _x /**< Point at which the objective function shall be evaluated. */
|
||||
) const;
|
||||
|
||||
/** Returns the primal solution vector.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_QP_NOT_SOLVED */
|
||||
returnValue getPrimalSolution( real_t* const xOpt /**< Output: Primal solution vector (if QP has been solved). */
|
||||
) const;
|
||||
|
||||
/** Returns the dual solution vector.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_QP_NOT_SOLVED */
|
||||
returnValue getDualSolution( real_t* const yOpt /**< Output: Dual solution vector (if QP has been solved). */
|
||||
) const;
|
||||
|
||||
|
||||
/** Returns status of the solution process.
|
||||
* \return Status of solution process. */
|
||||
inline QProblemStatus getStatus( ) const;
|
||||
|
||||
|
||||
/** Returns if the QProblem object is initialised.
|
||||
* \return BT_TRUE: QProblemB initialised \n
|
||||
BT_FALSE: QProblemB not initialised */
|
||||
inline BooleanType isInitialised( ) const;
|
||||
|
||||
/** Returns if the QP has been solved.
|
||||
* \return BT_TRUE: QProblemB solved \n
|
||||
BT_FALSE: QProblemB not solved */
|
||||
inline BooleanType isSolved( ) const;
|
||||
|
||||
/** Returns if the QP is infeasible.
|
||||
* \return BT_TRUE: QP infeasible \n
|
||||
BT_FALSE: QP feasible (or not known to be infeasible!) */
|
||||
inline BooleanType isInfeasible( ) const;
|
||||
|
||||
/** Returns if the QP is unbounded.
|
||||
* \return BT_TRUE: QP unbounded \n
|
||||
BT_FALSE: QP unbounded (or not known to be unbounded!) */
|
||||
inline BooleanType isUnbounded( ) const;
|
||||
|
||||
|
||||
/** Returns the print level.
|
||||
* \return Print level. */
|
||||
inline PrintLevel getPrintLevel( ) const;
|
||||
|
||||
/** Changes the print level.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue setPrintLevel( PrintLevel _printlevel /**< New print level. */
|
||||
);
|
||||
|
||||
|
||||
/** Returns Hessian type flag (type is not determined due to this call!).
|
||||
* \return Hessian type. */
|
||||
inline HessianType getHessianType( ) const;
|
||||
|
||||
/** Changes the print level.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setHessianType( HessianType _hessianType /**< New Hessian type. */
|
||||
);
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER FUNCTIONS
|
||||
*/
|
||||
protected:
|
||||
/** Checks if Hessian happens to be the identity matrix,
|
||||
* and sets corresponding status flag (otherwise the flag remains unaltered!).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue checkForIdentityHessian( );
|
||||
|
||||
/** Determines type of constraints and bounds (i.e. implicitly fixed, unbounded etc.).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SETUPSUBJECTTOTYPE_FAILED */
|
||||
returnValue setupSubjectToType( );
|
||||
|
||||
/** Computes the Cholesky decomposition R of the (simply projected) Hessian (i.e. R^T*R = Z^T*H*Z).
|
||||
* It only works in the case where Z is a simple projection matrix!
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
* RET_INDEXLIST_CORRUPTED */
|
||||
returnValue setupCholeskyDecomposition( );
|
||||
|
||||
|
||||
/** Solves a QProblemB whose QP data is assumed to be stored in the member variables.
|
||||
* A guess for its primal/dual optimal solution vectors and the corresponding
|
||||
* optimal working set can be provided.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INIT_FAILED \n
|
||||
RET_INIT_FAILED_CHOLESKY \n
|
||||
RET_INIT_FAILED_HOTSTART \n
|
||||
RET_INIT_FAILED_INFEASIBILITY \n
|
||||
RET_INIT_FAILED_UNBOUNDEDNESS \n
|
||||
RET_MAX_NWSR_REACHED */
|
||||
returnValue solveInitialQP( const real_t* const xOpt, /**< Optimal primal solution vector.
|
||||
* A NULL pointer can be passed. */
|
||||
const real_t* const yOpt, /**< Optimal dual solution vector.
|
||||
* A NULL pointer can be passed. */
|
||||
const Bounds* const guessedBounds, /**< Guessed working set for solution (xOpt,yOpt).
|
||||
* A NULL pointer can be passed. */
|
||||
int& nWSR, /**< Input: Maximum number of working set recalculations; \n
|
||||
* Output: Number of performed working set recalculations. */
|
||||
real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
|
||||
);
|
||||
|
||||
|
||||
/** Obtains the desired working set for the auxiliary initial QP in
|
||||
* accordance with the user specifications
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_OBTAINING_WORKINGSET_FAILED \n
|
||||
RET_INVALID_ARGUMENTS */
|
||||
returnValue obtainAuxiliaryWorkingSet( const real_t* const xOpt, /**< Optimal primal solution vector.
|
||||
* If a NULL pointer is passed, all entries are assumed to be zero. */
|
||||
const real_t* const yOpt, /**< Optimal dual solution vector.
|
||||
* If a NULL pointer is passed, all entries are assumed to be zero. */
|
||||
const Bounds* const guessedBounds, /**< Guessed working set for solution (xOpt,yOpt). */
|
||||
Bounds* auxiliaryBounds /**< Input: Allocated bound object. \n
|
||||
* Ouput: Working set for auxiliary QP. */
|
||||
) const;
|
||||
|
||||
/** Setups bound data structure according to auxiliaryBounds.
|
||||
* (If the working set shall be setup afresh, make sure that
|
||||
* bounds data structure has been resetted!)
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SETUP_WORKINGSET_FAILED \n
|
||||
RET_INVALID_ARGUMENTS \n
|
||||
RET_UNKNOWN BUG */
|
||||
returnValue setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds, /**< Working set for auxiliary QP. */
|
||||
BooleanType setupAfresh /**< Flag indicating if given working set shall be
|
||||
* setup afresh or by updating the current one. */
|
||||
);
|
||||
|
||||
/** Setups the optimal primal/dual solution of the auxiliary initial QP.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue setupAuxiliaryQPsolution( const real_t* const xOpt, /**< Optimal primal solution vector.
|
||||
* If a NULL pointer is passed, all entries are set to zero. */
|
||||
const real_t* const yOpt /**< Optimal dual solution vector.
|
||||
* If a NULL pointer is passed, all entries are set to zero. */
|
||||
);
|
||||
|
||||
/** Setups gradient of the auxiliary initial QP for given
|
||||
* optimal primal/dual solution and given initial working set
|
||||
* (assumes that members X, Y and BOUNDS have already been initialised!).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue setupAuxiliaryQPgradient( );
|
||||
|
||||
/** Setups bounds of the auxiliary initial QP for given
|
||||
* optimal primal/dual solution and given initial working set
|
||||
* (assumes that members X, Y and BOUNDS have already been initialised!).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNKNOWN BUG */
|
||||
returnValue setupAuxiliaryQPbounds( BooleanType useRelaxation /**< Flag indicating if inactive bounds shall be relaxed. */
|
||||
);
|
||||
|
||||
|
||||
/** Adds a bound to active set (specialised version for the case where no constraints exist).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_ADDBOUND_FAILED */
|
||||
returnValue addBound( int number, /**< Number of bound to be added to active set. */
|
||||
SubjectToStatus B_status, /**< Status of new active bound. */
|
||||
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
|
||||
);
|
||||
|
||||
/** Removes a bounds from active set (specialised version for the case where no constraints exist).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_HESSIAN_NOT_SPD \n
|
||||
RET_REMOVEBOUND_FAILED */
|
||||
returnValue removeBound( int number, /**< Number of bound to be removed from active set. */
|
||||
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
|
||||
);
|
||||
|
||||
|
||||
/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_DIV_BY_ZERO */
|
||||
returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */
|
||||
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
|
||||
real_t* const a /**< Output: Solution vector */
|
||||
);
|
||||
|
||||
/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n
|
||||
* Special variant for the case that this function is called from within "removeBound()".
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_DIV_BY_ZERO */
|
||||
returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */
|
||||
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
|
||||
BooleanType removingBound, /**< Indicates if function is called from "removeBound()". */
|
||||
real_t* const a /**< Output: Solution vector */
|
||||
);
|
||||
|
||||
|
||||
/** Determines step direction of the shift of the QP data.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue hotstart_determineDataShift(const int* const FX_idx, /**< Index array of fixed variables. */
|
||||
const real_t* const g_new, /**< New gradient vector. */
|
||||
const real_t* const lb_new, /**< New lower bounds. */
|
||||
const real_t* const ub_new, /**< New upper bounds. */
|
||||
real_t* const delta_g, /**< Output: Step direction of gradient vector. */
|
||||
real_t* const delta_lb, /**< Output: Step direction of lower bounds. */
|
||||
real_t* const delta_ub, /**< Output: Step direction of upper bounds. */
|
||||
BooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */
|
||||
);
|
||||
|
||||
|
||||
/** Checks if lower/upper bounds remain consistent
|
||||
* (i.e. if lb <= ub) during the current step.
|
||||
* \return BT_TRUE iff bounds remain consistent
|
||||
*/
|
||||
BooleanType areBoundsConsistent( const real_t* const delta_lb, /**< Step direction of lower bounds. */
|
||||
const real_t* const delta_ub /**< Step direction of upper bounds. */
|
||||
) const;
|
||||
|
||||
|
||||
/** Setups internal QP data.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INVALID_ARGUMENTS */
|
||||
returnValue setupQPdata( const real_t* const _H, /**< Hessian matrix. */
|
||||
const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */
|
||||
const real_t* const _g, /**< Gradient vector. */
|
||||
const real_t* const _lb, /**< Lower bounds (on variables). \n
|
||||
If no lower bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ub /**< Upper bounds (on variables). \n
|
||||
If no upper bounds exist, a NULL pointer can be passed. */
|
||||
);
|
||||
|
||||
|
||||
/** Sets Hessian matrix of the QP.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setH( const real_t* const H_new /**< New Hessian matrix (with correct dimension!). */
|
||||
);
|
||||
|
||||
/** Changes gradient vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setG( const real_t* const g_new /**< New gradient vector (with correct dimension!). */
|
||||
);
|
||||
|
||||
/** Changes lower bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setLB( const real_t* const lb_new /**< New lower bound vector (with correct dimension!). */
|
||||
);
|
||||
|
||||
/** Changes single entry of lower bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue setLB( int number, /**< Number of entry to be changed. */
|
||||
real_t value /**< New value for entry of lower bound vector. */
|
||||
);
|
||||
|
||||
/** Changes upper bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setUB( const real_t* const ub_new /**< New upper bound vector (with correct dimension!). */
|
||||
);
|
||||
|
||||
/** Changes single entry of upper bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue setUB( int number, /**< Number of entry to be changed. */
|
||||
real_t value /**< New value for entry of upper bound vector. */
|
||||
);
|
||||
|
||||
|
||||
/** Computes parameters for the Givens matrix G for which [x,y]*G = [z,0]
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline void computeGivens( real_t xold, /**< Matrix entry to be normalised. */
|
||||
real_t yold, /**< Matrix entry to be annihilated. */
|
||||
real_t& xnew, /**< Output: Normalised matrix entry. */
|
||||
real_t& ynew, /**< Output: Annihilated matrix entry. */
|
||||
real_t& c, /**< Output: Cosine entry of Givens matrix. */
|
||||
real_t& s /**< Output: Sine entry of Givens matrix. */
|
||||
) const;
|
||||
|
||||
/** Applies Givens matrix determined by c and s (cf. computeGivens).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline void applyGivens( real_t c, /**< Cosine entry of Givens matrix. */
|
||||
real_t s, /**< Sine entry of Givens matrix. */
|
||||
real_t xold, /**< Matrix entry to be transformed corresponding to
|
||||
* the normalised entry of the original matrix. */
|
||||
real_t yold, /**< Matrix entry to be transformed corresponding to
|
||||
* the annihilated entry of the original matrix. */
|
||||
real_t& xnew, /**< Output: Transformed matrix entry corresponding to
|
||||
* the normalised entry of the original matrix. */
|
||||
real_t& ynew /**< Output: Transformed matrix entry corresponding to
|
||||
* the annihilated entry of the original matrix. */
|
||||
) const;
|
||||
|
||||
|
||||
/*
|
||||
* PRIVATE MEMBER FUNCTIONS
|
||||
*/
|
||||
private:
|
||||
/** Determines step direction of the homotopy path.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_STEPDIRECTION_FAILED_CHOLESKY */
|
||||
returnValue hotstart_determineStepDirection(const int* const FR_idx, /**< Index array of free variables. */
|
||||
const int* const FX_idx, /**< Index array of fixed variables. */
|
||||
const real_t* const delta_g, /**< Step direction of gradient vector. */
|
||||
const real_t* const delta_lb, /**< Step direction of lower bounds. */
|
||||
const real_t* const delta_ub, /**< Step direction of upper bounds. */
|
||||
BooleanType Delta_bB_isZero, /**< Indicates if active bounds are to be shifted. */
|
||||
real_t* const delta_xFX, /**< Output: Primal homotopy step direction of fixed variables. */
|
||||
real_t* const delta_xFR, /**< Output: Primal homotopy step direction of free variables. */
|
||||
real_t* const delta_yFX /**< Output: Dual homotopy step direction of fixed variables' multiplier. */
|
||||
);
|
||||
|
||||
/** Determines the maximum possible step length along the homotopy path.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue hotstart_determineStepLength( const int* const FR_idx, /**< Index array of free variables. */
|
||||
const int* const FX_idx, /**< Index array of fixed variables. */
|
||||
const real_t* const delta_lb, /**< Step direction of lower bounds. */
|
||||
const real_t* const delta_ub, /**< Step direction of upper bounds. */
|
||||
const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */
|
||||
const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */
|
||||
int& BC_idx, /**< Output: Index of blocking constraint. */
|
||||
SubjectToStatus& BC_status /**< Output: Status of blocking constraint. */
|
||||
);
|
||||
|
||||
/** Performs a step along the homotopy path (and updates active set).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_OPTIMAL_SOLUTION_FOUND \n
|
||||
RET_REMOVE_FROM_ACTIVESET_FAILED \n
|
||||
RET_ADD_TO_ACTIVESET_FAILED \n
|
||||
RET_QP_INFEASIBLE */
|
||||
returnValue hotstart_performStep( const int* const FR_idx, /**< Index array of free variables. */
|
||||
const int* const FX_idx, /**< Index array of fixed variables. */
|
||||
const real_t* const delta_g, /**< Step direction of gradient vector. */
|
||||
const real_t* const delta_lb, /**< Step direction of lower bounds. */
|
||||
const real_t* const delta_ub, /**< Step direction of upper bounds. */
|
||||
const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */
|
||||
const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */
|
||||
const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */
|
||||
int BC_idx, /**< Index of blocking constraint. */
|
||||
SubjectToStatus BC_status /**< Status of blocking constraint. */
|
||||
);
|
||||
|
||||
|
||||
#ifdef PC_DEBUG /* Define print functions only for debugging! */
|
||||
|
||||
/** Prints concise information on the current iteration.
|
||||
* \return SUCCESSFUL_RETURN \n */
|
||||
returnValue printIteration( int iteration, /**< Number of current iteration. */
|
||||
int BC_idx, /**< Index of blocking bound. */
|
||||
SubjectToStatus BC_status /**< Status of blocking bound. */
|
||||
);
|
||||
|
||||
#endif /* PC_DEBUG */
|
||||
|
||||
|
||||
/** Determines the maximum violation of the KKT optimality conditions
|
||||
* of the current iterate within the QProblemB object.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
* RET_INACCURATE_SOLUTION \n
|
||||
* RET_NO_SOLUTION */
|
||||
returnValue checkKKTconditions( );
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER VARIABLES
|
||||
*/
|
||||
protected:
|
||||
real_t H[NVMAX*NVMAX]; /**< Hessian matrix. */
|
||||
BooleanType hasHessian; /**< Flag indicating whether H contains Hessian or corresponding Cholesky factor R; \sa init. */
|
||||
|
||||
real_t g[NVMAX]; /**< Gradient. */
|
||||
real_t lb[NVMAX]; /**< Lower bound vector (on variables). */
|
||||
real_t ub[NVMAX]; /**< Upper bound vector (on variables). */
|
||||
|
||||
Bounds bounds; /**< Data structure for problem's bounds. */
|
||||
|
||||
real_t R[NVMAX*NVMAX]; /**< Cholesky decomposition of H (i.e. H = R^T*R). */
|
||||
BooleanType hasCholesky; /**< Flag indicating whether Cholesky decomposition has already been setup. */
|
||||
|
||||
real_t x[NVMAX]; /**< Primal solution vector. */
|
||||
real_t y[NVMAX+NCMAX]; /**< Dual solution vector. */
|
||||
|
||||
real_t tau; /**< Last homotopy step length. */
|
||||
|
||||
QProblemStatus status; /**< Current status of the solution process. */
|
||||
|
||||
BooleanType infeasible; /**< QP infeasible? */
|
||||
BooleanType unbounded; /**< QP unbounded? */
|
||||
|
||||
HessianType hessianType; /**< Type of Hessian matrix. */
|
||||
|
||||
PrintLevel printlevel; /**< Print level. */
|
||||
|
||||
int count; /**< Counts the number of hotstart function calls (internal usage only!). */
|
||||
};
|
||||
|
||||
|
||||
#include <QProblemB.ipp>
|
||||
|
||||
#endif /* QPOASES_QPROBLEMB_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,178 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/SubjectTo.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of the SubjectTo class designed to manage working sets of
|
||||
* constraints and bounds within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_SUBJECTTO_HPP
|
||||
#define QPOASES_SUBJECTTO_HPP
|
||||
|
||||
|
||||
#include <Indexlist.hpp>
|
||||
|
||||
|
||||
|
||||
/** This class manages working sets of constraints and bounds by storing
|
||||
* index sets and other status information.
|
||||
*
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*/
|
||||
class SubjectTo
|
||||
{
|
||||
/*
|
||||
* PUBLIC MEMBER FUNCTIONS
|
||||
*/
|
||||
public:
|
||||
/** Default constructor. */
|
||||
SubjectTo( );
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
SubjectTo( const SubjectTo& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~SubjectTo( );
|
||||
|
||||
/** Assignment operator (deep copy). */
|
||||
SubjectTo& operator=( const SubjectTo& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
|
||||
/** Pseudo-constructor takes the number of constraints or bounds.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue init( int n /**< Number of constraints or bounds. */
|
||||
);
|
||||
|
||||
|
||||
/** Returns type of (constraints') bound.
|
||||
* \return Type of (constraints') bound \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline SubjectToType getType( int i /**< Number of (constraints') bound. */
|
||||
) const ;
|
||||
|
||||
/** Returns status of (constraints') bound.
|
||||
* \return Status of (constraints') bound \n
|
||||
ST_UNDEFINED */
|
||||
inline SubjectToStatus getStatus( int i /**< Number of (constraints') bound. */
|
||||
) const;
|
||||
|
||||
|
||||
/** Sets type of (constraints') bound.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue setType( int i, /**< Number of (constraints') bound. */
|
||||
SubjectToType value /**< Type of (constraints') bound. */
|
||||
);
|
||||
|
||||
/** Sets status of (constraints') bound.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue setStatus( int i, /**< Number of (constraints') bound. */
|
||||
SubjectToStatus value /**< Status of (constraints') bound. */
|
||||
);
|
||||
|
||||
|
||||
/** Sets status of lower (constraints') bounds. */
|
||||
inline void setNoLower( BooleanType _status /**< Status of lower (constraints') bounds. */
|
||||
);
|
||||
|
||||
/** Sets status of upper (constraints') bounds. */
|
||||
inline void setNoUpper( BooleanType _status /**< Status of upper (constraints') bounds. */
|
||||
);
|
||||
|
||||
|
||||
/** Returns status of lower (constraints') bounds.
|
||||
* \return BT_TRUE if there is no lower (constraints') bound on any variable. */
|
||||
inline BooleanType isNoLower( ) const;
|
||||
|
||||
/** Returns status of upper bounds.
|
||||
* \return BT_TRUE if there is no upper (constraints') bound on any variable. */
|
||||
inline BooleanType isNoUpper( ) const;
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER FUNCTIONS
|
||||
*/
|
||||
protected:
|
||||
/** Adds the index of a new constraint or bound to index set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_ADDINDEX_FAILED */
|
||||
returnValue addIndex( Indexlist* const indexlist, /**< Index list to which the new index shall be added. */
|
||||
int newnumber, /**< Number of new constraint or bound. */
|
||||
SubjectToStatus newstatus /**< Status of new constraint or bound. */
|
||||
);
|
||||
|
||||
/** Removes the index of a constraint or bound from index set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNKNOWN_BUG */
|
||||
returnValue removeIndex( Indexlist* const indexlist, /**< Index list from which the new index shall be removed. */
|
||||
int removenumber /**< Number of constraint or bound to be removed. */
|
||||
);
|
||||
|
||||
/** Swaps the indices of two constraints or bounds within the index set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SWAPINDEX_FAILED */
|
||||
returnValue swapIndex( Indexlist* const indexlist, /**< Index list in which the indices shold be swapped. */
|
||||
int number1, /**< Number of first constraint or bound. */
|
||||
int number2 /**< Number of second constraint or bound. */
|
||||
);
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER VARIABLES
|
||||
*/
|
||||
protected:
|
||||
SubjectToType type[NVMAX+NCMAX]; /**< Type of constraints/bounds. */
|
||||
SubjectToStatus status[NVMAX+NCMAX]; /**< Status of constraints/bounds. */
|
||||
|
||||
BooleanType noLower; /**< This flag indicates if there is no lower bound on any variable. */
|
||||
BooleanType noUpper; /**< This flag indicates if there is no upper bound on any variable. */
|
||||
|
||||
|
||||
/*
|
||||
* PRIVATE MEMBER VARIABLES
|
||||
*/
|
||||
private:
|
||||
int size;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#include <SubjectTo.ipp>
|
||||
|
||||
#endif /* QPOASES_SUBJECTTO_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,131 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/Types.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2008
|
||||
*
|
||||
* Declaration of all non-built-in types (except for classes).
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_TYPES_HPP
|
||||
#define QPOASES_TYPES_HPP
|
||||
|
||||
|
||||
|
||||
/** Define real_t for facilitating switching between double and float. */
|
||||
// typedef double real_t;
|
||||
|
||||
|
||||
/** Summarises all possible logical values. */
|
||||
enum BooleanType
|
||||
{
|
||||
BT_FALSE, /**< Logical value for "false". */
|
||||
BT_TRUE /**< Logical value for "true". */
|
||||
};
|
||||
|
||||
|
||||
/** Summarises all possible print levels. Print levels are used to describe
|
||||
* the desired amount of output during runtime of qpOASES. */
|
||||
enum PrintLevel
|
||||
{
|
||||
PL_NONE, /**< No output. */
|
||||
PL_LOW, /**< Print error messages only. */
|
||||
PL_MEDIUM, /**< Print error and warning messages as well as concise info messages. */
|
||||
PL_HIGH /**< Print all messages with full details. */
|
||||
};
|
||||
|
||||
|
||||
/** Defines visibility status of a message. */
|
||||
enum VisibilityStatus
|
||||
{
|
||||
VS_VISIBLE, /**< Message visible. */
|
||||
VS_HIDDEN /**< Message not visible. */
|
||||
};
|
||||
|
||||
|
||||
/** Summarises all possible states of the (S)QProblem(B) object during the
|
||||
solution process of a QP sequence. */
|
||||
enum QProblemStatus
|
||||
{
|
||||
QPS_NOTINITIALISED, /**< QProblem object is freshly instantiated or reset. */
|
||||
QPS_PREPARINGAUXILIARYQP, /**< An auxiliary problem is currently setup, either at the very beginning
|
||||
* via an initial homotopy or after changing the QP matrices. */
|
||||
QPS_AUXILIARYQPSOLVED, /**< An auxilary problem was solved, either at the very beginning
|
||||
* via an initial homotopy or after changing the QP matrices. */
|
||||
QPS_PERFORMINGHOMOTOPY, /**< A homotopy according to the main idea of the online active
|
||||
* set strategy is performed. */
|
||||
QPS_HOMOTOPYQPSOLVED, /**< An intermediate QP along the homotopy path was solved. */
|
||||
QPS_SOLVED /**< The solution of the actual QP was found. */
|
||||
};
|
||||
|
||||
|
||||
/** Summarises all possible types of bounds and constraints. */
|
||||
enum SubjectToType
|
||||
{
|
||||
ST_UNBOUNDED, /**< Bound/constraint is unbounded. */
|
||||
ST_BOUNDED, /**< Bound/constraint is bounded but not fixed. */
|
||||
ST_EQUALITY, /**< Bound/constraint is fixed (implicit equality bound/constraint). */
|
||||
ST_UNKNOWN /**< Type of bound/constraint unknown. */
|
||||
};
|
||||
|
||||
|
||||
/** Summarises all possible states of bounds and constraints. */
|
||||
enum SubjectToStatus
|
||||
{
|
||||
ST_INACTIVE, /**< Bound/constraint is inactive. */
|
||||
ST_LOWER, /**< Bound/constraint is at its lower bound. */
|
||||
ST_UPPER, /**< Bound/constraint is at its upper bound. */
|
||||
ST_UNDEFINED /**< Status of bound/constraint undefined. */
|
||||
};
|
||||
|
||||
|
||||
/** Summarises all possible cycling states of bounds and constraints. */
|
||||
enum CyclingStatus
|
||||
{
|
||||
CYC_NOT_INVOLVED, /**< Bound/constraint is not involved in current cycling. */
|
||||
CYC_PREV_ADDED, /**< Bound/constraint has previously been added during the current cycling. */
|
||||
CYC_PREV_REMOVED /**< Bound/constraint has previously been removed during the current cycling. */
|
||||
};
|
||||
|
||||
|
||||
/** Summarises all possible types of the QP's Hessian matrix. */
|
||||
enum HessianType
|
||||
{
|
||||
HST_SEMIDEF, /**< Hessian is positive semi-definite. */
|
||||
HST_POSDEF_NULLSPACE, /**< Hessian is positive definite on null space of active bounds/constraints. */
|
||||
HST_POSDEF, /**< Hessian is (strictly) positive definite. */
|
||||
HST_IDENTITY /**< Hessian is identity matrix. */
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif /* QPOASES_TYPES_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,197 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/Utils.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of global utility functions for working with qpOASES.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_UTILS_HPP
|
||||
#define QPOASES_UTILS_HPP
|
||||
|
||||
|
||||
#include <MessageHandling.hpp>
|
||||
|
||||
|
||||
#ifdef PC_DEBUG /* Define print functions only for debugging! */
|
||||
|
||||
/** Prints a vector.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue print( const real_t* const v, /**< Vector to be printed. */
|
||||
int n /**< Length of vector. */
|
||||
);
|
||||
|
||||
/** Prints a permuted vector.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue print( const real_t* const v, /**< Vector to be printed. */
|
||||
int n, /**< Length of vector. */
|
||||
const int* const V_idx /**< Pemutation vector. */
|
||||
);
|
||||
|
||||
/** Prints a named vector.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue print( const real_t* const v, /**< Vector to be printed. */
|
||||
int n, /**< Length of vector. */
|
||||
const char* name /** Name of vector. */
|
||||
);
|
||||
|
||||
/** Prints a matrix.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue print( const real_t* const M, /**< Matrix to be printed. */
|
||||
int nrow, /**< Row number of matrix. */
|
||||
int ncol /**< Column number of matrix. */
|
||||
);
|
||||
|
||||
/** Prints a permuted matrix.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue print( const real_t* const M, /**< Matrix to be printed. */
|
||||
int nrow, /**< Row number of matrix. */
|
||||
int ncol , /**< Column number of matrix. */
|
||||
const int* const ROW_idx, /**< Row pemutation vector. */
|
||||
const int* const COL_idx /**< Column pemutation vector. */
|
||||
);
|
||||
|
||||
/** Prints a named matrix.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue print( const real_t* const M, /**< Matrix to be printed. */
|
||||
int nrow, /**< Row number of matrix. */
|
||||
int ncol, /**< Column number of matrix. */
|
||||
const char* name /** Name of matrix. */
|
||||
);
|
||||
|
||||
/** Prints an index array.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue print( const int* const index, /**< Index array to be printed. */
|
||||
int n /**< Length of index array. */
|
||||
);
|
||||
|
||||
/** Prints a named index array.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue print( const int* const index, /**< Index array to be printed. */
|
||||
int n, /**< Length of index array. */
|
||||
const char* name /**< Name of index array. */
|
||||
);
|
||||
|
||||
|
||||
/** Prints a string to desired output target (useful also for MATLAB output!).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue myPrintf( const char* s /**< String to be written. */
|
||||
);
|
||||
|
||||
|
||||
/** Prints qpOASES copyright notice.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue printCopyrightNotice( );
|
||||
|
||||
|
||||
/** Reads a real_t matrix from file.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNABLE_TO_OPEN_FILE \n
|
||||
RET_UNABLE_TO_READ_FILE */
|
||||
returnValue readFromFile( real_t* data, /**< Matrix to be read from file. */
|
||||
int nrow, /**< Row number of matrix. */
|
||||
int ncol, /**< Column number of matrix. */
|
||||
const char* datafilename /**< Data file name. */
|
||||
);
|
||||
|
||||
/** Reads a real_t vector from file.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNABLE_TO_OPEN_FILE \n
|
||||
RET_UNABLE_TO_READ_FILE */
|
||||
returnValue readFromFile( real_t* data, /**< Vector to be read from file. */
|
||||
int n, /**< Length of vector. */
|
||||
const char* datafilename /**< Data file name. */
|
||||
);
|
||||
|
||||
/** Reads an integer (column) vector from file.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNABLE_TO_OPEN_FILE \n
|
||||
RET_UNABLE_TO_READ_FILE */
|
||||
returnValue readFromFile( int* data, /**< Vector to be read from file. */
|
||||
int n, /**< Length of vector. */
|
||||
const char* datafilename /**< Data file name. */
|
||||
);
|
||||
|
||||
|
||||
/** Writes a real_t matrix into a file.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNABLE_TO_OPEN_FILE */
|
||||
returnValue writeIntoFile( const real_t* const data, /**< Matrix to be written into file. */
|
||||
int nrow, /**< Row number of matrix. */
|
||||
int ncol, /**< Column number of matrix. */
|
||||
const char* datafilename, /**< Data file name. */
|
||||
BooleanType append /**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */
|
||||
);
|
||||
|
||||
/** Writes a real_t vector into a file.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNABLE_TO_OPEN_FILE */
|
||||
returnValue writeIntoFile( const real_t* const data, /**< Vector to be written into file. */
|
||||
int n, /**< Length of vector. */
|
||||
const char* datafilename, /**< Data file name. */
|
||||
BooleanType append /**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */
|
||||
);
|
||||
|
||||
/** Writes an integer (column) vector into a file.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNABLE_TO_OPEN_FILE */
|
||||
returnValue writeIntoFile( const int* const integer, /**< Integer vector to be written into file. */
|
||||
int n, /**< Length of vector. */
|
||||
const char* datafilename, /**< Data file name. */
|
||||
BooleanType append /**< Indicates if integer shall be appended if the file already exists (otherwise it is overwritten). */
|
||||
);
|
||||
|
||||
#endif /* PC_DEBUG */
|
||||
|
||||
|
||||
/** Returns the current system time.
|
||||
* \return current system time */
|
||||
real_t getCPUtime( );
|
||||
|
||||
|
||||
/** Returns the Euclidean norm of a vector.
|
||||
* \return 0: successful */
|
||||
real_t getNorm( const real_t* const v, /**< Vector. */
|
||||
int n /**< Vector's dimension. */
|
||||
);
|
||||
|
||||
/** Returns the absolute value of a real_t.
|
||||
* \return Absolute value of a real_t */
|
||||
inline real_t getAbs( real_t x /**< Input argument. */
|
||||
);
|
||||
|
||||
|
||||
|
||||
#include <Utils.ipp>
|
||||
|
||||
#endif /* QPOASES_UTILS_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
Import('env', 'interface_dir')
|
||||
|
||||
qp_files = [
|
||||
Glob("SRC/*.cpp"),
|
||||
Glob("SRC/EXTRAS/*.cpp"),
|
||||
]
|
||||
|
||||
cpp_path = [
|
||||
".",
|
||||
"INCLUDE",
|
||||
"INCLUDE/EXTRAS",
|
||||
"SRC/",
|
||||
interface_dir,
|
||||
]
|
||||
|
||||
env.Library('qpoases', qp_files, CPPPATH=cpp_path)
|
||||
|
|
@ -0,0 +1,252 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/Bounds.cpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of the Bounds class designed to manage working sets of
|
||||
* bounds within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
#include <Bounds.hpp>
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/*
|
||||
* B o u n d s
|
||||
*/
|
||||
Bounds::Bounds( ) : SubjectTo( ),
|
||||
nV( 0 ),
|
||||
nFV( 0 ),
|
||||
nBV( 0 ),
|
||||
nUV( 0 )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* B o u n d s
|
||||
*/
|
||||
Bounds::Bounds( const Bounds& rhs ) : SubjectTo( rhs ),
|
||||
nV( rhs.nV ),
|
||||
nFV( rhs.nFV ),
|
||||
nBV( rhs.nBV ),
|
||||
nUV( rhs.nUV )
|
||||
{
|
||||
free = rhs.free;
|
||||
fixed = rhs.fixed;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* ~ B o u n d s
|
||||
*/
|
||||
Bounds::~Bounds( )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* o p e r a t o r =
|
||||
*/
|
||||
Bounds& Bounds::operator=( const Bounds& rhs )
|
||||
{
|
||||
if ( this != &rhs )
|
||||
{
|
||||
SubjectTo::operator=( rhs );
|
||||
|
||||
nV = rhs.nV;
|
||||
nFV = rhs.nFV;
|
||||
nBV = rhs.nBV;
|
||||
nUV = rhs.nUV;
|
||||
|
||||
free = rhs.free;
|
||||
fixed = rhs.fixed;
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i n i t
|
||||
*/
|
||||
returnValue Bounds::init( int n )
|
||||
{
|
||||
nV = n;
|
||||
nFV = 0;
|
||||
nBV = 0;
|
||||
nUV = 0;
|
||||
|
||||
free.init( );
|
||||
fixed.init( );
|
||||
|
||||
return SubjectTo::init( n );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t u p B o u n d
|
||||
*/
|
||||
returnValue Bounds::setupBound( int _number, SubjectToStatus _status
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( ( _number < 0 ) || ( _number >= getNV( ) ) )
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
|
||||
/* Add bound index to respective index list. */
|
||||
switch ( _status )
|
||||
{
|
||||
case ST_INACTIVE:
|
||||
if ( this->addIndex( this->getFree( ),_number,_status ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_BOUND_FAILED );
|
||||
break;
|
||||
|
||||
case ST_LOWER:
|
||||
if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_BOUND_FAILED );
|
||||
break;
|
||||
|
||||
case ST_UPPER:
|
||||
if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_BOUND_FAILED );
|
||||
break;
|
||||
|
||||
default:
|
||||
return THROWERROR( RET_INVALID_ARGUMENTS );
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t u p A l l F r e e
|
||||
*/
|
||||
returnValue Bounds::setupAllFree( )
|
||||
{
|
||||
int i;
|
||||
|
||||
/* 1) Place unbounded variables at the beginning of the index list of free variables. */
|
||||
for( i=0; i<nV; ++i )
|
||||
{
|
||||
if ( getType( i ) == ST_UNBOUNDED )
|
||||
{
|
||||
if ( setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_BOUND_FAILED );
|
||||
}
|
||||
}
|
||||
|
||||
/* 2) Add remaining (i.e. bounded but possibly free) variables to the index list of free variables. */
|
||||
for( i=0; i<nV; ++i )
|
||||
{
|
||||
if ( getType( i ) == ST_BOUNDED )
|
||||
{
|
||||
if ( setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_BOUND_FAILED );
|
||||
}
|
||||
}
|
||||
|
||||
/* 3) Place implicitly fixed variables at the end of the index list of free variables. */
|
||||
for( i=0; i<nV; ++i )
|
||||
{
|
||||
if ( getType( i ) == ST_EQUALITY )
|
||||
{
|
||||
if ( setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_BOUND_FAILED );
|
||||
}
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* m o v e F i x e d T o F r e e
|
||||
*/
|
||||
returnValue Bounds::moveFixedToFree( int _number )
|
||||
{
|
||||
/* consistency check */
|
||||
if ( ( _number < 0 ) || ( _number >= getNV( ) ) )
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
|
||||
/* Move index from indexlist of fixed variables to that of free ones. */
|
||||
if ( this->removeIndex( this->getFixed( ),_number ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_MOVING_BOUND_FAILED );
|
||||
|
||||
if ( this->addIndex( this->getFree( ),_number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_MOVING_BOUND_FAILED );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* m o v e F r e e T o F i x e d
|
||||
*/
|
||||
returnValue Bounds::moveFreeToFixed( int _number, SubjectToStatus _status
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( ( _number < 0 ) || ( _number >= getNV( ) ) )
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
|
||||
/* Move index from indexlist of free variables to that of fixed ones. */
|
||||
if ( this->removeIndex( this->getFree( ),_number ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_MOVING_BOUND_FAILED );
|
||||
|
||||
if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_MOVING_BOUND_FAILED );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s w a p F r e e
|
||||
*/
|
||||
returnValue Bounds::swapFree( int number1, int number2
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( ( number1 < 0 ) || ( number1 >= getNV( ) ) || ( number2 < 0 ) || ( number2 >= getNV( ) ) )
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
|
||||
/* Swap index within indexlist of free variables. */
|
||||
return this->swapIndex( this->getFree( ),number1,number2 );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,144 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/Bounds.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of inlined member functions of the Bounds class designed
|
||||
* to manage working sets of bounds within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* g e t N V
|
||||
*/
|
||||
inline int Bounds::getNV( ) const
|
||||
{
|
||||
return nV;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N F X
|
||||
*/
|
||||
inline int Bounds::getNFV( ) const
|
||||
{
|
||||
return nFV;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N B V
|
||||
*/
|
||||
inline int Bounds::getNBV( ) const
|
||||
{
|
||||
return nBV;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N U V
|
||||
*/
|
||||
inline int Bounds::getNUV( ) const
|
||||
{
|
||||
return nUV;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* s e t N F X
|
||||
*/
|
||||
inline returnValue Bounds::setNFV( int n )
|
||||
{
|
||||
nFV = n;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t N B V
|
||||
*/
|
||||
inline returnValue Bounds::setNBV( int n )
|
||||
{
|
||||
nBV = n;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t N U V
|
||||
*/
|
||||
inline returnValue Bounds::setNUV( int n )
|
||||
{
|
||||
nUV = n;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N F R
|
||||
*/
|
||||
inline int Bounds::getNFR( )
|
||||
{
|
||||
return free.getLength( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N F X
|
||||
*/
|
||||
inline int Bounds::getNFX( )
|
||||
{
|
||||
return fixed.getLength( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t F r e e
|
||||
*/
|
||||
inline Indexlist* Bounds::getFree( )
|
||||
{
|
||||
return &free;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t F i x e d
|
||||
*/
|
||||
inline Indexlist* Bounds::getFixed( )
|
||||
{
|
||||
return &fixed;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,248 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/Constraints.cpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of the Constraints class designed to manage working sets of
|
||||
* constraints within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
#include <Constraints.hpp>
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/*
|
||||
* C o n s t r a i n t s
|
||||
*/
|
||||
Constraints::Constraints( ) : SubjectTo( ),
|
||||
nC( 0 ),
|
||||
nEC( 0 ),
|
||||
nIC( 0 ),
|
||||
nUC( 0 )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* C o n s t r a i n t s
|
||||
*/
|
||||
Constraints::Constraints( const Constraints& rhs ) : SubjectTo( rhs ),
|
||||
nC( rhs.nC ),
|
||||
nEC( rhs.nEC ),
|
||||
nIC( rhs.nIC ),
|
||||
nUC( rhs.nUC )
|
||||
{
|
||||
active = rhs.active;
|
||||
inactive = rhs.inactive;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* ~ C o n s t r a i n t s
|
||||
*/
|
||||
Constraints::~Constraints( )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* o p e r a t o r =
|
||||
*/
|
||||
Constraints& Constraints::operator=( const Constraints& rhs )
|
||||
{
|
||||
if ( this != &rhs )
|
||||
{
|
||||
SubjectTo::operator=( rhs );
|
||||
|
||||
nC = rhs.nC;
|
||||
nEC = rhs.nEC;
|
||||
nIC = rhs.nIC;
|
||||
nUC = rhs.nUC;
|
||||
|
||||
active = rhs.active;
|
||||
inactive = rhs.inactive;
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i n i t
|
||||
*/
|
||||
returnValue Constraints::init( int n )
|
||||
{
|
||||
nC = n;
|
||||
nEC = 0;
|
||||
nIC = 0;
|
||||
nUC = 0;
|
||||
|
||||
active.init( );
|
||||
inactive.init( );
|
||||
|
||||
return SubjectTo::init( n );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t u p C o n s t r a i n t
|
||||
*/
|
||||
returnValue Constraints::setupConstraint( int _number, SubjectToStatus _status
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( ( _number < 0 ) || ( _number >= getNC( ) ) )
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
|
||||
/* Add constraint index to respective index list. */
|
||||
switch ( _status )
|
||||
{
|
||||
case ST_INACTIVE:
|
||||
if ( this->addIndex( this->getInactive( ),_number,_status ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
|
||||
break;
|
||||
|
||||
case ST_LOWER:
|
||||
if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
|
||||
break;
|
||||
|
||||
case ST_UPPER:
|
||||
if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
|
||||
break;
|
||||
|
||||
default:
|
||||
return THROWERROR( RET_INVALID_ARGUMENTS );
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t u p A l l I n a c t i v e
|
||||
*/
|
||||
returnValue Constraints::setupAllInactive( )
|
||||
{
|
||||
int i;
|
||||
|
||||
|
||||
/* 1) Place unbounded constraints at the beginning of the index list of inactive constraints. */
|
||||
for( i=0; i<nC; ++i )
|
||||
{
|
||||
if ( getType( i ) == ST_UNBOUNDED )
|
||||
{
|
||||
if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
|
||||
}
|
||||
}
|
||||
|
||||
/* 2) Add remaining (i.e. "real" inequality) constraints to the index list of inactive constraints. */
|
||||
for( i=0; i<nC; ++i )
|
||||
{
|
||||
if ( getType( i ) == ST_BOUNDED )
|
||||
{
|
||||
if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
|
||||
}
|
||||
}
|
||||
|
||||
/* 3) Place implicit equality constraints at the end of the index list of inactive constraints. */
|
||||
for( i=0; i<nC; ++i )
|
||||
{
|
||||
if ( getType( i ) == ST_EQUALITY )
|
||||
{
|
||||
if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
|
||||
}
|
||||
}
|
||||
|
||||
/* 4) Moreover, add all constraints of unknown type. */
|
||||
for( i=0; i<nC; ++i )
|
||||
{
|
||||
if ( getType( i ) == ST_UNKNOWN )
|
||||
{
|
||||
if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
|
||||
}
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* m o v e A c t i v e T o I n a c t i v e
|
||||
*/
|
||||
returnValue Constraints::moveActiveToInactive( int _number )
|
||||
{
|
||||
/* consistency check */
|
||||
if ( ( _number < 0 ) || ( _number >= getNC( ) ) )
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
|
||||
/* Move index from indexlist of active constraints to that of inactive ones. */
|
||||
if ( this->removeIndex( this->getActive( ),_number ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_MOVING_BOUND_FAILED );
|
||||
|
||||
if ( this->addIndex( this->getInactive( ),_number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_MOVING_BOUND_FAILED );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* m o v e I n a c t i v e T o A c t i v e
|
||||
*/
|
||||
returnValue Constraints::moveInactiveToActive( int _number, SubjectToStatus _status
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( ( _number < 0 ) || ( _number >= getNC( ) ) )
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
|
||||
/* Move index from indexlist of inactive constraints to that of active ones. */
|
||||
if ( this->removeIndex( this->getInactive( ),_number ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_MOVING_BOUND_FAILED );
|
||||
|
||||
if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_MOVING_BOUND_FAILED );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,144 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/Constraints.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of inlined member functions of the Constraints class designed
|
||||
* to manage working sets of constraints within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* g e t N C
|
||||
*/
|
||||
inline int Constraints::getNC( ) const
|
||||
{
|
||||
return nC;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N E C
|
||||
*/
|
||||
inline int Constraints::getNEC( ) const
|
||||
{
|
||||
return nEC;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N I C
|
||||
*/
|
||||
inline int Constraints::getNIC( ) const
|
||||
{
|
||||
return nIC;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N U C
|
||||
*/
|
||||
inline int Constraints::getNUC( ) const
|
||||
{
|
||||
return nUC;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t N E C
|
||||
*/
|
||||
inline returnValue Constraints::setNEC( int n )
|
||||
{
|
||||
nEC = n;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t N I C
|
||||
*/
|
||||
inline returnValue Constraints::setNIC( int n )
|
||||
{
|
||||
nIC = n;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t N U C
|
||||
*/
|
||||
inline returnValue Constraints::setNUC( int n )
|
||||
{
|
||||
nUC = n;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N A C
|
||||
*/
|
||||
inline int Constraints::getNAC( )
|
||||
{
|
||||
return active.getLength( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N I A C
|
||||
*/
|
||||
inline int Constraints::getNIAC( )
|
||||
{
|
||||
return inactive.getLength( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t A c t i v e
|
||||
*/
|
||||
inline Indexlist* Constraints::getActive( )
|
||||
{
|
||||
return &active;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t I n a c t i v e
|
||||
*/
|
||||
inline Indexlist* Constraints::getInactive( )
|
||||
{
|
||||
return &inactive;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,188 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/CyclingManager.cpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of the CyclingManager class designed to detect
|
||||
* and handle possible cycling during QP iterations.
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#include <CyclingManager.hpp>
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/*
|
||||
* C y c l i n g M a n a g e r
|
||||
*/
|
||||
CyclingManager::CyclingManager( ) : nV( 0 ),
|
||||
nC( 0 )
|
||||
{
|
||||
cyclingDetected = BT_FALSE;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* C y c l i n g M a n a g e r
|
||||
*/
|
||||
CyclingManager::CyclingManager( const CyclingManager& rhs ) : nV( rhs.nV ),
|
||||
nC( rhs.nC ),
|
||||
cyclingDetected( rhs.cyclingDetected )
|
||||
{
|
||||
int i;
|
||||
|
||||
for( i=0; i<nV+nC; ++i )
|
||||
status[i] = rhs.status[i];
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* ~ C y c l i n g M a n a g e r
|
||||
*/
|
||||
CyclingManager::~CyclingManager( )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* o p e r a t o r =
|
||||
*/
|
||||
CyclingManager& CyclingManager::operator=( const CyclingManager& rhs )
|
||||
{
|
||||
int i;
|
||||
|
||||
if ( this != &rhs )
|
||||
{
|
||||
nV = rhs.nV;
|
||||
nC = rhs.nC;
|
||||
|
||||
for( i=0; i<nV+nC; ++i )
|
||||
status[i] = rhs.status[i];
|
||||
|
||||
cyclingDetected = rhs.cyclingDetected;
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* i n i t
|
||||
*/
|
||||
returnValue CyclingManager::init( int _nV, int _nC )
|
||||
{
|
||||
nV = _nV;
|
||||
nC = _nC;
|
||||
|
||||
cyclingDetected = BT_FALSE;
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* s e t C y c l i n g S t a t u s
|
||||
*/
|
||||
returnValue CyclingManager::setCyclingStatus( int number,
|
||||
BooleanType isBound, CyclingStatus _status
|
||||
)
|
||||
{
|
||||
if ( isBound == BT_TRUE )
|
||||
{
|
||||
/* Set cycling status of a bound. */
|
||||
if ( ( number >= 0 ) && ( number < nV ) )
|
||||
{
|
||||
status[number] = _status;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Set cycling status of a constraint. */
|
||||
if ( ( number >= 0 ) && ( number < nC ) )
|
||||
{
|
||||
status[nV+number] = _status;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t C y c l i n g S t a t u s
|
||||
*/
|
||||
CyclingStatus CyclingManager::getCyclingStatus( int number, BooleanType isBound ) const
|
||||
{
|
||||
if ( isBound == BT_TRUE )
|
||||
{
|
||||
/* Return cycling status of a bound. */
|
||||
if ( ( number >= 0 ) && ( number < nV ) )
|
||||
return status[number];
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Return cycling status of a constraint. */
|
||||
if ( ( number >= 0 ) && ( number < nC ) )
|
||||
return status[nV+number];
|
||||
}
|
||||
|
||||
return CYC_NOT_INVOLVED;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* c l e a r C y c l i n g D a t a
|
||||
*/
|
||||
returnValue CyclingManager::clearCyclingData( )
|
||||
{
|
||||
int i;
|
||||
|
||||
/* Reset all status values ... */
|
||||
for( i=0; i<nV+nC; ++i )
|
||||
status[i] = CYC_NOT_INVOLVED;
|
||||
|
||||
/* ... and the main cycling flag. */
|
||||
cyclingDetected = BT_FALSE;
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,51 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/CyclingManager.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of inlined member functions of the CyclingManager class
|
||||
* designed to detect and handle possible cycling during QP iterations.
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* i s C y c l i n g D e t e c t e d
|
||||
*/
|
||||
inline BooleanType CyclingManager::isCyclingDetected( ) const
|
||||
{
|
||||
return cyclingDetected;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,434 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/EXTRAS/SolutionAnalysis.cpp
|
||||
* \author Milan Vukov, Boris Houska, Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2012
|
||||
*
|
||||
* Solution analysis class, based on a class in the standard version of the qpOASES
|
||||
*/
|
||||
|
||||
#include <EXTRAS/SolutionAnalysis.hpp>
|
||||
|
||||
/*
|
||||
* S o l u t i o n A n a l y s i s
|
||||
*/
|
||||
SolutionAnalysis::SolutionAnalysis( )
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
* S o l u t i o n A n a l y s i s
|
||||
*/
|
||||
SolutionAnalysis::SolutionAnalysis( const SolutionAnalysis& rhs )
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
* ~ S o l u t i o n A n a l y s i s
|
||||
*/
|
||||
SolutionAnalysis::~SolutionAnalysis( )
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
* o p e r a t o r =
|
||||
*/
|
||||
SolutionAnalysis& SolutionAnalysis::operator=( const SolutionAnalysis& rhs )
|
||||
{
|
||||
if ( this != &rhs )
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
/*
|
||||
* g e t H e s s i a n I n v e r s e
|
||||
*/
|
||||
returnValue SolutionAnalysis::getHessianInverse( QProblem* qp, real_t* hessianInverse )
|
||||
{
|
||||
returnValue returnvalue; /* the return value */
|
||||
BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */
|
||||
BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
|
||||
|
||||
register int run1, run2, run3;
|
||||
|
||||
register int nFR, nFX;
|
||||
|
||||
/* Ask for the number of free and fixed variables, assumes that active set
|
||||
* is constant for the covariance evaluation */
|
||||
nFR = qp->getNFR( );
|
||||
nFX = qp->getNFX( );
|
||||
|
||||
/* Ask for the corresponding index arrays: */
|
||||
if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_HOTSTART_FAILED );
|
||||
|
||||
if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_HOTSTART_FAILED );
|
||||
|
||||
if ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_HOTSTART_FAILED );
|
||||
|
||||
/* Initialization: */
|
||||
for( run1 = 0; run1 < NVMAX; run1++ )
|
||||
delta_g_cov[ run1 ] = 0.0;
|
||||
|
||||
for( run1 = 0; run1 < NVMAX; run1++ )
|
||||
delta_lb_cov[ run1 ] = 0.0;
|
||||
|
||||
for( run1 = 0; run1 < NVMAX; run1++ )
|
||||
delta_ub_cov[ run1 ] = 0.0;
|
||||
|
||||
for( run1 = 0; run1 < NCMAX; run1++ )
|
||||
delta_lbA_cov[ run1 ] = 0.0;
|
||||
|
||||
for( run1 = 0; run1 < NCMAX; run1++ )
|
||||
delta_ubA_cov[ run1 ] = 0.0;
|
||||
|
||||
/* The following loop solves the following:
|
||||
*
|
||||
* KKT * x =
|
||||
* [delta_g_cov', delta_lbA_cov', delta_ubA_cov', delta_lb_cov', delta_ub_cov]'
|
||||
*
|
||||
* for the first NVMAX (negative) elementary vectors in order to get
|
||||
* transposed inverse of the Hessian. Assuming that the Hessian is
|
||||
* symmetric, the function will return transposed inverse, instead of the
|
||||
* true inverse.
|
||||
*
|
||||
* Note, that we use negative elementary vectors due because internal
|
||||
* implementation of the function hotstart_determineStepDirection requires
|
||||
* so.
|
||||
*
|
||||
* */
|
||||
|
||||
for( run3 = 0; run3 < NVMAX; run3++ )
|
||||
{
|
||||
/* Line wise loading of the corresponding (negative) elementary vector: */
|
||||
delta_g_cov[ run3 ] = -1.0;
|
||||
|
||||
/* Evaluation of the step: */
|
||||
returnvalue = qp->hotstart_determineStepDirection(
|
||||
FR_idx, FX_idx, AC_idx,
|
||||
delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
|
||||
Delta_bC_isZero, Delta_bB_isZero,
|
||||
delta_xFX, delta_xFR, delta_yAC, delta_yFX
|
||||
);
|
||||
if ( returnvalue != SUCCESSFUL_RETURN )
|
||||
{
|
||||
return returnvalue;
|
||||
}
|
||||
|
||||
/* Line wise storage of the QP reaction: */
|
||||
for( run1 = 0; run1 < nFR; run1++ )
|
||||
{
|
||||
run2 = FR_idx[ run1 ];
|
||||
|
||||
hessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ];
|
||||
}
|
||||
|
||||
for( run1 = 0; run1 < nFX; run1++ )
|
||||
{
|
||||
run2 = FX_idx[ run1 ];
|
||||
|
||||
hessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ];
|
||||
}
|
||||
|
||||
/* Prepare for the next iteration */
|
||||
delta_g_cov[ run3 ] = 0.0;
|
||||
}
|
||||
|
||||
// TODO: Perform the transpose of the inverse of the Hessian matrix
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
/*
|
||||
* g e t H e s s i a n I n v e r s e
|
||||
*/
|
||||
returnValue SolutionAnalysis::getHessianInverse( QProblemB* qp, real_t* hessianInverse )
|
||||
{
|
||||
returnValue returnvalue; /* the return value */
|
||||
BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
|
||||
|
||||
register int run1, run2, run3;
|
||||
|
||||
register int nFR, nFX;
|
||||
|
||||
/* Ask for the number of free and fixed variables, assumes that active set
|
||||
* is constant for the covariance evaluation */
|
||||
nFR = qp->getNFR( );
|
||||
nFX = qp->getNFX( );
|
||||
|
||||
/* Ask for the corresponding index arrays: */
|
||||
if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_HOTSTART_FAILED );
|
||||
|
||||
if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_HOTSTART_FAILED );
|
||||
|
||||
/* Initialization: */
|
||||
for( run1 = 0; run1 < NVMAX; run1++ )
|
||||
delta_g_cov[ run1 ] = 0.0;
|
||||
|
||||
for( run1 = 0; run1 < NVMAX; run1++ )
|
||||
delta_lb_cov[ run1 ] = 0.0;
|
||||
|
||||
for( run1 = 0; run1 < NVMAX; run1++ )
|
||||
delta_ub_cov[ run1 ] = 0.0;
|
||||
|
||||
/* The following loop solves the following:
|
||||
*
|
||||
* KKT * x =
|
||||
* [delta_g_cov', delta_lb_cov', delta_ub_cov']'
|
||||
*
|
||||
* for the first NVMAX (negative) elementary vectors in order to get
|
||||
* transposed inverse of the Hessian. Assuming that the Hessian is
|
||||
* symmetric, the function will return transposed inverse, instead of the
|
||||
* true inverse.
|
||||
*
|
||||
* Note, that we use negative elementary vectors due because internal
|
||||
* implementation of the function hotstart_determineStepDirection requires
|
||||
* so.
|
||||
*
|
||||
* */
|
||||
|
||||
for( run3 = 0; run3 < NVMAX; run3++ )
|
||||
{
|
||||
/* Line wise loading of the corresponding (negative) elementary vector: */
|
||||
delta_g_cov[ run3 ] = -1.0;
|
||||
|
||||
/* Evaluation of the step: */
|
||||
returnvalue = qp->hotstart_determineStepDirection(
|
||||
FR_idx, FX_idx,
|
||||
delta_g_cov, delta_lb_cov, delta_ub_cov,
|
||||
Delta_bB_isZero,
|
||||
delta_xFX, delta_xFR, delta_yFX
|
||||
);
|
||||
if ( returnvalue != SUCCESSFUL_RETURN )
|
||||
{
|
||||
return returnvalue;
|
||||
}
|
||||
|
||||
/* Line wise storage of the QP reaction: */
|
||||
for( run1 = 0; run1 < nFR; run1++ )
|
||||
{
|
||||
run2 = FR_idx[ run1 ];
|
||||
|
||||
hessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ];
|
||||
}
|
||||
|
||||
for( run1 = 0; run1 < nFX; run1++ )
|
||||
{
|
||||
run2 = FX_idx[ run1 ];
|
||||
|
||||
hessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ];
|
||||
}
|
||||
|
||||
/* Prepare for the next iteration */
|
||||
delta_g_cov[ run3 ] = 0.0;
|
||||
}
|
||||
|
||||
// TODO: Perform the transpose of the inverse of the Hessian matrix
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
/*
|
||||
* g e t V a r i a n c e C o v a r i a n c e
|
||||
*/
|
||||
|
||||
#if QPOASES_USE_OLD_VERSION
|
||||
|
||||
returnValue SolutionAnalysis::getVarianceCovariance( QProblem* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR )
|
||||
{
|
||||
int run1, run2, run3; /* simple run variables (for loops). */
|
||||
|
||||
returnValue returnvalue; /* the return value */
|
||||
BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */
|
||||
BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
|
||||
|
||||
/* ASK FOR THE NUMBER OF FREE AND FIXED VARIABLES:
|
||||
* (ASSUMES THAT ACTIVE SET IS CONSTANT FOR THE
|
||||
* VARIANCE-COVARIANCE EVALUATION)
|
||||
* ----------------------------------------------- */
|
||||
int nFR, nFX, nAC;
|
||||
|
||||
nFR = qp->getNFR( );
|
||||
nFX = qp->getNFX( );
|
||||
nAC = qp->getNAC( );
|
||||
|
||||
if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_HOTSTART_FAILED );
|
||||
|
||||
if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_HOTSTART_FAILED );
|
||||
|
||||
if ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_HOTSTART_FAILED );
|
||||
|
||||
/* SOME INITIALIZATIONS:
|
||||
* --------------------- */
|
||||
for( run1 = 0; run1 < KKT_DIM * KKT_DIM; run1++ )
|
||||
{
|
||||
K [run1] = 0.0;
|
||||
Primal_Dual_VAR[run1] = 0.0;
|
||||
}
|
||||
|
||||
/* ================================================================= */
|
||||
|
||||
/* FIRST MATRIX MULTIPLICATION (OBTAINS THE INTERMEDIATE RESULT
|
||||
* K := [ ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * g_b_bA_VAR ]^T )
|
||||
* THE EVALUATION OF THE INVERSE OF THE KKT-MATRIX OF THE QP
|
||||
* WITH RESPECT TO THE CURRENT ACTIVE SET
|
||||
* USES THE EXISTING CHOLESKY AND TQ-DECOMPOSITIONS. FOR DETAILS
|
||||
* cf. THE (protected) FUNCTION determineStepDirection. */
|
||||
|
||||
for( run3 = 0; run3 < KKT_DIM; run3++ )
|
||||
{
|
||||
|
||||
for( run1 = 0; run1 < NVMAX; run1++ )
|
||||
{
|
||||
delta_g_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+run1];
|
||||
delta_lb_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /* LINE-WISE LOADING OF THE INPUT */
|
||||
delta_ub_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /* VARIANCE-COVARIANCE */
|
||||
}
|
||||
for( run1 = 0; run1 < NCMAX; run1++ )
|
||||
{
|
||||
delta_lbA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1];
|
||||
delta_ubA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1];
|
||||
}
|
||||
|
||||
/* EVALUATION OF THE STEP:
|
||||
* ------------------------------------------------------------------------------ */
|
||||
|
||||
returnvalue = qp->hotstart_determineStepDirection(
|
||||
FR_idx, FX_idx, AC_idx,
|
||||
delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
|
||||
Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
|
||||
delta_yAC,delta_yFX );
|
||||
|
||||
/* ------------------------------------------------------------------------------ */
|
||||
|
||||
/* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
|
||||
* ------------------------------------------------------ */
|
||||
if ( returnvalue != SUCCESSFUL_RETURN )
|
||||
{
|
||||
return returnvalue;
|
||||
}
|
||||
|
||||
/* LINE WISE */
|
||||
/* STORAGE OF THE QP-REACTION */
|
||||
/* (uses the index list) */
|
||||
|
||||
for( run1=0; run1<nFR; run1++ )
|
||||
{
|
||||
run2 = FR_idx[run1];
|
||||
K[run3*KKT_DIM+run2] = delta_xFR[run1];
|
||||
}
|
||||
for( run1=0; run1<nFX; run1++ )
|
||||
{
|
||||
run2 = FX_idx[run1];
|
||||
K[run3*KKT_DIM+run2] = delta_xFX[run1];
|
||||
K[run3*KKT_DIM+NVMAX+run2] = delta_yFX[run1];
|
||||
}
|
||||
for( run1=0; run1<nAC; run1++ )
|
||||
{
|
||||
run2 = AC_idx[run1];
|
||||
K[run3*KKT_DIM+2*NVMAX+run2] = delta_yAC[run1];
|
||||
}
|
||||
}
|
||||
|
||||
/* ================================================================= */
|
||||
|
||||
/* SECOND MATRIX MULTIPLICATION (OBTAINS THE FINAL RESULT
|
||||
* Primal_Dual_VAR := ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * K )
|
||||
* THE APPLICATION OF THE KKT-INVERSE IS AGAIN REALIZED
|
||||
* BY USING THE PROTECTED FUNCTION
|
||||
* determineStepDirection */
|
||||
|
||||
for( run3 = 0; run3 < KKT_DIM; run3++ )
|
||||
{
|
||||
|
||||
for( run1 = 0; run1 < NVMAX; run1++ )
|
||||
{
|
||||
delta_g_cov [run1] = K[run3+ run1*KKT_DIM];
|
||||
delta_lb_cov [run1] = K[run3+(NVMAX+run1)*KKT_DIM]; /* ROW WISE LOADING OF THE */
|
||||
delta_ub_cov [run1] = K[run3+(NVMAX+run1)*KKT_DIM]; /* INTERMEDIATE RESULT K */
|
||||
}
|
||||
for( run1 = 0; run1 < NCMAX; run1++ )
|
||||
{
|
||||
delta_lbA_cov [run1] = K[run3+(2*NVMAX+run1)*KKT_DIM];
|
||||
delta_ubA_cov [run1] = K[run3+(2*NVMAX+run1)*KKT_DIM];
|
||||
}
|
||||
|
||||
/* EVALUATION OF THE STEP:
|
||||
* ------------------------------------------------------------------------------ */
|
||||
|
||||
returnvalue = qp->hotstart_determineStepDirection(
|
||||
FR_idx, FX_idx, AC_idx,
|
||||
delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
|
||||
Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
|
||||
delta_yAC,delta_yFX );
|
||||
|
||||
/* ------------------------------------------------------------------------------ */
|
||||
|
||||
/* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
|
||||
* ------------------------------------------------------ */
|
||||
if ( returnvalue != SUCCESSFUL_RETURN )
|
||||
{
|
||||
return returnvalue;
|
||||
}
|
||||
|
||||
/* ROW-WISE STORAGE */
|
||||
/* OF THE RESULT. */
|
||||
|
||||
for( run1=0; run1<nFR; run1++ )
|
||||
{
|
||||
run2 = FR_idx[run1];
|
||||
Primal_Dual_VAR[run3+run2*KKT_DIM] = delta_xFR[run1];
|
||||
}
|
||||
for( run1=0; run1<nFX; run1++ )
|
||||
{
|
||||
run2 = FX_idx[run1];
|
||||
Primal_Dual_VAR[run3+run2*KKT_DIM ] = delta_xFX[run1];
|
||||
Primal_Dual_VAR[run3+(NVMAX+run2)*KKT_DIM] = delta_yFX[run1];
|
||||
}
|
||||
for( run1=0; run1<nAC; run1++ )
|
||||
{
|
||||
run2 = AC_idx[run1];
|
||||
Primal_Dual_VAR[run3+(2*NVMAX+run2)*KKT_DIM] = delta_yAC[run1];
|
||||
}
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,342 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/Indexlist.cpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of the Indexlist class designed to manage index lists of
|
||||
* constraints and bounds within a QProblem_SubjectTo.
|
||||
*/
|
||||
|
||||
|
||||
#include <Indexlist.hpp>
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/*
|
||||
* I n d e x l i s t
|
||||
*/
|
||||
Indexlist::Indexlist( ) : length( 0 ),
|
||||
first( -1 ),
|
||||
last( -1 ),
|
||||
lastusedindex( -1 ),
|
||||
physicallength( INDEXLISTFACTOR*(NVMAX+NCMAX) )
|
||||
{
|
||||
int i;
|
||||
|
||||
for( i=0; i<physicallength; ++i )
|
||||
{
|
||||
number[i] = -1;
|
||||
next[i] = -1;
|
||||
previous[i] = -1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* I n d e x l i s t
|
||||
*/
|
||||
Indexlist::Indexlist( const Indexlist& rhs ) : length( rhs.length ),
|
||||
first( rhs.first ),
|
||||
last( rhs.last ),
|
||||
lastusedindex( rhs.lastusedindex ),
|
||||
physicallength( rhs.physicallength )
|
||||
{
|
||||
int i;
|
||||
|
||||
for( i=0; i<physicallength; ++i )
|
||||
{
|
||||
number[i] = rhs.number[i];
|
||||
next[i] = rhs.next[i];
|
||||
previous[i] = rhs.previous[i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* ~ I n d e x l i s t
|
||||
*/
|
||||
Indexlist::~Indexlist( )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* o p e r a t o r =
|
||||
*/
|
||||
Indexlist& Indexlist::operator=( const Indexlist& rhs )
|
||||
{
|
||||
int i;
|
||||
|
||||
if ( this != &rhs )
|
||||
{
|
||||
length = rhs.length;
|
||||
first = rhs.first;
|
||||
last = rhs.last;
|
||||
lastusedindex = rhs.lastusedindex;
|
||||
physicallength = rhs.physicallength;
|
||||
|
||||
for( i=0; i<physicallength; ++i )
|
||||
{
|
||||
number[i] = rhs.number[i];
|
||||
next[i] = rhs.next[i];
|
||||
previous[i] = rhs.previous[i];
|
||||
}
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i n i t
|
||||
*/
|
||||
returnValue Indexlist::init( )
|
||||
{
|
||||
int i;
|
||||
|
||||
length = 0;
|
||||
first = -1;
|
||||
last = -1;
|
||||
lastusedindex = -1;
|
||||
physicallength = INDEXLISTFACTOR*(NVMAX+NCMAX);
|
||||
|
||||
for( i=0; i<physicallength; ++i )
|
||||
{
|
||||
number[i] = -1;
|
||||
next[i] = -1;
|
||||
previous[i] = -1;
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N u m b e r A r r a y
|
||||
*/
|
||||
returnValue Indexlist::getNumberArray( int* const numberarray ) const
|
||||
{
|
||||
int i;
|
||||
int n = first;
|
||||
|
||||
/* Run trough indexlist and store numbers in numberarray. */
|
||||
for( i=0; i<length; ++i )
|
||||
{
|
||||
if ( ( n >= 0 ) && ( number[n] >= 0 ) )
|
||||
numberarray[i] = number[n];
|
||||
else
|
||||
return THROWERROR( RET_INDEXLIST_CORRUPTED );
|
||||
|
||||
n = next[n];
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t I n d e x
|
||||
*/
|
||||
int Indexlist::getIndex( int givennumber ) const
|
||||
{
|
||||
int i;
|
||||
int n = first;
|
||||
int index = -1; /* return -1 by default */
|
||||
|
||||
/* Run trough indexlist until number is found, if so return it index. */
|
||||
for ( i=0; i<length; ++i )
|
||||
{
|
||||
if ( number[n] == givennumber )
|
||||
{
|
||||
index = i;
|
||||
break;
|
||||
}
|
||||
|
||||
n = next[n];
|
||||
}
|
||||
|
||||
return index;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t P h y s i c a l I n d e x
|
||||
*/
|
||||
int Indexlist::getPhysicalIndex( int givennumber ) const
|
||||
{
|
||||
int i;
|
||||
int n = first;
|
||||
int index = -1; /* return -1 by default */
|
||||
|
||||
/* Run trough indexlist until number is found, if so return it physicalindex. */
|
||||
for ( i=0; i<length; ++i )
|
||||
{
|
||||
if ( number[n] == givennumber )
|
||||
{
|
||||
index = n;
|
||||
break;
|
||||
}
|
||||
|
||||
n = next[n];
|
||||
}
|
||||
|
||||
return index;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* a d d N u m b e r
|
||||
*/
|
||||
returnValue Indexlist::addNumber( int addnumber )
|
||||
{
|
||||
int i;
|
||||
|
||||
if ( lastusedindex+1 < physicallength )
|
||||
{
|
||||
/* If there is enough storage, add number to indexlist. */
|
||||
++lastusedindex;
|
||||
number[lastusedindex] = addnumber;
|
||||
next[lastusedindex] = 0;
|
||||
|
||||
if ( length == 0 )
|
||||
{
|
||||
first = lastusedindex;
|
||||
previous[lastusedindex] = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
next[last] = lastusedindex;
|
||||
previous[lastusedindex] = last;
|
||||
}
|
||||
|
||||
last = lastusedindex;
|
||||
++length;
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Rearrangement of index list necessary! */
|
||||
if ( length == physicallength )
|
||||
return THROWERROR( RET_INDEXLIST_EXCEEDS_MAX_LENGTH );
|
||||
else
|
||||
{
|
||||
int numberArray[NVMAX+NCMAX];
|
||||
getNumberArray( numberArray );
|
||||
|
||||
/* copy existing elements */
|
||||
for ( i=0; i<length; ++i )
|
||||
{
|
||||
number[i] = numberArray[i];
|
||||
next[i] = i+1;
|
||||
previous[i] = i-1;
|
||||
}
|
||||
|
||||
/* add new number at end of list */
|
||||
number[length] = addnumber;
|
||||
next[length] = -1;
|
||||
previous[length] = length-1;
|
||||
|
||||
/* and set remaining entries to empty */
|
||||
for ( i=length+1; i<physicallength; ++i )
|
||||
{
|
||||
number[i] = -1;
|
||||
next[i] = -1;
|
||||
previous[i] = -1;
|
||||
}
|
||||
|
||||
first = 0;
|
||||
last = length;
|
||||
lastusedindex = length;
|
||||
++length;
|
||||
|
||||
return THROWWARNING( RET_INDEXLIST_MUST_BE_REORDERD );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* r e m o v e N u m b e r
|
||||
*/
|
||||
returnValue Indexlist::removeNumber( int removenumber )
|
||||
{
|
||||
int i = getPhysicalIndex( removenumber );
|
||||
|
||||
/* nothing to be done if number is not contained in index set */
|
||||
if ( i < 0 )
|
||||
return SUCCESSFUL_RETURN;
|
||||
|
||||
int p = previous[i];
|
||||
int n = next[i];
|
||||
|
||||
if ( i == last )
|
||||
last = p;
|
||||
else
|
||||
previous[n] = p;
|
||||
|
||||
if ( i == first )
|
||||
first = n;
|
||||
else
|
||||
next[p] = n;
|
||||
|
||||
number[i] = -1;
|
||||
next[i] = -1;
|
||||
previous[i] = -1;
|
||||
--length;
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s w a p N u m b e r s
|
||||
*/
|
||||
returnValue Indexlist::swapNumbers( int number1, int number2 )
|
||||
{
|
||||
int index1 = getPhysicalIndex( number1 );
|
||||
int index2 = getPhysicalIndex( number2 );
|
||||
|
||||
/* consistency check */
|
||||
if ( ( index1 < 0 ) || ( index2 < 0 ) )
|
||||
return THROWERROR( RET_INDEXLIST_CORRUPTED );
|
||||
|
||||
int tmp = number[index1];
|
||||
number[index1] = number[index2];
|
||||
number[index2] = tmp;
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,85 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/Indexlist.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of inlined member functions of the Indexlist class designed
|
||||
* to manage index lists of constraints and bounds within a QProblem_SubjectTo.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* g e t N u m b e r
|
||||
*/
|
||||
inline int Indexlist::getNumber( int physicalindex ) const
|
||||
{
|
||||
/* consistency check */
|
||||
if ( ( physicalindex < 0 ) || ( physicalindex > length ) )
|
||||
return -RET_INDEXLIST_OUTOFBOUNDS;
|
||||
|
||||
return number[physicalindex];
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t L e n g t h
|
||||
*/
|
||||
inline int Indexlist::getLength( )
|
||||
{
|
||||
return length;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t L a s t N u m b e r
|
||||
*/
|
||||
inline int Indexlist::getLastNumber( ) const
|
||||
{
|
||||
return number[last];
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t L a s t N u m b e r
|
||||
*/
|
||||
inline BooleanType Indexlist::isMember( int _number ) const
|
||||
{
|
||||
if ( getIndex( _number ) >= 0 )
|
||||
return BT_TRUE;
|
||||
else
|
||||
return BT_FALSE;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,529 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/MessageHandling.cpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of the MessageHandling class including global return values.
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include <MessageHandling.hpp>
|
||||
#include <Utils.hpp>
|
||||
|
||||
|
||||
|
||||
|
||||
/** Defines pairs of global return values and messages. */
|
||||
MessageHandling::ReturnValueList returnValueList[] =
|
||||
{
|
||||
/* miscellaneous */
|
||||
{ SUCCESSFUL_RETURN, "Successful return", VS_VISIBLE },
|
||||
{ RET_DIV_BY_ZERO, "Division by zero", VS_VISIBLE },
|
||||
{ RET_INDEX_OUT_OF_BOUNDS, "Index out of bounds", VS_VISIBLE },
|
||||
{ RET_INVALID_ARGUMENTS, "At least one of the arguments is invalid", VS_VISIBLE },
|
||||
{ RET_ERROR_UNDEFINED, "Error number undefined", VS_VISIBLE },
|
||||
{ RET_WARNING_UNDEFINED, "Warning number undefined", VS_VISIBLE },
|
||||
{ RET_INFO_UNDEFINED, "Info number undefined", VS_VISIBLE },
|
||||
{ RET_EWI_UNDEFINED, "Error/warning/info number undefined", VS_VISIBLE },
|
||||
{ RET_AVAILABLE_WITH_LINUX_ONLY, "This function is available under Linux only", VS_HIDDEN },
|
||||
{ RET_UNKNOWN_BUG, "The error occured is not yet known", VS_VISIBLE },
|
||||
{ RET_PRINTLEVEL_CHANGED, "Print level changed", VS_VISIBLE },
|
||||
{ RET_NOT_YET_IMPLEMENTED, "Requested function is not yet implemented.", VS_VISIBLE },
|
||||
/* Indexlist */
|
||||
{ RET_INDEXLIST_MUST_BE_REORDERD, "Index list has to be reordered", VS_VISIBLE },
|
||||
{ RET_INDEXLIST_EXCEEDS_MAX_LENGTH, "Index list exceeds its maximal physical length", VS_VISIBLE },
|
||||
{ RET_INDEXLIST_CORRUPTED, "Index list corrupted", VS_VISIBLE },
|
||||
{ RET_INDEXLIST_OUTOFBOUNDS, "Physical index is out of bounds", VS_VISIBLE },
|
||||
{ RET_INDEXLIST_ADD_FAILED, "Adding indices from another index set failed", VS_VISIBLE },
|
||||
{ RET_INDEXLIST_INTERSECT_FAILED, "Intersection with another index set failed", VS_VISIBLE },
|
||||
/* SubjectTo / Bounds / Constraints */
|
||||
{ RET_INDEX_ALREADY_OF_DESIRED_STATUS, "Index is already of desired status", VS_VISIBLE },
|
||||
{ RET_SWAPINDEX_FAILED, "Cannot swap between different indexsets", VS_VISIBLE },
|
||||
{ RET_ADDINDEX_FAILED, "Adding index to index set failed", VS_VISIBLE },
|
||||
{ RET_NOTHING_TO_DO, "Nothing to do", VS_VISIBLE },
|
||||
{ RET_SETUP_BOUND_FAILED, "Setting up bound index failed", VS_VISIBLE },
|
||||
{ RET_SETUP_CONSTRAINT_FAILED, "Setting up constraint index failed", VS_VISIBLE },
|
||||
{ RET_MOVING_BOUND_FAILED, "Moving bound between index sets failed", VS_VISIBLE },
|
||||
{ RET_MOVING_CONSTRAINT_FAILED, "Moving constraint between index sets failed", VS_VISIBLE },
|
||||
/* QProblem */
|
||||
{ RET_QP_ALREADY_INITIALISED, "QProblem has already been initialised", VS_VISIBLE },
|
||||
{ RET_NO_INIT_WITH_STANDARD_SOLVER, "Initialisation via extern QP solver is not yet implemented", VS_VISIBLE },
|
||||
{ RET_RESET_FAILED, "Reset failed", VS_VISIBLE },
|
||||
{ RET_INIT_FAILED, "Initialisation failed", VS_VISIBLE },
|
||||
{ RET_INIT_FAILED_TQ, "Initialisation failed due to TQ factorisation", VS_VISIBLE },
|
||||
{ RET_INIT_FAILED_CHOLESKY, "Initialisation failed due to Cholesky decomposition", VS_VISIBLE },
|
||||
{ RET_INIT_FAILED_HOTSTART, "Initialisation failed! QP could not be solved!", VS_VISIBLE },
|
||||
{ RET_INIT_FAILED_INFEASIBILITY, "Initial QP could not be solved due to infeasibility!", VS_VISIBLE },
|
||||
{ RET_INIT_FAILED_UNBOUNDEDNESS, "Initial QP could not be solved due to unboundedness!", VS_VISIBLE },
|
||||
{ RET_INIT_SUCCESSFUL, "Initialisation done", VS_VISIBLE },
|
||||
{ RET_OBTAINING_WORKINGSET_FAILED, "Failed to obtain working set for auxiliary QP", VS_VISIBLE },
|
||||
{ RET_SETUP_WORKINGSET_FAILED, "Failed to setup working set for auxiliary QP", VS_VISIBLE },
|
||||
{ RET_SETUP_AUXILIARYQP_FAILED, "Failed to setup auxiliary QP for initialised homotopy", VS_VISIBLE },
|
||||
{ RET_NO_EXTERN_SOLVER, "No extern QP solver available", VS_VISIBLE },
|
||||
{ RET_QP_UNBOUNDED, "QP is unbounded", VS_VISIBLE },
|
||||
{ RET_QP_INFEASIBLE, "QP is infeasible", VS_VISIBLE },
|
||||
{ RET_QP_NOT_SOLVED, "Problems occured while solving QP with standard solver", VS_VISIBLE },
|
||||
{ RET_QP_SOLVED, "QP successfully solved", VS_VISIBLE },
|
||||
{ RET_UNABLE_TO_SOLVE_QP, "Problems occured while solving QP", VS_VISIBLE },
|
||||
{ RET_INITIALISATION_STARTED, "Starting problem initialisation...", VS_VISIBLE },
|
||||
{ RET_HOTSTART_FAILED, "Unable to perform homotopy due to internal error", VS_VISIBLE },
|
||||
{ RET_HOTSTART_FAILED_TO_INIT, "Unable to initialise problem", VS_VISIBLE },
|
||||
{ RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED, "Unable to perform homotopy as previous QP is not solved", VS_VISIBLE },
|
||||
{ RET_ITERATION_STARTED, "Iteration", VS_VISIBLE },
|
||||
{ RET_SHIFT_DETERMINATION_FAILED, "Determination of shift of the QP data failed", VS_VISIBLE },
|
||||
{ RET_STEPDIRECTION_DETERMINATION_FAILED, "Determination of step direction failed", VS_VISIBLE },
|
||||
{ RET_STEPLENGTH_DETERMINATION_FAILED, "Determination of step direction failed", VS_VISIBLE },
|
||||
{ RET_OPTIMAL_SOLUTION_FOUND, "Optimal solution of neighbouring QP found", VS_VISIBLE },
|
||||
{ RET_HOMOTOPY_STEP_FAILED, "Unable to perform homotopy step", VS_VISIBLE },
|
||||
{ RET_HOTSTART_STOPPED_INFEASIBILITY, "Premature homotopy termination because QP is infeasible", VS_VISIBLE },
|
||||
{ RET_HOTSTART_STOPPED_UNBOUNDEDNESS, "Premature homotopy termination because QP is unbounded", VS_VISIBLE },
|
||||
{ RET_WORKINGSET_UPDATE_FAILED, "Unable to update working sets according to initial guesses", VS_VISIBLE },
|
||||
{ RET_MAX_NWSR_REACHED, "Maximum number of working set recalculations performed", VS_VISIBLE },
|
||||
{ RET_CONSTRAINTS_NOT_SPECIFIED, "Problem does comprise constraints! You have to specify new constraints' bounds", VS_VISIBLE },
|
||||
{ RET_INVALID_FACTORISATION_FLAG, "Invalid factorisation flag", VS_VISIBLE },
|
||||
{ RET_UNABLE_TO_SAVE_QPDATA, "Unable to save QP data", VS_VISIBLE },
|
||||
{ RET_STEPDIRECTION_FAILED_TQ, "Abnormal termination due to TQ factorisation", VS_VISIBLE },
|
||||
{ RET_STEPDIRECTION_FAILED_CHOLESKY, "Abnormal termination due to Cholesky factorisation", VS_VISIBLE },
|
||||
{ RET_CYCLING_DETECTED, "Cycling detected", VS_VISIBLE },
|
||||
{ RET_CYCLING_NOT_RESOLVED, "Cycling cannot be resolved, QP is probably infeasible", VS_VISIBLE },
|
||||
{ RET_CYCLING_RESOLVED, "Cycling probably resolved", VS_VISIBLE },
|
||||
{ RET_STEPSIZE, "", VS_VISIBLE },
|
||||
{ RET_STEPSIZE_NONPOSITIVE, "", VS_VISIBLE },
|
||||
{ RET_SETUPSUBJECTTOTYPE_FAILED, "Setup of SubjectToTypes failed", VS_VISIBLE },
|
||||
{ RET_ADDCONSTRAINT_FAILED, "Addition of constraint to working set failed", VS_VISIBLE },
|
||||
{ RET_ADDCONSTRAINT_FAILED_INFEASIBILITY, "Addition of constraint to working set failed", VS_VISIBLE },
|
||||
{ RET_ADDBOUND_FAILED, "Addition of bound to working set failed", VS_VISIBLE },
|
||||
{ RET_ADDBOUND_FAILED_INFEASIBILITY, "Addition of bound to working set failed", VS_VISIBLE },
|
||||
{ RET_REMOVECONSTRAINT_FAILED, "Removal of constraint from working set failed", VS_VISIBLE },
|
||||
{ RET_REMOVEBOUND_FAILED, "Removal of bound from working set failed", VS_VISIBLE },
|
||||
{ RET_REMOVE_FROM_ACTIVESET, "Removing from active set:", VS_VISIBLE },
|
||||
{ RET_ADD_TO_ACTIVESET, "Adding to active set:", VS_VISIBLE },
|
||||
{ RET_REMOVE_FROM_ACTIVESET_FAILED, "Removing from active set failed", VS_VISIBLE },
|
||||
{ RET_ADD_TO_ACTIVESET_FAILED, "Adding to active set failed", VS_VISIBLE },
|
||||
{ RET_CONSTRAINT_ALREADY_ACTIVE, "Constraint is already active", VS_VISIBLE },
|
||||
{ RET_ALL_CONSTRAINTS_ACTIVE, "All constraints are active, no further constraint can be added", VS_VISIBLE },
|
||||
{ RET_LINEARLY_DEPENDENT, "New bound/constraint is linearly dependent", VS_VISIBLE },
|
||||
{ RET_LINEARLY_INDEPENDENT, "New bound/constraint is linearly independent", VS_VISIBLE },
|
||||
{ RET_LI_RESOLVED, "Linear independence of active contraint matrix successfully resolved", VS_VISIBLE },
|
||||
{ RET_ENSURELI_FAILED, "Failed to ensure linear indepence of active contraint matrix", VS_VISIBLE },
|
||||
{ RET_ENSURELI_FAILED_TQ, "Abnormal termination due to TQ factorisation", VS_VISIBLE },
|
||||
{ RET_ENSURELI_FAILED_NOINDEX, "No index found, QP is probably infeasible", VS_VISIBLE },
|
||||
{ RET_ENSURELI_FAILED_CYCLING, "Cycling detected, QP is probably infeasible", VS_VISIBLE },
|
||||
{ RET_BOUND_ALREADY_ACTIVE, "Bound is already active", VS_VISIBLE },
|
||||
{ RET_ALL_BOUNDS_ACTIVE, "All bounds are active, no further bound can be added", VS_VISIBLE },
|
||||
{ RET_CONSTRAINT_NOT_ACTIVE, "Constraint is not active", VS_VISIBLE },
|
||||
{ RET_BOUND_NOT_ACTIVE, "Bound is not active", VS_VISIBLE },
|
||||
{ RET_HESSIAN_NOT_SPD, "Projected Hessian matrix not positive definite", VS_VISIBLE },
|
||||
{ RET_MATRIX_SHIFT_FAILED, "Unable to update matrices or to transform vectors", VS_VISIBLE },
|
||||
{ RET_MATRIX_FACTORISATION_FAILED, "Unable to calculate new matrix factorisations", VS_VISIBLE },
|
||||
{ RET_PRINT_ITERATION_FAILED, "Unable to print information on current iteration", VS_VISIBLE },
|
||||
{ RET_NO_GLOBAL_MESSAGE_OUTPUTFILE, "No global message output file initialised", VS_VISIBLE },
|
||||
/* Utils */
|
||||
{ RET_UNABLE_TO_OPEN_FILE, "Unable to open file", VS_VISIBLE },
|
||||
{ RET_UNABLE_TO_WRITE_FILE, "Unable to write into file", VS_VISIBLE },
|
||||
{ RET_UNABLE_TO_READ_FILE, "Unable to read from file", VS_VISIBLE },
|
||||
{ RET_FILEDATA_INCONSISTENT, "File contains inconsistent data", VS_VISIBLE },
|
||||
/* SolutionAnalysis */
|
||||
{ RET_NO_SOLUTION, "QP solution does not satisfy KKT optimality conditions", VS_VISIBLE },
|
||||
{ RET_INACCURATE_SOLUTION, "KKT optimality conditions not satisfied to sufficient accuracy", VS_VISIBLE },
|
||||
{ TERMINAL_LIST_ELEMENT, "", VS_HIDDEN } /* IMPORTANT: Terminal list element! */
|
||||
};
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/*
|
||||
* M e s s a g e H a n d l i n g
|
||||
*/
|
||||
MessageHandling::MessageHandling( ) : errorVisibility( VS_VISIBLE ),
|
||||
warningVisibility( VS_VISIBLE ),
|
||||
infoVisibility( VS_VISIBLE ),
|
||||
outputFile( myStdout ),
|
||||
errorCount( 0 )
|
||||
{
|
||||
}
|
||||
|
||||
/*
|
||||
* M e s s a g e H a n d l i n g
|
||||
*/
|
||||
MessageHandling::MessageHandling( myFILE* _outputFile ) :
|
||||
errorVisibility( VS_VISIBLE ),
|
||||
warningVisibility( VS_VISIBLE ),
|
||||
infoVisibility( VS_VISIBLE ),
|
||||
outputFile( _outputFile ),
|
||||
errorCount( 0 )
|
||||
{
|
||||
}
|
||||
|
||||
/*
|
||||
* M e s s a g e H a n d l i n g
|
||||
*/
|
||||
MessageHandling::MessageHandling( VisibilityStatus _errorVisibility,
|
||||
VisibilityStatus _warningVisibility,
|
||||
VisibilityStatus _infoVisibility
|
||||
) :
|
||||
errorVisibility( _errorVisibility ),
|
||||
warningVisibility( _warningVisibility ),
|
||||
infoVisibility( _infoVisibility ),
|
||||
outputFile( myStderr ),
|
||||
errorCount( 0 )
|
||||
{
|
||||
}
|
||||
|
||||
/*
|
||||
* M e s s a g e H a n d l i n g
|
||||
*/
|
||||
MessageHandling::MessageHandling( myFILE* _outputFile,
|
||||
VisibilityStatus _errorVisibility,
|
||||
VisibilityStatus _warningVisibility,
|
||||
VisibilityStatus _infoVisibility
|
||||
) :
|
||||
errorVisibility( _errorVisibility ),
|
||||
warningVisibility( _warningVisibility ),
|
||||
infoVisibility( _infoVisibility ),
|
||||
outputFile( _outputFile ),
|
||||
errorCount( 0 )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* M e s s a g e H a n d l i n g
|
||||
*/
|
||||
MessageHandling::MessageHandling( const MessageHandling& rhs ) :
|
||||
errorVisibility( rhs.errorVisibility ),
|
||||
warningVisibility( rhs.warningVisibility ),
|
||||
infoVisibility( rhs.infoVisibility ),
|
||||
outputFile( rhs.outputFile ),
|
||||
errorCount( rhs.errorCount )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* ~ M e s s a g e H a n d l i n g
|
||||
*/
|
||||
MessageHandling::~MessageHandling( )
|
||||
{
|
||||
#ifdef PC_DEBUG
|
||||
if ( outputFile != 0 )
|
||||
fclose( outputFile );
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* o p e r a t o r =
|
||||
*/
|
||||
MessageHandling& MessageHandling::operator=( const MessageHandling& rhs )
|
||||
{
|
||||
if ( this != &rhs )
|
||||
{
|
||||
errorVisibility = rhs.errorVisibility;
|
||||
warningVisibility = rhs.warningVisibility;
|
||||
infoVisibility = rhs.infoVisibility;
|
||||
outputFile = rhs.outputFile;
|
||||
errorCount = rhs.errorCount;
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* t h r o w E r r o r
|
||||
*/
|
||||
returnValue MessageHandling::throwError(
|
||||
returnValue Enumber,
|
||||
const char* additionaltext,
|
||||
const char* functionname,
|
||||
const char* filename,
|
||||
const unsigned long linenumber,
|
||||
VisibilityStatus localVisibilityStatus
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( Enumber <= SUCCESSFUL_RETURN )
|
||||
return throwError( RET_ERROR_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
|
||||
/* Call to common throwMessage function if error shall be displayed. */
|
||||
if ( errorVisibility == VS_VISIBLE )
|
||||
return throwMessage( Enumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"ERROR" );
|
||||
else
|
||||
return Enumber;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* t h r o w W a r n i n g
|
||||
*/
|
||||
returnValue MessageHandling::throwWarning(
|
||||
returnValue Wnumber,
|
||||
const char* additionaltext,
|
||||
const char* functionname,
|
||||
const char* filename,
|
||||
const unsigned long linenumber,
|
||||
VisibilityStatus localVisibilityStatus
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( Wnumber <= SUCCESSFUL_RETURN )
|
||||
return throwError( RET_WARNING_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
|
||||
/* Call to common throwMessage function if warning shall be displayed. */
|
||||
if ( warningVisibility == VS_VISIBLE )
|
||||
return throwMessage( Wnumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"WARNING" );
|
||||
else
|
||||
return Wnumber;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* t h r o w I n f o
|
||||
*/
|
||||
returnValue MessageHandling::throwInfo(
|
||||
returnValue Inumber,
|
||||
const char* additionaltext,
|
||||
const char* functionname,
|
||||
const char* filename,
|
||||
const unsigned long linenumber,
|
||||
VisibilityStatus localVisibilityStatus
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( Inumber < SUCCESSFUL_RETURN )
|
||||
return throwError( RET_INFO_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
|
||||
/* Call to common throwMessage function if info shall be displayed. */
|
||||
if ( infoVisibility == VS_VISIBLE )
|
||||
return throwMessage( Inumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"INFO" );
|
||||
else
|
||||
return Inumber;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* r e s e t
|
||||
*/
|
||||
returnValue MessageHandling::reset( )
|
||||
{
|
||||
setErrorVisibilityStatus( VS_VISIBLE );
|
||||
setWarningVisibilityStatus( VS_VISIBLE );
|
||||
setInfoVisibilityStatus( VS_VISIBLE );
|
||||
|
||||
setOutputFile( myStderr );
|
||||
setErrorCount( 0 );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* l i s t A l l M e s s a g e s
|
||||
*/
|
||||
returnValue MessageHandling::listAllMessages( )
|
||||
{
|
||||
#ifdef PC_DEBUG
|
||||
int keypos = 0;
|
||||
char myPrintfString[160];
|
||||
|
||||
/* Run through whole returnValueList and print each item. */
|
||||
while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
|
||||
{
|
||||
sprintf( myPrintfString," %d - %s \n",keypos,returnValueList[keypos].data );
|
||||
myPrintf( myPrintfString );
|
||||
|
||||
++keypos;
|
||||
}
|
||||
#endif
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P R O T E C T E D *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
#ifdef PC_DEBUG /* Re-define throwMessage function for embedded code! */
|
||||
|
||||
/*
|
||||
* t h r o w M e s s a g e
|
||||
*/
|
||||
returnValue MessageHandling::throwMessage(
|
||||
returnValue RETnumber,
|
||||
const char* additionaltext,
|
||||
const char* functionname,
|
||||
const char* filename,
|
||||
const unsigned long linenumber,
|
||||
VisibilityStatus localVisibilityStatus,
|
||||
const char* RETstring
|
||||
)
|
||||
{
|
||||
int i;
|
||||
|
||||
int keypos = 0;
|
||||
char myPrintfString[160];
|
||||
|
||||
/* 1) Determine number of whitespace for output. */
|
||||
char whitespaces[41];
|
||||
int numberOfWhitespaces = (errorCount-1)*2;
|
||||
|
||||
if ( numberOfWhitespaces < 0 )
|
||||
numberOfWhitespaces = 0;
|
||||
|
||||
if ( numberOfWhitespaces > 40 )
|
||||
numberOfWhitespaces = 40;
|
||||
|
||||
for( i=0; i<numberOfWhitespaces; ++i )
|
||||
whitespaces[i] = ' ';
|
||||
whitespaces[numberOfWhitespaces] = '\0';
|
||||
|
||||
/* 2) Find error/warning/info in list. */
|
||||
while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
|
||||
{
|
||||
if ( returnValueList[keypos].key == RETnumber )
|
||||
break;
|
||||
else
|
||||
++keypos;
|
||||
}
|
||||
|
||||
if ( returnValueList[keypos].key == TERMINAL_LIST_ELEMENT )
|
||||
{
|
||||
throwError( RET_EWI_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
return RETnumber;
|
||||
}
|
||||
|
||||
/* 3) Print error/warning/info. */
|
||||
if ( ( returnValueList[keypos].globalVisibilityStatus == VS_VISIBLE ) && ( localVisibilityStatus == VS_VISIBLE ) )
|
||||
{
|
||||
if ( errorCount > 0 )
|
||||
{
|
||||
sprintf( myPrintfString,"%s->", whitespaces );
|
||||
myPrintf( myPrintfString );
|
||||
}
|
||||
|
||||
if ( additionaltext == 0 )
|
||||
{
|
||||
sprintf( myPrintfString,"%s (%s, %s:%d): \t%s\n",
|
||||
RETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data
|
||||
);
|
||||
myPrintf( myPrintfString );
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf( myPrintfString,"%s (%s, %s:%d): \t%s %s\n",
|
||||
RETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data,additionaltext
|
||||
);
|
||||
myPrintf( myPrintfString );
|
||||
}
|
||||
|
||||
/* take care of proper indention for subsequent error messages */
|
||||
if ( RETstring[0] == 'E' )
|
||||
{
|
||||
++errorCount;
|
||||
}
|
||||
else
|
||||
{
|
||||
if ( errorCount > 0 )
|
||||
myPrintf( "\n" );
|
||||
errorCount = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return RETnumber;
|
||||
}
|
||||
|
||||
#else /* = PC_DEBUG not defined */
|
||||
|
||||
/*
|
||||
* t h r o w M e s s a g e
|
||||
*/
|
||||
returnValue MessageHandling::throwMessage(
|
||||
returnValue RETnumber,
|
||||
const char* additionaltext,
|
||||
const char* functionname,
|
||||
const char* filename,
|
||||
const unsigned long linenumber,
|
||||
VisibilityStatus localVisibilityStatus,
|
||||
const char* RETstring
|
||||
)
|
||||
{
|
||||
/* DUMMY CODE FOR PRETENDING USE OF ARGUMENTS
|
||||
* FOR SUPPRESSING COMPILER WARNINGS! */
|
||||
int i = 0;
|
||||
if ( additionaltext == 0 ) i++;
|
||||
if ( functionname == 0 ) i++;
|
||||
if ( filename == 0 ) i++;
|
||||
if ( linenumber == 0 ) i++;
|
||||
if ( localVisibilityStatus == VS_VISIBLE ) i++;
|
||||
if ( RETstring == 0 ) i++;
|
||||
/* END OF DUMMY CODE */
|
||||
|
||||
return RETnumber;
|
||||
}
|
||||
|
||||
#endif /* PC_DEBUG */
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* G L O B A L M E S S A G E H A N D L E R *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/** Global message handler for all qpOASES modules.*/
|
||||
MessageHandling globalMessageHandler( myStderr,VS_VISIBLE,VS_VISIBLE,VS_VISIBLE );
|
||||
|
||||
|
||||
/*
|
||||
* g e t G l o b a l M e s s a g e H a n d l e r
|
||||
*/
|
||||
MessageHandling* getGlobalMessageHandler( )
|
||||
{
|
||||
return &globalMessageHandler;
|
||||
}
|
||||
|
||||
const char* MessageHandling::getErrorString(int error)
|
||||
{
|
||||
return returnValueList[ error ].data;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,137 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/MessageHandling.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of inlined member functions of the MessageHandling class.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* g e t E r r o r V i s i b i l i t y S t a t u s
|
||||
*/
|
||||
inline VisibilityStatus MessageHandling::getErrorVisibilityStatus( ) const
|
||||
{
|
||||
return errorVisibility;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t W a r n i n g V i s i b i l i t y S t a t u s
|
||||
*/
|
||||
inline VisibilityStatus MessageHandling::getWarningVisibilityStatus( ) const
|
||||
{
|
||||
return warningVisibility;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t I n f o V i s i b i l i t y S t a t u s
|
||||
*/
|
||||
inline VisibilityStatus MessageHandling::getInfoVisibilityStatus( ) const
|
||||
{
|
||||
return infoVisibility;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t O u t p u t F i l e
|
||||
*/
|
||||
inline myFILE* MessageHandling::getOutputFile( ) const
|
||||
{
|
||||
return outputFile;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t E r r o r C o u n t
|
||||
*/
|
||||
inline int MessageHandling::getErrorCount( ) const
|
||||
{
|
||||
return errorCount;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t E r r o r V i s i b i l i t y S t a t u s
|
||||
*/
|
||||
inline void MessageHandling::setErrorVisibilityStatus( VisibilityStatus _errorVisibility )
|
||||
{
|
||||
errorVisibility = _errorVisibility;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t W a r n i n g V i s i b i l i t y S t a t u s
|
||||
*/
|
||||
inline void MessageHandling::setWarningVisibilityStatus( VisibilityStatus _warningVisibility )
|
||||
{
|
||||
warningVisibility = _warningVisibility;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t I n f o V i s i b i l i t y S t a t u s
|
||||
*/
|
||||
inline void MessageHandling::setInfoVisibilityStatus( VisibilityStatus _infoVisibility )
|
||||
{
|
||||
infoVisibility = _infoVisibility;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t O u t p u t F i l e
|
||||
*/
|
||||
inline void MessageHandling::setOutputFile( myFILE* _outputFile )
|
||||
{
|
||||
outputFile = _outputFile;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t E r r o r C o u n t
|
||||
*/
|
||||
inline returnValue MessageHandling::setErrorCount( int _errorCount )
|
||||
{
|
||||
if ( _errorCount >= 0 )
|
||||
{
|
||||
errorCount = _errorCount;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return RET_INVALID_ARGUMENTS;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,299 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/QProblem.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of inlined member functions of the QProblem class which
|
||||
* is able to use the newly developed online active set strategy for
|
||||
* parametric quadratic programming.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* g e t A
|
||||
*/
|
||||
inline returnValue QProblem::getA( real_t* const _A ) const
|
||||
{
|
||||
int i;
|
||||
|
||||
for ( i=0; i<getNV( )*getNC( ); ++i )
|
||||
_A[i] = A[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t A
|
||||
*/
|
||||
inline returnValue QProblem::getA( int number, real_t* const row ) const
|
||||
{
|
||||
int nV = getNV( );
|
||||
|
||||
if ( ( number >= 0 ) && ( number < getNC( ) ) )
|
||||
{
|
||||
for ( int i=0; i<nV; ++i )
|
||||
row[i] = A[number*NVMAX + i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t L B A
|
||||
*/
|
||||
inline returnValue QProblem::getLBA( real_t* const _lbA ) const
|
||||
{
|
||||
int i;
|
||||
|
||||
for ( i=0; i<getNC( ); ++i )
|
||||
_lbA[i] = lbA[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t L B A
|
||||
*/
|
||||
inline returnValue QProblem::getLBA( int number, real_t& value ) const
|
||||
{
|
||||
if ( ( number >= 0 ) && ( number < getNC( ) ) )
|
||||
{
|
||||
value = lbA[number];
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t U B A
|
||||
*/
|
||||
inline returnValue QProblem::getUBA( real_t* const _ubA ) const
|
||||
{
|
||||
int i;
|
||||
|
||||
for ( i=0; i<getNC( ); ++i )
|
||||
_ubA[i] = ubA[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t U B A
|
||||
*/
|
||||
inline returnValue QProblem::getUBA( int number, real_t& value ) const
|
||||
{
|
||||
if ( ( number >= 0 ) && ( number < getNC( ) ) )
|
||||
{
|
||||
value = ubA[number];
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t C o n s t r a i n t s
|
||||
*/
|
||||
inline returnValue QProblem::getConstraints( Constraints* const _constraints ) const
|
||||
{
|
||||
*_constraints = constraints;
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* g e t N C
|
||||
*/
|
||||
inline int QProblem::getNC( ) const
|
||||
{
|
||||
return constraints.getNC( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N E C
|
||||
*/
|
||||
inline int QProblem::getNEC( ) const
|
||||
{
|
||||
return constraints.getNEC( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N A C
|
||||
*/
|
||||
inline int QProblem::getNAC( )
|
||||
{
|
||||
return constraints.getNAC( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N I A C
|
||||
*/
|
||||
inline int QProblem::getNIAC( )
|
||||
{
|
||||
return constraints.getNIAC( );
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P R O T E C T E D *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/*
|
||||
* s e t A
|
||||
*/
|
||||
inline returnValue QProblem::setA( const real_t* const A_new )
|
||||
{
|
||||
int i, j;
|
||||
int nV = getNV( );
|
||||
int nC = getNC( );
|
||||
|
||||
/* Set constraint matrix AND update member AX. */
|
||||
for( j=0; j<nC; ++j )
|
||||
{
|
||||
Ax[j] = 0.0;
|
||||
|
||||
for( i=0; i<nV; ++i )
|
||||
{
|
||||
A[j*NVMAX + i] = A_new[j*nV + i];
|
||||
Ax[j] += A[j*NVMAX + i] * x[i];
|
||||
}
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t A
|
||||
*/
|
||||
inline returnValue QProblem::setA( int number, const real_t* const row )
|
||||
{
|
||||
int i;
|
||||
int nV = getNV( );
|
||||
|
||||
/* Set constraint matrix AND update member AX. */
|
||||
if ( ( number >= 0 ) && ( number < getNC( ) ) )
|
||||
{
|
||||
Ax[number] = 0.0;
|
||||
|
||||
for( i=0; i<nV; ++i )
|
||||
{
|
||||
A[number*NVMAX + i] = row[i];
|
||||
Ax[number] += A[number*NVMAX + i] * x[i];
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t L B A
|
||||
*/
|
||||
inline returnValue QProblem::setLBA( const real_t* const lbA_new )
|
||||
{
|
||||
int i;
|
||||
int nC = getNC();
|
||||
|
||||
for( i=0; i<nC; ++i )
|
||||
lbA[i] = lbA_new[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t L B A
|
||||
*/
|
||||
inline returnValue QProblem::setLBA( int number, real_t value )
|
||||
{
|
||||
if ( ( number >= 0 ) && ( number < getNC( ) ) )
|
||||
{
|
||||
lbA[number] = value;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t U B A
|
||||
*/
|
||||
inline returnValue QProblem::setUBA( const real_t* const ubA_new )
|
||||
{
|
||||
int i;
|
||||
int nC = getNC();
|
||||
|
||||
for( i=0; i<nC; ++i )
|
||||
ubA[i] = ubA_new[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t U B A
|
||||
*/
|
||||
inline returnValue QProblem::setUBA( int number, real_t value )
|
||||
{
|
||||
if ( ( number >= 0 ) && ( number < getNC( ) ) )
|
||||
{
|
||||
ubA[number] = value;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,425 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/QProblemB.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of inlined member functions of the QProblemB class which
|
||||
* is able to use the newly developed online active set strategy for
|
||||
* parametric quadratic programming.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include <math.h>
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* g e t H
|
||||
*/
|
||||
inline returnValue QProblemB::getH( real_t* const _H ) const
|
||||
{
|
||||
int i;
|
||||
|
||||
for ( i=0; i<getNV( )*getNV( ); ++i )
|
||||
_H[i] = H[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t G
|
||||
*/
|
||||
inline returnValue QProblemB::getG( real_t* const _g ) const
|
||||
{
|
||||
int i;
|
||||
|
||||
for ( i=0; i<getNV( ); ++i )
|
||||
_g[i] = g[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t L B
|
||||
*/
|
||||
inline returnValue QProblemB::getLB( real_t* const _lb ) const
|
||||
{
|
||||
int i;
|
||||
|
||||
for ( i=0; i<getNV( ); ++i )
|
||||
_lb[i] = lb[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t L B
|
||||
*/
|
||||
inline returnValue QProblemB::getLB( int number, real_t& value ) const
|
||||
{
|
||||
if ( ( number >= 0 ) && ( number < getNV( ) ) )
|
||||
{
|
||||
value = lb[number];
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
{
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t U B
|
||||
*/
|
||||
inline returnValue QProblemB::getUB( real_t* const _ub ) const
|
||||
{
|
||||
int i;
|
||||
|
||||
for ( i=0; i<getNV( ); ++i )
|
||||
_ub[i] = ub[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t U B
|
||||
*/
|
||||
inline returnValue QProblemB::getUB( int number, real_t& value ) const
|
||||
{
|
||||
if ( ( number >= 0 ) && ( number < getNV( ) ) )
|
||||
{
|
||||
value = ub[number];
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
{
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t B o u n d s
|
||||
*/
|
||||
inline returnValue QProblemB::getBounds( Bounds* const _bounds ) const
|
||||
{
|
||||
*_bounds = bounds;
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N V
|
||||
*/
|
||||
inline int QProblemB::getNV( ) const
|
||||
{
|
||||
return bounds.getNV( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N F R
|
||||
*/
|
||||
inline int QProblemB::getNFR( )
|
||||
{
|
||||
return bounds.getNFR( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N F X
|
||||
*/
|
||||
inline int QProblemB::getNFX( )
|
||||
{
|
||||
return bounds.getNFX( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N F V
|
||||
*/
|
||||
inline int QProblemB::getNFV( ) const
|
||||
{
|
||||
return bounds.getNFV( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t S t a t u s
|
||||
*/
|
||||
inline QProblemStatus QProblemB::getStatus( ) const
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i s I n i t i a l i s e d
|
||||
*/
|
||||
inline BooleanType QProblemB::isInitialised( ) const
|
||||
{
|
||||
if ( status == QPS_NOTINITIALISED )
|
||||
return BT_FALSE;
|
||||
else
|
||||
return BT_TRUE;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i s S o l v e d
|
||||
*/
|
||||
inline BooleanType QProblemB::isSolved( ) const
|
||||
{
|
||||
if ( status == QPS_SOLVED )
|
||||
return BT_TRUE;
|
||||
else
|
||||
return BT_FALSE;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i s I n f e a s i b l e
|
||||
*/
|
||||
inline BooleanType QProblemB::isInfeasible( ) const
|
||||
{
|
||||
return infeasible;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i s U n b o u n d e d
|
||||
*/
|
||||
inline BooleanType QProblemB::isUnbounded( ) const
|
||||
{
|
||||
return unbounded;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t P r i n t L e v e l
|
||||
*/
|
||||
inline PrintLevel QProblemB::getPrintLevel( ) const
|
||||
{
|
||||
return printlevel;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t H e s s i a n T y p e
|
||||
*/
|
||||
inline HessianType QProblemB::getHessianType( ) const
|
||||
{
|
||||
return hessianType;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t H e s s i a n T y p e
|
||||
*/
|
||||
inline returnValue QProblemB::setHessianType( HessianType _hessianType )
|
||||
{
|
||||
hessianType = _hessianType;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P R O T E C T E D *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* s e t H
|
||||
*/
|
||||
inline returnValue QProblemB::setH( const real_t* const H_new )
|
||||
{
|
||||
int i, j;
|
||||
|
||||
int nV = getNV();
|
||||
|
||||
for( i=0; i<nV; ++i )
|
||||
for( j=0; j<nV; ++j )
|
||||
H[i*NVMAX + j] = H_new[i*nV + j];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t G
|
||||
*/
|
||||
inline returnValue QProblemB::setG( const real_t* const g_new )
|
||||
{
|
||||
int i;
|
||||
|
||||
int nV = getNV();
|
||||
|
||||
for( i=0; i<nV; ++i )
|
||||
g[i] = g_new[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t L B
|
||||
*/
|
||||
inline returnValue QProblemB::setLB( const real_t* const lb_new )
|
||||
{
|
||||
int i;
|
||||
|
||||
int nV = getNV();
|
||||
|
||||
for( i=0; i<nV; ++i )
|
||||
lb[i] = lb_new[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t L B
|
||||
*/
|
||||
inline returnValue QProblemB::setLB( int number, real_t value )
|
||||
{
|
||||
if ( ( number >= 0 ) && ( number < getNV( ) ) )
|
||||
{
|
||||
lb[number] = value;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
{
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t U B
|
||||
*/
|
||||
inline returnValue QProblemB::setUB( const real_t* const ub_new )
|
||||
{
|
||||
int i;
|
||||
|
||||
int nV = getNV();
|
||||
|
||||
for( i=0; i<nV; ++i )
|
||||
ub[i] = ub_new[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t U B
|
||||
*/
|
||||
inline returnValue QProblemB::setUB( int number, real_t value )
|
||||
{
|
||||
if ( ( number >= 0 ) && ( number < getNV( ) ) )
|
||||
{
|
||||
ub[number] = value;
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
{
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* c o m p u t e G i v e n s
|
||||
*/
|
||||
inline void QProblemB::computeGivens( real_t xold, real_t yold, real_t& xnew, real_t& ynew,
|
||||
real_t& c, real_t& s
|
||||
) const
|
||||
{
|
||||
if ( getAbs( yold ) <= ZERO )
|
||||
{
|
||||
c = 1.0;
|
||||
s = 0.0;
|
||||
|
||||
xnew = xold;
|
||||
ynew = yold;
|
||||
}
|
||||
else
|
||||
{
|
||||
real_t t, mu;
|
||||
|
||||
mu = getAbs( xold );
|
||||
if ( getAbs( yold ) > mu )
|
||||
mu = getAbs( yold );
|
||||
|
||||
t = mu * sqrt( (xold/mu)*(xold/mu) + (yold/mu)*(yold/mu) );
|
||||
|
||||
if ( xold < 0.0 )
|
||||
t = -t;
|
||||
|
||||
c = xold/t;
|
||||
s = yold/t;
|
||||
xnew = t;
|
||||
ynew = 0.0;
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* a p p l y G i v e n s
|
||||
*/
|
||||
inline void QProblemB::applyGivens( real_t c, real_t s, real_t xold, real_t yold,
|
||||
real_t& xnew, real_t& ynew
|
||||
) const
|
||||
{
|
||||
/* Usual Givens plane rotation requiring four multiplications. */
|
||||
xnew = c*xold + s*yold;
|
||||
ynew = -s*xold + c*yold;
|
||||
// double nu = s/(1.0+c);
|
||||
//
|
||||
// xnew = xold*c + yold*s;
|
||||
// ynew = (xnew+xold)*nu - yold;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,200 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/SubjectTo.cpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of the SubjectTo class designed to manage working sets of
|
||||
* constraints and bounds within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
#include <SubjectTo.hpp>
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/*
|
||||
* S u b j e c t T o
|
||||
*/
|
||||
SubjectTo::SubjectTo( ) : noLower( BT_TRUE ),
|
||||
noUpper( BT_TRUE ),
|
||||
size( 0 )
|
||||
{
|
||||
int i;
|
||||
|
||||
for( i=0; i<size; ++i )
|
||||
{
|
||||
type[i] = ST_UNKNOWN;
|
||||
status[i] = ST_UNDEFINED;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* S u b j e c t T o
|
||||
*/
|
||||
SubjectTo::SubjectTo( const SubjectTo& rhs ) : noLower( rhs.noLower ),
|
||||
noUpper( rhs.noUpper ),
|
||||
size( rhs.size )
|
||||
{
|
||||
int i;
|
||||
|
||||
for( i=0; i<size; ++i )
|
||||
{
|
||||
type[i] = rhs.type[i];
|
||||
status[i] = rhs.status[i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* ~ S u b j e c t T o
|
||||
*/
|
||||
SubjectTo::~SubjectTo( )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* o p e r a t o r =
|
||||
*/
|
||||
SubjectTo& SubjectTo::operator=( const SubjectTo& rhs )
|
||||
{
|
||||
int i;
|
||||
|
||||
if ( this != &rhs )
|
||||
{
|
||||
size = rhs.size;
|
||||
|
||||
for( i=0; i<size; ++i )
|
||||
{
|
||||
type[i] = rhs.type[i];
|
||||
status[i] = rhs.status[i];
|
||||
}
|
||||
|
||||
noLower = rhs.noLower;
|
||||
noUpper = rhs.noUpper;
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* i n i t
|
||||
*/
|
||||
returnValue SubjectTo::init( int n )
|
||||
{
|
||||
int i;
|
||||
|
||||
size = n;
|
||||
|
||||
noLower = BT_TRUE;
|
||||
noUpper = BT_TRUE;
|
||||
|
||||
for( i=0; i<size; ++i )
|
||||
{
|
||||
type[i] = ST_UNKNOWN;
|
||||
status[i] = ST_UNDEFINED;
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P R O T E C T E D *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* a d d I n d e x
|
||||
*/
|
||||
returnValue SubjectTo::addIndex( Indexlist* const indexlist,
|
||||
int newnumber, SubjectToStatus newstatus
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( status[newnumber] == newstatus )
|
||||
return THROWERROR( RET_INDEX_ALREADY_OF_DESIRED_STATUS );
|
||||
|
||||
status[newnumber] = newstatus;
|
||||
|
||||
if ( indexlist->addNumber( newnumber ) == RET_INDEXLIST_EXCEEDS_MAX_LENGTH )
|
||||
return THROWERROR( RET_ADDINDEX_FAILED );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* r e m o v e I n d e x
|
||||
*/
|
||||
returnValue SubjectTo::removeIndex( Indexlist* const indexlist,
|
||||
int removenumber
|
||||
)
|
||||
{
|
||||
status[removenumber] = ST_UNDEFINED;
|
||||
|
||||
if ( indexlist->removeNumber( removenumber ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_UNKNOWN_BUG );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s w a p I n d e x
|
||||
*/
|
||||
returnValue SubjectTo::swapIndex( Indexlist* const indexlist,
|
||||
int number1, int number2
|
||||
)
|
||||
{
|
||||
/* consistency checks */
|
||||
if ( status[number1] != status[number2] )
|
||||
return THROWERROR( RET_SWAPINDEX_FAILED );
|
||||
|
||||
if ( number1 == number2 )
|
||||
{
|
||||
THROWWARNING( RET_NOTHING_TO_DO );
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
if ( indexlist->swapNumbers( number1,number2 ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SWAPINDEX_FAILED );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,132 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/SubjectTo.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of the inlined member functions of the SubjectTo class
|
||||
* designed to manage working sets of constraints and bounds within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/*
|
||||
* g e t T y p e
|
||||
*/
|
||||
inline SubjectToType SubjectTo::getType( int i ) const
|
||||
{
|
||||
if ( ( i >= 0 ) && ( i < size ) )
|
||||
return type[i];
|
||||
else
|
||||
return ST_UNKNOWN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t S t a t u s
|
||||
*/
|
||||
inline SubjectToStatus SubjectTo::getStatus( int i ) const
|
||||
{
|
||||
if ( ( i >= 0 ) && ( i < size ) )
|
||||
return status[i];
|
||||
else
|
||||
return ST_UNDEFINED;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t T y p e
|
||||
*/
|
||||
inline returnValue SubjectTo::setType( int i, SubjectToType value )
|
||||
{
|
||||
if ( ( i >= 0 ) && ( i < size ) )
|
||||
{
|
||||
type[i] = value;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t S t a t u s
|
||||
*/
|
||||
inline returnValue SubjectTo::setStatus( int i, SubjectToStatus value )
|
||||
{
|
||||
if ( ( i >= 0 ) && ( i < size ) )
|
||||
{
|
||||
status[i] = value;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t N o L o w e r
|
||||
*/
|
||||
inline void SubjectTo::setNoLower( BooleanType _status )
|
||||
{
|
||||
noLower = _status;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t N o U p p e r
|
||||
*/
|
||||
inline void SubjectTo::setNoUpper( BooleanType _status )
|
||||
{
|
||||
noUpper = _status;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i s N o L o w e r
|
||||
*/
|
||||
inline BooleanType SubjectTo::isNoLower( ) const
|
||||
{
|
||||
return noLower;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i s N o L o w e r
|
||||
*/
|
||||
inline BooleanType SubjectTo::isNoUpper( ) const
|
||||
{
|
||||
return noUpper;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,471 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/Utils.cpp
|
||||
* \author Hans Joachim Ferreau, Eckhard Arnold
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of some inlined utilities for working with the different QProblem
|
||||
* classes.
|
||||
*/
|
||||
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#if defined(__WIN32__) || defined(WIN32)
|
||||
#include <windows.h>
|
||||
#elif defined(LINUX)
|
||||
#include <sys/stat.h>
|
||||
#include <sys/time.h>
|
||||
#endif
|
||||
|
||||
#ifdef __MATLAB__
|
||||
#include <mex.h>
|
||||
#endif
|
||||
|
||||
|
||||
#include <Utils.hpp>
|
||||
|
||||
|
||||
|
||||
#ifdef PC_DEBUG /* Define print functions only for debugging! */
|
||||
/*
|
||||
* p r i n t
|
||||
*/
|
||||
returnValue print( const real_t* const v, int n )
|
||||
{
|
||||
int i;
|
||||
char myPrintfString[160];
|
||||
|
||||
/* Print a vector. */
|
||||
myPrintf( "[\t" );
|
||||
for( i=0; i<n; ++i )
|
||||
{
|
||||
sprintf( myPrintfString," %.16e\t", v[i] );
|
||||
myPrintf( myPrintfString );
|
||||
}
|
||||
myPrintf( "]\n" );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* p r i n t
|
||||
*/
|
||||
returnValue print( const real_t* const v, int n,
|
||||
const int* const V_idx
|
||||
)
|
||||
{
|
||||
int i;
|
||||
char myPrintfString[160];
|
||||
|
||||
/* Print a permuted vector. */
|
||||
myPrintf( "[\t" );
|
||||
for( i=0; i<n; ++i )
|
||||
{
|
||||
sprintf( myPrintfString," %.16e\t", v[ V_idx[i] ] );
|
||||
myPrintf( myPrintfString );
|
||||
}
|
||||
myPrintf( "]\n" );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* p r i n t
|
||||
*/
|
||||
returnValue print( const real_t* const v, int n,
|
||||
const char* name
|
||||
)
|
||||
{
|
||||
char myPrintfString[160];
|
||||
|
||||
/* Print vector name ... */
|
||||
sprintf( myPrintfString,"%s = ", name );
|
||||
myPrintf( myPrintfString );
|
||||
|
||||
/* ... and the vector itself. */
|
||||
return print( v, n );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* p r i n t
|
||||
*/
|
||||
returnValue print( const real_t* const M, int nrow, int ncol )
|
||||
{
|
||||
int i;
|
||||
|
||||
/* Print a matrix as a collection of row vectors. */
|
||||
for( i=0; i<nrow; ++i )
|
||||
print( &(M[i*ncol]), ncol );
|
||||
myPrintf( "\n" );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* p r i n t
|
||||
*/
|
||||
returnValue print( const real_t* const M, int nrow, int ncol,
|
||||
const int* const ROW_idx, const int* const COL_idx
|
||||
)
|
||||
{
|
||||
int i;
|
||||
|
||||
/* Print a permuted matrix as a collection of permuted row vectors. */
|
||||
for( i=0; i<nrow; ++i )
|
||||
print( &( M[ ROW_idx[i]*ncol ] ), ncol, COL_idx );
|
||||
myPrintf( "\n" );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* p r i n t
|
||||
*/
|
||||
returnValue print( const real_t* const M, int nrow, int ncol,
|
||||
const char* name
|
||||
)
|
||||
{
|
||||
char myPrintfString[160];
|
||||
|
||||
/* Print matrix name ... */
|
||||
sprintf( myPrintfString,"%s = ", name );
|
||||
myPrintf( myPrintfString );
|
||||
|
||||
/* ... and the matrix itself. */
|
||||
return print( M, nrow, ncol );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* p r i n t
|
||||
*/
|
||||
returnValue print( const int* const index, int n )
|
||||
{
|
||||
int i;
|
||||
char myPrintfString[160];
|
||||
|
||||
/* Print a indexlist. */
|
||||
myPrintf( "[\t" );
|
||||
for( i=0; i<n; ++i )
|
||||
{
|
||||
sprintf( myPrintfString," %d\t", index[i] );
|
||||
myPrintf( myPrintfString );
|
||||
}
|
||||
myPrintf( "]\n" );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* p r i n t
|
||||
*/
|
||||
returnValue print( const int* const index, int n,
|
||||
const char* name
|
||||
)
|
||||
{
|
||||
char myPrintfString[160];
|
||||
|
||||
/* Print indexlist name ... */
|
||||
sprintf( myPrintfString,"%s = ", name );
|
||||
myPrintf( myPrintfString );
|
||||
|
||||
/* ... and the indexlist itself. */
|
||||
return print( index, n );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* m y P r i n t f
|
||||
*/
|
||||
returnValue myPrintf( const char* s )
|
||||
{
|
||||
#ifdef __MATLAB__
|
||||
mexPrintf( s );
|
||||
#else
|
||||
myFILE* outputfile = getGlobalMessageHandler( )->getOutputFile( );
|
||||
if ( outputfile == 0 )
|
||||
return THROWERROR( RET_NO_GLOBAL_MESSAGE_OUTPUTFILE );
|
||||
|
||||
fprintf( outputfile, "%s", s );
|
||||
#endif
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* p r i n t C o p y r i g h t N o t i c e
|
||||
*/
|
||||
returnValue printCopyrightNotice( )
|
||||
{
|
||||
return myPrintf( "\nqpOASES -- An Implementation of the Online Active Set Strategy.\nCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\n\nqpOASES is distributed under the terms of the \nGNU Lesser General Public License 2.1 in the hope that it will be \nuseful, but WITHOUT ANY WARRANTY; without even the implied warranty \nof MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. \nSee the GNU Lesser General Public License for more details.\n\n" );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* r e a d F r o m F i l e
|
||||
*/
|
||||
returnValue readFromFile( real_t* data, int nrow, int ncol,
|
||||
const char* datafilename
|
||||
)
|
||||
{
|
||||
int i, j;
|
||||
float float_data;
|
||||
myFILE* datafile;
|
||||
|
||||
/* 1) Open file. */
|
||||
if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
|
||||
{
|
||||
char errstr[80];
|
||||
sprintf( errstr,"(%s)",datafilename );
|
||||
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
}
|
||||
|
||||
/* 2) Read data from file. */
|
||||
for( i=0; i<nrow; ++i )
|
||||
{
|
||||
for( j=0; j<ncol; ++j )
|
||||
{
|
||||
if ( fscanf( datafile, "%f ", &float_data ) == 0 )
|
||||
{
|
||||
fclose( datafile );
|
||||
char errstr[80];
|
||||
sprintf( errstr,"(%s)",datafilename );
|
||||
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
}
|
||||
data[i*ncol + j] = ( (real_t) float_data );
|
||||
}
|
||||
}
|
||||
|
||||
/* 3) Close file. */
|
||||
fclose( datafile );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* r e a d F r o m F i l e
|
||||
*/
|
||||
returnValue readFromFile( real_t* data, int n,
|
||||
const char* datafilename
|
||||
)
|
||||
{
|
||||
return readFromFile( data, n, 1, datafilename );
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* r e a d F r o m F i l e
|
||||
*/
|
||||
returnValue readFromFile( int* data, int n,
|
||||
const char* datafilename
|
||||
)
|
||||
{
|
||||
int i;
|
||||
myFILE* datafile;
|
||||
|
||||
/* 1) Open file. */
|
||||
if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
|
||||
{
|
||||
char errstr[80];
|
||||
sprintf( errstr,"(%s)",datafilename );
|
||||
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
}
|
||||
|
||||
/* 2) Read data from file. */
|
||||
for( i=0; i<n; ++i )
|
||||
{
|
||||
if ( fscanf( datafile, "%d\n", &(data[i]) ) == 0 )
|
||||
{
|
||||
fclose( datafile );
|
||||
char errstr[80];
|
||||
sprintf( errstr,"(%s)",datafilename );
|
||||
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
}
|
||||
}
|
||||
|
||||
/* 3) Close file. */
|
||||
fclose( datafile );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* w r i t e I n t o F i l e
|
||||
*/
|
||||
returnValue writeIntoFile( const real_t* const data, int nrow, int ncol,
|
||||
const char* datafilename, BooleanType append
|
||||
)
|
||||
{
|
||||
int i, j;
|
||||
myFILE* datafile;
|
||||
|
||||
/* 1) Open file. */
|
||||
if ( append == BT_TRUE )
|
||||
{
|
||||
/* append data */
|
||||
if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
|
||||
{
|
||||
char errstr[80];
|
||||
sprintf( errstr,"(%s)",datafilename );
|
||||
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
/* do not append data */
|
||||
if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
|
||||
{
|
||||
char errstr[80];
|
||||
sprintf( errstr,"(%s)",datafilename );
|
||||
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
}
|
||||
}
|
||||
|
||||
/* 2) Write data into file. */
|
||||
for( i=0; i<nrow; ++i )
|
||||
{
|
||||
for( j=0; j<ncol; ++j )
|
||||
fprintf( datafile, "%.16e ", data[i*ncol+j] );
|
||||
|
||||
fprintf( datafile, "\n" );
|
||||
}
|
||||
|
||||
/* 3) Close file. */
|
||||
fclose( datafile );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* w r i t e I n t o F i l e
|
||||
*/
|
||||
returnValue writeIntoFile( const real_t* const data, int n,
|
||||
const char* datafilename, BooleanType append
|
||||
)
|
||||
{
|
||||
return writeIntoFile( data,1,n,datafilename,append );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* w r i t e I n t o F i l e
|
||||
*/
|
||||
returnValue writeIntoFile( const int* const data, int n,
|
||||
const char* datafilename, BooleanType append
|
||||
)
|
||||
{
|
||||
int i;
|
||||
|
||||
myFILE* datafile;
|
||||
|
||||
/* 1) Open file. */
|
||||
if ( append == BT_TRUE )
|
||||
{
|
||||
/* append data */
|
||||
if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
|
||||
{
|
||||
char errstr[80];
|
||||
sprintf( errstr,"(%s)",datafilename );
|
||||
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
/* do not append data */
|
||||
if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
|
||||
{
|
||||
char errstr[80];
|
||||
sprintf( errstr,"(%s)",datafilename );
|
||||
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
}
|
||||
}
|
||||
|
||||
/* 2) Write data into file. */
|
||||
for( i=0; i<n; ++i )
|
||||
fprintf( datafile, "%d\n", data[i] );
|
||||
|
||||
/* 3) Close file. */
|
||||
fclose( datafile );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
#endif /* PC_DEBUG */
|
||||
|
||||
|
||||
/*
|
||||
* g e t C P U t i m e
|
||||
*/
|
||||
real_t getCPUtime( )
|
||||
{
|
||||
real_t current_time = -1.0;
|
||||
|
||||
#if defined(__WIN32__) || defined(WIN32)
|
||||
LARGE_INTEGER counter, frequency;
|
||||
QueryPerformanceFrequency(&frequency);
|
||||
QueryPerformanceCounter(&counter);
|
||||
current_time = ((real_t) counter.QuadPart) / ((real_t) frequency.QuadPart);
|
||||
#elif defined(LINUX)
|
||||
struct timeval theclock;
|
||||
gettimeofday( &theclock,0 );
|
||||
current_time = 1.0*theclock.tv_sec + 1.0e-6*theclock.tv_usec;
|
||||
#endif
|
||||
|
||||
return current_time;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N o r m
|
||||
*/
|
||||
real_t getNorm( const real_t* const v, int n )
|
||||
{
|
||||
int i;
|
||||
|
||||
real_t norm = 0.0;
|
||||
|
||||
for( i=0; i<n; ++i )
|
||||
norm += v[i]*v[i];
|
||||
|
||||
return sqrt( norm );
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,51 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/Utils.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of some inlined utilities for working with the different QProblem
|
||||
* classes.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* g e t A b s
|
||||
*/
|
||||
inline real_t getAbs( real_t x )
|
||||
{
|
||||
if ( x < 0.0 )
|
||||
return -x;
|
||||
else
|
||||
return x;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,189 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/Bounds.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of the Bounds class designed to manage working sets of
|
||||
* bounds within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_BOUNDS_HPP
|
||||
#define QPOASES_BOUNDS_HPP
|
||||
|
||||
|
||||
#include <SubjectTo.hpp>
|
||||
|
||||
|
||||
|
||||
/** This class manages working sets of bounds by storing
|
||||
* index sets and other status information.
|
||||
*
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*/
|
||||
class Bounds : public SubjectTo
|
||||
{
|
||||
/*
|
||||
* PUBLIC MEMBER FUNCTIONS
|
||||
*/
|
||||
public:
|
||||
/** Default constructor. */
|
||||
Bounds( );
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
Bounds( const Bounds& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~Bounds( );
|
||||
|
||||
/** Assignment operator (deep copy). */
|
||||
Bounds& operator=( const Bounds& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
|
||||
/** Pseudo-constructor takes the number of bounds.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue init( int n /**< Number of bounds. */
|
||||
);
|
||||
|
||||
|
||||
/** Initially adds number of a new (i.e. not yet in the list) bound to
|
||||
* given index set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SETUP_BOUND_FAILED \n
|
||||
RET_INDEX_OUT_OF_BOUNDS \n
|
||||
RET_INVALID_ARGUMENTS */
|
||||
returnValue setupBound( int _number, /**< Number of new bound. */
|
||||
SubjectToStatus _status /**< Status of new bound. */
|
||||
);
|
||||
|
||||
/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
|
||||
* to the index set of free bounds; the order depends on the SujectToType
|
||||
* of each index.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SETUP_BOUND_FAILED */
|
||||
returnValue setupAllFree( );
|
||||
|
||||
|
||||
/** Moves index of a bound from index list of fixed to that of free bounds.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_MOVING_BOUND_FAILED \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
returnValue moveFixedToFree( int _number /**< Number of bound to be freed. */
|
||||
);
|
||||
|
||||
/** Moves index of a bound from index list of free to that of fixed bounds.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_MOVING_BOUND_FAILED \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
returnValue moveFreeToFixed( int _number, /**< Number of bound to be fixed. */
|
||||
SubjectToStatus _status /**< Status of bound to be fixed. */
|
||||
);
|
||||
|
||||
/** Swaps the indices of two free bounds within the index set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SWAPINDEX_FAILED */
|
||||
returnValue swapFree( int number1, /**< Number of first constraint or bound. */
|
||||
int number2 /**< Number of second constraint or bound. */
|
||||
);
|
||||
|
||||
|
||||
/** Returns number of variables.
|
||||
* \return Number of variables. */
|
||||
inline int getNV( ) const;
|
||||
|
||||
/** Returns number of implicitly fixed variables.
|
||||
* \return Number of implicitly fixed variables. */
|
||||
inline int getNFV( ) const;
|
||||
|
||||
/** Returns number of bounded (but possibly free) variables.
|
||||
* \return Number of bounded (but possibly free) variables. */
|
||||
inline int getNBV( ) const;
|
||||
|
||||
/** Returns number of unbounded variables.
|
||||
* \return Number of unbounded variables. */
|
||||
inline int getNUV( ) const;
|
||||
|
||||
|
||||
/** Sets number of implicitly fixed variables.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setNFV( int n /**< Number of implicitly fixed variables. */
|
||||
);
|
||||
|
||||
/** Sets number of bounded (but possibly free) variables.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setNBV( int n /**< Number of bounded (but possibly free) variables. */
|
||||
);
|
||||
|
||||
/** Sets number of unbounded variables.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setNUV( int n /**< Number of unbounded variables */
|
||||
);
|
||||
|
||||
|
||||
/** Returns number of free variables.
|
||||
* \return Number of free variables. */
|
||||
inline int getNFR( );
|
||||
|
||||
/** Returns number of fixed variables.
|
||||
* \return Number of fixed variables. */
|
||||
inline int getNFX( );
|
||||
|
||||
|
||||
/** Returns a pointer to free variables index list.
|
||||
* \return Pointer to free variables index list. */
|
||||
inline Indexlist* getFree( );
|
||||
|
||||
/** Returns a pointer to fixed variables index list.
|
||||
* \return Pointer to fixed variables index list. */
|
||||
inline Indexlist* getFixed( );
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER VARIABLES
|
||||
*/
|
||||
protected:
|
||||
int nV; /**< Number of variables (nV = nFV + nBV + nUV). */
|
||||
int nFV; /**< Number of implicitly fixed variables. */
|
||||
int nBV; /**< Number of bounded (but possibly free) variables. */
|
||||
int nUV; /**< Number of unbounded variables. */
|
||||
|
||||
Indexlist free; /**< Index list of free variables. */
|
||||
Indexlist fixed; /**< Index list of fixed variables. */
|
||||
};
|
||||
|
||||
#include <Bounds.ipp>
|
||||
|
||||
#endif /* QPOASES_BOUNDS_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,108 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/Constants.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2008
|
||||
*
|
||||
* Definition of all global constants.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_CONSTANTS_HPP
|
||||
#define QPOASES_CONSTANTS_HPP
|
||||
|
||||
#ifndef QPOASES_CUSTOM_INTERFACE
|
||||
#include "acado_qpoases_interface.hpp"
|
||||
#else
|
||||
#define XSTR(x) #x
|
||||
#define STR(x) XSTR(x)
|
||||
#include STR(QPOASES_CUSTOM_INTERFACE)
|
||||
#endif
|
||||
|
||||
/** Maximum number of variables within a QP formulation.
|
||||
Note: this value has to be positive! */
|
||||
const int NVMAX = QPOASES_NVMAX;
|
||||
|
||||
/** Maximum number of constraints within a QP formulation.
|
||||
Note: this value has to be positive! */
|
||||
const int NCMAX = QPOASES_NCMAX;
|
||||
|
||||
/** Redefinition of NCMAX used for memory allocation, to avoid zero sized arrays
|
||||
and compiler errors. */
|
||||
const int NCMAX_ALLOC = (NCMAX == 0) ? 1 : NCMAX;
|
||||
|
||||
/**< Maximum number of working set recalculations.
|
||||
Note: this value has to be positive! */
|
||||
const int NWSRMAX = QPOASES_NWSRMAX;
|
||||
|
||||
/** Desired KKT tolerance of QP solution; a warning RET_INACCURATE_SOLUTION is
|
||||
* issued if this tolerance is not met.
|
||||
* Note: this value has to be positive! */
|
||||
const real_t DESIREDACCURACY = (real_t) 1.0e-3;
|
||||
|
||||
/** Critical KKT tolerance of QP solution; an error is issued if this
|
||||
* tolerance is not met.
|
||||
* Note: this value has to be positive! */
|
||||
const real_t CRITICALACCURACY = (real_t) 1.0e-2;
|
||||
|
||||
|
||||
|
||||
/** Numerical value of machine precision (min eps, s.t. 1+eps > 1).
|
||||
Note: this value has to be positive! */
|
||||
const real_t EPS = (real_t) QPOASES_EPS;
|
||||
|
||||
/** Numerical value of zero (for situations in which it would be
|
||||
* unreasonable to compare with 0.0).
|
||||
* Note: this value has to be positive! */
|
||||
const real_t ZERO = (real_t) 1.0e-50;
|
||||
|
||||
/** Numerical value of infinity (e.g. for non-existing bounds).
|
||||
* Note: this value has to be positive! */
|
||||
const real_t INFTY = (real_t) 1.0e12;
|
||||
|
||||
|
||||
/** Lower/upper (constraints') bound tolerance (an inequality constraint
|
||||
* whose lower and upper bound differ by less than BOUNDTOL is regarded
|
||||
* to be an equality constraint).
|
||||
* Note: this value has to be positive! */
|
||||
const real_t BOUNDTOL = (real_t) 1.0e-10;
|
||||
|
||||
/** Offset for relaxing (constraints') bounds at beginning of an initial homotopy.
|
||||
* Note: this value has to be positive! */
|
||||
const real_t BOUNDRELAXATION = (real_t) 1.0e3;
|
||||
|
||||
|
||||
/** Factor that determines physical lengths of index lists.
|
||||
* Note: this value has to be greater than 1! */
|
||||
const int INDEXLISTFACTOR = 5;
|
||||
|
||||
|
||||
#endif /* QPOASES_CONSTANTS_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,181 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/Constraints.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of the Constraints class designed to manage working sets of
|
||||
* constraints within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_CONSTRAINTS_HPP
|
||||
#define QPOASES_CONSTRAINTS_HPP
|
||||
|
||||
|
||||
#include <SubjectTo.hpp>
|
||||
|
||||
|
||||
|
||||
/** This class manages working sets of constraints by storing
|
||||
* index sets and other status information.
|
||||
*
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*/
|
||||
class Constraints : public SubjectTo
|
||||
{
|
||||
/*
|
||||
* PUBLIC MEMBER FUNCTIONS
|
||||
*/
|
||||
public:
|
||||
/** Default constructor. */
|
||||
Constraints( );
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
Constraints( const Constraints& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~Constraints( );
|
||||
|
||||
/** Assignment operator (deep copy). */
|
||||
Constraints& operator=( const Constraints& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
|
||||
/** Pseudo-constructor takes the number of constraints.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue init( int n /**< Number of constraints. */
|
||||
);
|
||||
|
||||
|
||||
/** Initially adds number of a new (i.e. not yet in the list) constraint to
|
||||
* a given index set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SETUP_CONSTRAINT_FAILED \n
|
||||
RET_INDEX_OUT_OF_BOUNDS \n
|
||||
RET_INVALID_ARGUMENTS */
|
||||
returnValue setupConstraint( int _number, /**< Number of new constraint. */
|
||||
SubjectToStatus _status /**< Status of new constraint. */
|
||||
);
|
||||
|
||||
/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to
|
||||
* to the index set of inactive constraints; the order depends on the SujectToType
|
||||
* of each index. Only disabled constraints are added to index set of disabled constraints!
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SETUP_CONSTRAINT_FAILED */
|
||||
returnValue setupAllInactive( );
|
||||
|
||||
|
||||
/** Moves index of a constraint from index list of active to that of inactive constraints.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_MOVING_CONSTRAINT_FAILED */
|
||||
returnValue moveActiveToInactive( int _number /**< Number of constraint to become inactive. */
|
||||
);
|
||||
|
||||
/** Moves index of a constraint from index list of inactive to that of active constraints.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_MOVING_CONSTRAINT_FAILED */
|
||||
returnValue moveInactiveToActive( int _number, /**< Number of constraint to become active. */
|
||||
SubjectToStatus _status /**< Status of constraint to become active. */
|
||||
);
|
||||
|
||||
|
||||
/** Returns the number of constraints.
|
||||
* \return Number of constraints. */
|
||||
inline int getNC( ) const;
|
||||
|
||||
/** Returns the number of implicit equality constraints.
|
||||
* \return Number of implicit equality constraints. */
|
||||
inline int getNEC( ) const;
|
||||
|
||||
/** Returns the number of "real" inequality constraints.
|
||||
* \return Number of "real" inequality constraints. */
|
||||
inline int getNIC( ) const;
|
||||
|
||||
/** Returns the number of unbounded constraints (i.e. without any bounds).
|
||||
* \return Number of unbounded constraints (i.e. without any bounds). */
|
||||
inline int getNUC( ) const;
|
||||
|
||||
|
||||
/** Sets number of implicit equality constraints.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setNEC( int n /**< Number of implicit equality constraints. */
|
||||
);
|
||||
|
||||
/** Sets number of "real" inequality constraints.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setNIC( int n /**< Number of "real" inequality constraints. */
|
||||
);
|
||||
|
||||
/** Sets number of unbounded constraints (i.e. without any bounds).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setNUC( int n /**< Number of unbounded constraints (i.e. without any bounds). */
|
||||
);
|
||||
|
||||
|
||||
/** Returns the number of active constraints.
|
||||
* \return Number of constraints. */
|
||||
inline int getNAC( );
|
||||
|
||||
/** Returns the number of inactive constraints.
|
||||
* \return Number of constraints. */
|
||||
inline int getNIAC( );
|
||||
|
||||
|
||||
/** Returns a pointer to active constraints index list.
|
||||
* \return Pointer to active constraints index list. */
|
||||
inline Indexlist* getActive( );
|
||||
|
||||
/** Returns a pointer to inactive constraints index list.
|
||||
* \return Pointer to inactive constraints index list. */
|
||||
inline Indexlist* getInactive( );
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER VARIABLES
|
||||
*/
|
||||
protected:
|
||||
int nC; /**< Number of constraints (nC = nEC + nIC + nUC). */
|
||||
int nEC; /**< Number of implicit equality constraints. */
|
||||
int nIC; /**< Number of "real" inequality constraints. */
|
||||
int nUC; /**< Number of unbounded constraints (i.e. without any bounds). */
|
||||
|
||||
Indexlist active; /**< Index list of active constraints. */
|
||||
Indexlist inactive; /**< Index list of inactive constraints. */
|
||||
};
|
||||
|
||||
|
||||
#include <Constraints.ipp>
|
||||
|
||||
#endif /* QPOASES_CONSTRAINTS_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,126 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/CyclingManager.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of the CyclingManager class designed to detect
|
||||
* and handle possible cycling during QP iterations.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_CYCLINGMANAGER_HPP
|
||||
#define QPOASES_CYCLINGMANAGER_HPP
|
||||
|
||||
|
||||
#include <Utils.hpp>
|
||||
|
||||
|
||||
|
||||
/** This class is intended to detect and handle possible cycling during QP iterations.
|
||||
* As cycling seems to occur quite rarely, this class is NOT FULLY IMPLEMENTED YET!
|
||||
*
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*/
|
||||
class CyclingManager
|
||||
{
|
||||
/*
|
||||
* PUBLIC MEMBER FUNCTIONS
|
||||
*/
|
||||
public:
|
||||
/** Default constructor. */
|
||||
CyclingManager( );
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
CyclingManager( const CyclingManager& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~CyclingManager( );
|
||||
|
||||
/** Copy asingment operator (deep copy). */
|
||||
CyclingManager& operator=( const CyclingManager& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
|
||||
/** Pseudo-constructor which takes the number of bounds/constraints.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue init( int _nV, /**< Number of bounds to be managed. */
|
||||
int _nC /**< Number of constraints to be managed. */
|
||||
);
|
||||
|
||||
|
||||
/** Stores index of a bound/constraint that might cause cycling.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
returnValue setCyclingStatus( int number, /**< Number of bound/constraint. */
|
||||
BooleanType isBound, /**< Flag that indicates if given number corresponds to a
|
||||
* bound (BT_TRUE) or a constraint (BT_FALSE). */
|
||||
CyclingStatus _status /**< Cycling status of bound/constraint. */
|
||||
);
|
||||
|
||||
/** Returns if bound/constraint might cause cycling.
|
||||
* \return BT_TRUE: bound/constraint might cause cycling \n
|
||||
BT_FALSE: otherwise */
|
||||
CyclingStatus getCyclingStatus( int number, /**< Number of bound/constraint. */
|
||||
BooleanType isBound /**< Flag that indicates if given number corresponds to
|
||||
* a bound (BT_TRUE) or a constraint (BT_FALSE). */
|
||||
) const;
|
||||
|
||||
|
||||
/** Clears all previous cycling information.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue clearCyclingData( );
|
||||
|
||||
|
||||
/** Returns if cycling was detected.
|
||||
* \return BT_TRUE iff cycling was detected. */
|
||||
inline BooleanType isCyclingDetected( ) const;
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER VARIABLES
|
||||
*/
|
||||
protected:
|
||||
int nV; /**< Number of managed bounds. */
|
||||
int nC; /**< Number of managed constraints. */
|
||||
|
||||
CyclingStatus status[NVMAX+NCMAX]; /**< Array to store cycling status of all bounds/constraints. */
|
||||
|
||||
BooleanType cyclingDetected; /**< Flag if cycling was detected. */
|
||||
};
|
||||
|
||||
|
||||
#include <CyclingManager.ipp>
|
||||
|
||||
#endif /* QPOASES_CYCLINGMANAGER_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,107 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/EXTRAS/SolutionAnalysis.hpp
|
||||
* \author Milan Vukov, Boris Houska, Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2012
|
||||
*
|
||||
* Solution analysis class, based on a class in the standard version of the qpOASES
|
||||
*/
|
||||
|
||||
|
||||
//
|
||||
|
||||
#ifndef QPOASES_SOLUTIONANALYSIS_HPP
|
||||
#define QPOASES_SOLUTIONANALYSIS_HPP
|
||||
|
||||
#include <QProblem.hpp>
|
||||
|
||||
/** Enables the computation of variance as is in the standard version of qpOASES */
|
||||
#define QPOASES_USE_OLD_VERSION 0
|
||||
|
||||
#if QPOASES_USE_OLD_VERSION
|
||||
#define KKT_DIM (2 * NVMAX + NCMAX)
|
||||
#endif
|
||||
|
||||
class SolutionAnalysis
|
||||
{
|
||||
public:
|
||||
|
||||
/** Default constructor. */
|
||||
SolutionAnalysis( );
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
SolutionAnalysis( const SolutionAnalysis& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~SolutionAnalysis( );
|
||||
|
||||
/** Copy asingment operator (deep copy). */
|
||||
SolutionAnalysis& operator=( const SolutionAnalysis& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** A routine for computation of inverse of the Hessian matrix. */
|
||||
returnValue getHessianInverse(
|
||||
QProblem* qp, /** QP */
|
||||
real_t* hessianInverse /** Inverse of the Hessian matrix*/
|
||||
);
|
||||
|
||||
/** A routine for computation of inverse of the Hessian matrix. */
|
||||
returnValue getHessianInverse( QProblemB* qp, /** QP */
|
||||
real_t* hessianInverse /** Inverse of the Hessian matrix*/
|
||||
);
|
||||
|
||||
#if QPOASES_USE_OLD_VERSION
|
||||
returnValue getVarianceCovariance(
|
||||
QProblem* qp,
|
||||
real_t* g_b_bA_VAR,
|
||||
real_t* Primal_Dual_VAR
|
||||
);
|
||||
#endif
|
||||
|
||||
private:
|
||||
|
||||
real_t delta_g_cov[ NVMAX ]; /** A covariance-vector of g */
|
||||
real_t delta_lb_cov[ NVMAX ]; /** A covariance-vector of lb */
|
||||
real_t delta_ub_cov[ NVMAX ]; /** A covariance-vector of ub */
|
||||
real_t delta_lbA_cov[ NCMAX_ALLOC ]; /** A covariance-vector of lbA */
|
||||
real_t delta_ubA_cov[ NCMAX_ALLOC ]; /** A covariance-vector of ubA */
|
||||
|
||||
#if QPOASES_USE_OLD_VERSION
|
||||
real_t K[KKT_DIM * KKT_DIM]; /** A matrix to store an intermediate result */
|
||||
#endif
|
||||
|
||||
int FR_idx[ NVMAX ]; /** Index array for free variables */
|
||||
int FX_idx[ NVMAX ]; /** Index array for fixed variables */
|
||||
int AC_idx[ NCMAX_ALLOC ]; /** Index array for active constraints */
|
||||
|
||||
real_t delta_xFR[ NVMAX ]; /** QP reaction, primal, w.r.t. free */
|
||||
real_t delta_xFX[ NVMAX ]; /** QP reaction, primal, w.r.t. fixed */
|
||||
real_t delta_yAC[ NVMAX ]; /** QP reaction, dual, w.r.t. active */
|
||||
real_t delta_yFX[ NVMAX ]; /** QP reaction, dual, w.r.t. fixed*/
|
||||
};
|
||||
|
||||
#endif // QPOASES_SOLUTIONANALYSIS_HPP
|
||||
|
|
@ -0,0 +1,154 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/Indexlist.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of the Indexlist class designed to manage index lists of
|
||||
* constraints and bounds within a SubjectTo object.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_INDEXLIST_HPP
|
||||
#define QPOASES_INDEXLIST_HPP
|
||||
|
||||
|
||||
#include <Utils.hpp>
|
||||
|
||||
|
||||
/** This class manages index lists.
|
||||
*
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*/
|
||||
class Indexlist
|
||||
{
|
||||
/*
|
||||
* PUBLIC MEMBER FUNCTIONS
|
||||
*/
|
||||
public:
|
||||
/** Default constructor. */
|
||||
Indexlist( );
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
Indexlist( const Indexlist& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~Indexlist( );
|
||||
|
||||
/** Assingment operator (deep copy). */
|
||||
Indexlist& operator=( const Indexlist& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Pseudo-constructor.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue init( );
|
||||
|
||||
|
||||
/** Creates an array of all numbers within the index set in correct order.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEXLIST_CORRUPTED */
|
||||
returnValue getNumberArray( int* const numberarray /**< Output: Array of numbers (NULL on error). */
|
||||
) const;
|
||||
|
||||
|
||||
/** Determines the index within the index list at with a given number is stored.
|
||||
* \return >= 0: Index of given number. \n
|
||||
-1: Number not found. */
|
||||
int getIndex( int givennumber /**< Number whose index shall be determined. */
|
||||
) const;
|
||||
|
||||
/** Determines the physical index within the index list at with a given number is stored.
|
||||
* \return >= 0: Index of given number. \n
|
||||
-1: Number not found. */
|
||||
int getPhysicalIndex( int givennumber /**< Number whose physical index shall be determined. */
|
||||
) const;
|
||||
|
||||
/** Returns the number stored at a given physical index.
|
||||
* \return >= 0: Number stored at given physical index. \n
|
||||
-RET_INDEXLIST_OUTOFBOUNDS */
|
||||
int getNumber( int physicalindex /**< Physical index of the number to be returned. */
|
||||
) const;
|
||||
|
||||
|
||||
/** Returns the current length of the index list.
|
||||
* \return Current length of the index list. */
|
||||
inline int getLength( );
|
||||
|
||||
/** Returns last number within the index list.
|
||||
* \return Last number within the index list. */
|
||||
inline int getLastNumber( ) const;
|
||||
|
||||
|
||||
/** Adds number to index list.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEXLIST_MUST_BE_REORDERD \n
|
||||
RET_INDEXLIST_EXCEEDS_MAX_LENGTH */
|
||||
returnValue addNumber( int addnumber /**< Number to be added. */
|
||||
);
|
||||
|
||||
/** Removes number from index list.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue removeNumber( int removenumber /**< Number to be removed. */
|
||||
);
|
||||
|
||||
/** Swaps two numbers within index list.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue swapNumbers( int number1,/**< First number for swapping. */
|
||||
int number2 /**< Second number for swapping. */
|
||||
);
|
||||
|
||||
/** Determines if a given number is contained in the index set.
|
||||
* \return BT_TRUE iff number is contain in the index set */
|
||||
inline BooleanType isMember( int _number /**< Number to be tested for membership. */
|
||||
) const;
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER VARIABLES
|
||||
*/
|
||||
protected:
|
||||
int number[INDEXLISTFACTOR*(NVMAX+NCMAX)]; /**< Array to store numbers of constraints or bounds. */
|
||||
int next[INDEXLISTFACTOR*(NVMAX+NCMAX)]; /**< Array to store physical index of successor. */
|
||||
int previous[INDEXLISTFACTOR*(NVMAX+NCMAX)]; /**< Array to store physical index of predecossor. */
|
||||
int length; /**< Length of index list. */
|
||||
int first; /**< Physical index of first element. */
|
||||
int last; /**< Physical index of last element. */
|
||||
int lastusedindex; /**< Physical index of last entry in index list. */
|
||||
int physicallength; /**< Physical length of index list. */
|
||||
};
|
||||
|
||||
|
||||
#include <Indexlist.ipp>
|
||||
|
||||
#endif /* QPOASES_INDEXLIST_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,415 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/MessageHandling.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of the MessageHandling class including global return values.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_MESSAGEHANDLING_HPP
|
||||
#define QPOASES_MESSAGEHANDLING_HPP
|
||||
|
||||
// #define PC_DEBUG
|
||||
|
||||
#ifdef PC_DEBUG
|
||||
#include <stdio.h>
|
||||
|
||||
/** Defines an alias for FILE from stdio.h. */
|
||||
#define myFILE FILE
|
||||
/** Defines an alias for stderr from stdio.h. */
|
||||
#define myStderr stderr
|
||||
/** Defines an alias for stdout from stdio.h. */
|
||||
#define myStdout stdout
|
||||
#else
|
||||
/** Defines an alias for FILE from stdio.h. */
|
||||
#define myFILE int
|
||||
/** Defines an alias for stderr from stdio.h. */
|
||||
#define myStderr 0
|
||||
/** Defines an alias for stdout from stdio.h. */
|
||||
#define myStdout 0
|
||||
#endif
|
||||
|
||||
|
||||
#include <Types.hpp>
|
||||
#include <Constants.hpp>
|
||||
|
||||
|
||||
/** Defines symbols for global return values. \n
|
||||
* Important: All return values are assumed to be nonnegative! */
|
||||
enum returnValue
|
||||
{
|
||||
TERMINAL_LIST_ELEMENT = -1, /**< Terminal list element, internal usage only! */
|
||||
/* miscellaneous */
|
||||
SUCCESSFUL_RETURN = 0, /**< Successful return. */
|
||||
RET_DIV_BY_ZERO, /**< Division by zero. */
|
||||
RET_INDEX_OUT_OF_BOUNDS, /**< Index out of bounds. */
|
||||
RET_INVALID_ARGUMENTS, /**< At least one of the arguments is invalid. */
|
||||
RET_ERROR_UNDEFINED, /**< Error number undefined. */
|
||||
RET_WARNING_UNDEFINED, /**< Warning number undefined. */
|
||||
RET_INFO_UNDEFINED, /**< Info number undefined. */
|
||||
RET_EWI_UNDEFINED, /**< Error/warning/info number undefined. */
|
||||
RET_AVAILABLE_WITH_LINUX_ONLY, /**< This function is available under Linux only. */
|
||||
RET_UNKNOWN_BUG, /**< The error occured is not yet known. */
|
||||
RET_PRINTLEVEL_CHANGED, /**< 10 Print level changed. */
|
||||
RET_NOT_YET_IMPLEMENTED, /**< Requested function is not yet implemented in this version of qpOASES. */
|
||||
/* Indexlist */
|
||||
RET_INDEXLIST_MUST_BE_REORDERD, /**< Index list has to be reordered. */
|
||||
RET_INDEXLIST_EXCEEDS_MAX_LENGTH, /**< Index list exceeds its maximal physical length. */
|
||||
RET_INDEXLIST_CORRUPTED, /**< Index list corrupted. */
|
||||
RET_INDEXLIST_OUTOFBOUNDS, /**< Physical index is out of bounds. */
|
||||
RET_INDEXLIST_ADD_FAILED, /**< Adding indices from another index set failed. */
|
||||
RET_INDEXLIST_INTERSECT_FAILED, /**< Intersection with another index set failed. */
|
||||
/* SubjectTo / Bounds / Constraints */
|
||||
RET_INDEX_ALREADY_OF_DESIRED_STATUS, /**< Index is already of desired status. */
|
||||
RET_ADDINDEX_FAILED, /**< Cannot swap between different indexsets. */
|
||||
RET_SWAPINDEX_FAILED, /**< 20 Adding index to index set failed. */
|
||||
RET_NOTHING_TO_DO, /**< Nothing to do. */
|
||||
RET_SETUP_BOUND_FAILED, /**< Setting up bound index failed. */
|
||||
RET_SETUP_CONSTRAINT_FAILED, /**< Setting up constraint index failed. */
|
||||
RET_MOVING_BOUND_FAILED, /**< Moving bound between index sets failed. */
|
||||
RET_MOVING_CONSTRAINT_FAILED, /**< Moving constraint between index sets failed. */
|
||||
/* QProblem */
|
||||
RET_QP_ALREADY_INITIALISED, /**< QProblem has already been initialised. */
|
||||
RET_NO_INIT_WITH_STANDARD_SOLVER, /**< Initialisation via extern QP solver is not yet implemented. */
|
||||
RET_RESET_FAILED, /**< Reset failed. */
|
||||
RET_INIT_FAILED, /**< Initialisation failed. */
|
||||
RET_INIT_FAILED_TQ, /**< 30 Initialisation failed due to TQ factorisation. */
|
||||
RET_INIT_FAILED_CHOLESKY, /**< Initialisation failed due to Cholesky decomposition. */
|
||||
RET_INIT_FAILED_HOTSTART, /**< Initialisation failed! QP could not be solved! */
|
||||
RET_INIT_FAILED_INFEASIBILITY, /**< Initial QP could not be solved due to infeasibility! */
|
||||
RET_INIT_FAILED_UNBOUNDEDNESS, /**< Initial QP could not be solved due to unboundedness! */
|
||||
RET_INIT_SUCCESSFUL, /**< Initialisation done. */
|
||||
RET_OBTAINING_WORKINGSET_FAILED, /**< Failed to obtain working set for auxiliary QP. */
|
||||
RET_SETUP_WORKINGSET_FAILED, /**< Failed to setup working set for auxiliary QP. */
|
||||
RET_SETUP_AUXILIARYQP_FAILED, /**< Failed to setup auxiliary QP for initialised homotopy. */
|
||||
RET_NO_EXTERN_SOLVER, /**< No extern QP solver available. */
|
||||
RET_QP_UNBOUNDED, /**< 40 QP is unbounded. */
|
||||
RET_QP_INFEASIBLE, /**< QP is infeasible. */
|
||||
RET_QP_NOT_SOLVED, /**< Problems occured while solving QP with standard solver. */
|
||||
RET_QP_SOLVED, /**< QP successfully solved. */
|
||||
RET_UNABLE_TO_SOLVE_QP, /**< Problems occured while solving QP. */
|
||||
RET_INITIALISATION_STARTED, /**< Starting problem initialisation. */
|
||||
RET_HOTSTART_FAILED, /**< Unable to perform homotopy due to internal error. */
|
||||
RET_HOTSTART_FAILED_TO_INIT, /**< Unable to initialise problem. */
|
||||
RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED, /**< Unable to perform homotopy as previous QP is not solved. */
|
||||
RET_ITERATION_STARTED, /**< Iteration... */
|
||||
RET_SHIFT_DETERMINATION_FAILED, /**< 50 Determination of shift of the QP data failed. */
|
||||
RET_STEPDIRECTION_DETERMINATION_FAILED, /**< Determination of step direction failed. */
|
||||
RET_STEPLENGTH_DETERMINATION_FAILED, /**< Determination of step direction failed. */
|
||||
RET_OPTIMAL_SOLUTION_FOUND, /**< Optimal solution of neighbouring QP found. */
|
||||
RET_HOMOTOPY_STEP_FAILED, /**< Unable to perform homotopy step. */
|
||||
RET_HOTSTART_STOPPED_INFEASIBILITY, /**< Premature homotopy termination because QP is infeasible. */
|
||||
RET_HOTSTART_STOPPED_UNBOUNDEDNESS, /**< Premature homotopy termination because QP is unbounded. */
|
||||
RET_WORKINGSET_UPDATE_FAILED, /**< Unable to update working sets according to initial guesses. */
|
||||
RET_MAX_NWSR_REACHED, /**< Maximum number of working set recalculations performed. */
|
||||
RET_CONSTRAINTS_NOT_SPECIFIED, /**< Problem does comprise constraints! You also have to specify new constraints' bounds. */
|
||||
RET_INVALID_FACTORISATION_FLAG, /**< 60 Invalid factorisation flag. */
|
||||
RET_UNABLE_TO_SAVE_QPDATA, /**< Unable to save QP data. */
|
||||
RET_STEPDIRECTION_FAILED_TQ, /**< Abnormal termination due to TQ factorisation. */
|
||||
RET_STEPDIRECTION_FAILED_CHOLESKY, /**< Abnormal termination due to Cholesky factorisation. */
|
||||
RET_CYCLING_DETECTED, /**< Cycling detected. */
|
||||
RET_CYCLING_NOT_RESOLVED, /**< Cycling cannot be resolved, QP probably infeasible. */
|
||||
RET_CYCLING_RESOLVED, /**< Cycling probably resolved. */
|
||||
RET_STEPSIZE, /**< For displaying performed stepsize. */
|
||||
RET_STEPSIZE_NONPOSITIVE, /**< For displaying non-positive stepsize. */
|
||||
RET_SETUPSUBJECTTOTYPE_FAILED, /**< Setup of SubjectToTypes failed. */
|
||||
RET_ADDCONSTRAINT_FAILED, /**< 70 Addition of constraint to working set failed. */
|
||||
RET_ADDCONSTRAINT_FAILED_INFEASIBILITY, /**< Addition of constraint to working set failed (due to QP infeasibility). */
|
||||
RET_ADDBOUND_FAILED, /**< Addition of bound to working set failed. */
|
||||
RET_ADDBOUND_FAILED_INFEASIBILITY, /**< Addition of bound to working set failed (due to QP infeasibility). */
|
||||
RET_REMOVECONSTRAINT_FAILED, /**< Removal of constraint from working set failed. */
|
||||
RET_REMOVEBOUND_FAILED, /**< Removal of bound from working set failed. */
|
||||
RET_REMOVE_FROM_ACTIVESET, /**< Removing from active set... */
|
||||
RET_ADD_TO_ACTIVESET, /**< Adding to active set... */
|
||||
RET_REMOVE_FROM_ACTIVESET_FAILED, /**< Removing from active set failed. */
|
||||
RET_ADD_TO_ACTIVESET_FAILED, /**< Adding to active set failed. */
|
||||
RET_CONSTRAINT_ALREADY_ACTIVE, /**< 80 Constraint is already active. */
|
||||
RET_ALL_CONSTRAINTS_ACTIVE, /**< All constraints are active, no further constraint can be added. */
|
||||
RET_LINEARLY_DEPENDENT, /**< New bound/constraint is linearly dependent. */
|
||||
RET_LINEARLY_INDEPENDENT, /**< New bound/constraint is linearly independent. */
|
||||
RET_LI_RESOLVED, /**< Linear independence of active contraint matrix successfully resolved. */
|
||||
RET_ENSURELI_FAILED, /**< Failed to ensure linear indepence of active contraint matrix. */
|
||||
RET_ENSURELI_FAILED_TQ, /**< Abnormal termination due to TQ factorisation. */
|
||||
RET_ENSURELI_FAILED_NOINDEX, /**< No index found, QP probably infeasible. */
|
||||
RET_ENSURELI_FAILED_CYCLING, /**< Cycling detected, QP probably infeasible. */
|
||||
RET_BOUND_ALREADY_ACTIVE, /**< Bound is already active. */
|
||||
RET_ALL_BOUNDS_ACTIVE, /**< 90 All bounds are active, no further bound can be added. */
|
||||
RET_CONSTRAINT_NOT_ACTIVE, /**< Constraint is not active. */
|
||||
RET_BOUND_NOT_ACTIVE, /**< Bound is not active. */
|
||||
RET_HESSIAN_NOT_SPD, /**< Projected Hessian matrix not positive definite. */
|
||||
RET_MATRIX_SHIFT_FAILED, /**< Unable to update matrices or to transform vectors. */
|
||||
RET_MATRIX_FACTORISATION_FAILED, /**< Unable to calculate new matrix factorisations. */
|
||||
RET_PRINT_ITERATION_FAILED, /**< Unable to print information on current iteration. */
|
||||
RET_NO_GLOBAL_MESSAGE_OUTPUTFILE, /**< No global message output file initialised. */
|
||||
/* Utils */
|
||||
RET_UNABLE_TO_OPEN_FILE, /**< Unable to open file. */
|
||||
RET_UNABLE_TO_WRITE_FILE, /**< Unable to write into file. */
|
||||
RET_UNABLE_TO_READ_FILE, /**< 100 Unable to read from file. */
|
||||
RET_FILEDATA_INCONSISTENT, /**< File contains inconsistent data. */
|
||||
/* SolutionAnalysis */
|
||||
RET_NO_SOLUTION, /**< QP solution does not satisfy KKT optimality conditions. */
|
||||
RET_INACCURATE_SOLUTION /**< KKT optimality conditions not satisfied to sufficient accuracy. */
|
||||
};
|
||||
|
||||
|
||||
|
||||
/** This class handles all kinds of messages (errors, warnings, infos) initiated
|
||||
* by qpOASES modules and stores the correspoding global preferences.
|
||||
*
|
||||
* \author Hans Joachim Ferreau (special thanks to Leonard Wirsching)
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*/
|
||||
class MessageHandling
|
||||
{
|
||||
/*
|
||||
* INTERNAL DATA STRUCTURES
|
||||
*/
|
||||
public:
|
||||
/** Data structure for entries in global message list. */
|
||||
typedef struct {
|
||||
returnValue key; /**< Global return value. */
|
||||
const char* data; /**< Corresponding message. */
|
||||
VisibilityStatus globalVisibilityStatus; /**< Determines if message can be printed.
|
||||
* If this value is set to VS_HIDDEN, no message is printed! */
|
||||
} ReturnValueList;
|
||||
|
||||
|
||||
/*
|
||||
* PUBLIC MEMBER FUNCTIONS
|
||||
*/
|
||||
public:
|
||||
/** Default constructor. */
|
||||
MessageHandling( );
|
||||
|
||||
/** Constructor which takes the desired output file. */
|
||||
MessageHandling( myFILE* _outputFile /**< Output file. */
|
||||
);
|
||||
|
||||
/** Constructor which takes the desired visibility states. */
|
||||
MessageHandling( VisibilityStatus _errorVisibility, /**< Visibility status for error messages. */
|
||||
VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */
|
||||
VisibilityStatus _infoVisibility /**< Visibility status for info messages. */
|
||||
);
|
||||
|
||||
/** Constructor which takes the desired output file and desired visibility states. */
|
||||
MessageHandling( myFILE* _outputFile, /**< Output file. */
|
||||
VisibilityStatus _errorVisibility, /**< Visibility status for error messages. */
|
||||
VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */
|
||||
VisibilityStatus _infoVisibility /**< Visibility status for info messages. */
|
||||
);
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
MessageHandling( const MessageHandling& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~MessageHandling( );
|
||||
|
||||
/** Assignment operator (deep copy). */
|
||||
MessageHandling& operator=( const MessageHandling& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
|
||||
/** Prints an error message(a simplified macro THROWERROR is also provided). \n
|
||||
* Errors are definied as abnormal events which cause an immediate termination of the current (sub) function.
|
||||
* Errors of a sub function should be commented by the calling function by means of a warning message
|
||||
* (if this error does not cause an error of the calling function, either)!
|
||||
* \return Error number returned by sub function call
|
||||
*/
|
||||
returnValue throwError(
|
||||
returnValue Enumber, /**< Error number returned by sub function call. */
|
||||
const char* additionaltext, /**< Additional error text (0, if none). */
|
||||
const char* functionname, /**< Name of function which caused the error. */
|
||||
const char* filename, /**< Name of file which caused the error. */
|
||||
const unsigned long linenumber, /**< Number of line which caused the error.incompatible binary file */
|
||||
VisibilityStatus localVisibilityStatus /**< Determines (locally) if error message can be printed to myStderr.
|
||||
* If GLOBAL visibility status of the message is set to VS_HIDDEN,
|
||||
* no message is printed, anyway! */
|
||||
);
|
||||
|
||||
/** Prints a warning message (a simplified macro THROWWARNING is also provided).
|
||||
* Warnings are definied as abnormal events which does NOT cause an immediate termination of the current (sub) function.
|
||||
* \return Warning number returned by sub function call
|
||||
*/
|
||||
returnValue throwWarning(
|
||||
returnValue Wnumber, /**< Warning number returned by sub function call. */
|
||||
const char* additionaltext, /**< Additional warning text (0, if none). */
|
||||
const char* functionname, /**< Name of function which caused the warning. */
|
||||
const char* filename, /**< Name of file which caused the warning. */
|
||||
const unsigned long linenumber, /**< Number of line which caused the warning. */
|
||||
VisibilityStatus localVisibilityStatus /**< Determines (locally) if warning message can be printed to myStderr.
|
||||
* If GLOBAL visibility status of the message is set to VS_HIDDEN,
|
||||
* no message is printed, anyway! */
|
||||
);
|
||||
|
||||
/** Prints a info message (a simplified macro THROWINFO is also provided).
|
||||
* \return Info number returned by sub function call
|
||||
*/
|
||||
returnValue throwInfo(
|
||||
returnValue Inumber, /**< Info number returned by sub function call. */
|
||||
const char* additionaltext, /**< Additional warning text (0, if none). */
|
||||
const char* functionname, /**< Name of function which submitted the info. */
|
||||
const char* filename, /**< Name of file which submitted the info. */
|
||||
const unsigned long linenumber, /**< Number of line which submitted the info. */
|
||||
VisibilityStatus localVisibilityStatus /**< Determines (locally) if info message can be printed to myStderr.
|
||||
* If GLOBAL visibility status of the message is set to VS_HIDDEN,
|
||||
* no message is printed, anyway! */
|
||||
);
|
||||
|
||||
|
||||
/** Resets all preferences to default values.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue reset( );
|
||||
|
||||
|
||||
/** Prints a complete list of all messages to output file.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue listAllMessages( );
|
||||
|
||||
|
||||
/** Returns visibility status for error messages.
|
||||
* \return Visibility status for error messages. */
|
||||
inline VisibilityStatus getErrorVisibilityStatus( ) const;
|
||||
|
||||
/** Returns visibility status for warning messages.
|
||||
* \return Visibility status for warning messages. */
|
||||
inline VisibilityStatus getWarningVisibilityStatus( ) const;
|
||||
|
||||
/** Returns visibility status for info messages.
|
||||
* \return Visibility status for info messages. */
|
||||
inline VisibilityStatus getInfoVisibilityStatus( ) const;
|
||||
|
||||
/** Returns pointer to output file.
|
||||
* \return Pointer to output file. */
|
||||
inline myFILE* getOutputFile( ) const;
|
||||
|
||||
/** Returns error count value.
|
||||
* \return Error count value. */
|
||||
inline int getErrorCount( ) const;
|
||||
|
||||
|
||||
/** Changes visibility status for error messages. */
|
||||
inline void setErrorVisibilityStatus( VisibilityStatus _errorVisibility /**< New visibility status for error messages. */
|
||||
);
|
||||
|
||||
/** Changes visibility status for warning messages. */
|
||||
inline void setWarningVisibilityStatus( VisibilityStatus _warningVisibility /**< New visibility status for warning messages. */
|
||||
);
|
||||
|
||||
/** Changes visibility status for info messages. */
|
||||
inline void setInfoVisibilityStatus( VisibilityStatus _infoVisibility /**< New visibility status for info messages. */
|
||||
);
|
||||
|
||||
/** Changes output file for messages. */
|
||||
inline void setOutputFile( myFILE* _outputFile /**< New output file for messages. */
|
||||
);
|
||||
|
||||
/** Changes error count.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
* RET_INVALID_ARGUMENT */
|
||||
inline returnValue setErrorCount( int _errorCount /**< New error count value. */
|
||||
);
|
||||
|
||||
/** Return the error code string. */
|
||||
static const char* getErrorString(int error);
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER FUNCTIONS
|
||||
*/
|
||||
protected:
|
||||
/** Prints a info message to myStderr (auxiliary function).
|
||||
* \return Error/warning/info number returned by sub function call
|
||||
*/
|
||||
returnValue throwMessage(
|
||||
returnValue RETnumber, /**< Error/warning/info number returned by sub function call. */
|
||||
const char* additionaltext, /**< Additional warning text (0, if none). */
|
||||
const char* functionname, /**< Name of function which caused the error/warning/info. */
|
||||
const char* filename, /**< Name of file which caused the error/warning/info. */
|
||||
const unsigned long linenumber, /**< Number of line which caused the error/warning/info. */
|
||||
VisibilityStatus localVisibilityStatus, /**< Determines (locally) if info message can be printed to myStderr.
|
||||
* If GLOBAL visibility status of the message is set to VS_HIDDEN,
|
||||
* no message is printed, anyway! */
|
||||
const char* RETstring /**< Leading string of error/warning/info message. */
|
||||
);
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER VARIABLES
|
||||
*/
|
||||
protected:
|
||||
VisibilityStatus errorVisibility; /**< Error messages visible? */
|
||||
VisibilityStatus warningVisibility; /**< Warning messages visible? */
|
||||
VisibilityStatus infoVisibility; /**< Info messages visible? */
|
||||
|
||||
myFILE* outputFile; /**< Output file for messages. */
|
||||
|
||||
int errorCount; /**< Counts number of errors (for nicer output only). */
|
||||
};
|
||||
|
||||
|
||||
#ifndef __FUNCTION__
|
||||
/** Ensures that __FUNCTION__ macro is defined. */
|
||||
#define __FUNCTION__ 0
|
||||
#endif
|
||||
|
||||
#ifndef __FILE__
|
||||
/** Ensures that __FILE__ macro is defined. */
|
||||
#define __FILE__ 0
|
||||
#endif
|
||||
|
||||
#ifndef __LINE__
|
||||
/** Ensures that __LINE__ macro is defined. */
|
||||
#define __LINE__ 0
|
||||
#endif
|
||||
|
||||
|
||||
/** Short version of throwError with default values, only returnValue is needed */
|
||||
#define THROWERROR(retval) ( getGlobalMessageHandler( )->throwError((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
|
||||
|
||||
/** Short version of throwWarning with default values, only returnValue is needed */
|
||||
#define THROWWARNING(retval) ( getGlobalMessageHandler( )->throwWarning((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
|
||||
|
||||
/** Short version of throwInfo with default values, only returnValue is needed */
|
||||
#define THROWINFO(retval) ( getGlobalMessageHandler( )->throwInfo((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
|
||||
|
||||
|
||||
/** Returns a pointer to global message handler.
|
||||
* \return Pointer to global message handler.
|
||||
*/
|
||||
MessageHandling* getGlobalMessageHandler( );
|
||||
|
||||
|
||||
#include <MessageHandling.ipp>
|
||||
|
||||
#endif /* QPOASES_MESSAGEHANDLING_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,666 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/QProblem.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of the QProblem class which is able to use the newly
|
||||
* developed online active set strategy for parametric quadratic programming.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifndef QPOASES_QPROBLEM_HPP
|
||||
#define QPOASES_QPROBLEM_HPP
|
||||
|
||||
|
||||
#include <QProblemB.hpp>
|
||||
#include <Constraints.hpp>
|
||||
#include <CyclingManager.hpp>
|
||||
|
||||
|
||||
/** A class for setting up and solving quadratic programs. The main feature is
|
||||
* the possibily to use the newly developed online active set strategy for
|
||||
* parametric quadratic programming.
|
||||
*
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*/
|
||||
class QProblem : public QProblemB
|
||||
{
|
||||
/* allow SolutionAnalysis class to access private members */
|
||||
friend class SolutionAnalysis;
|
||||
|
||||
/*
|
||||
* PUBLIC MEMBER FUNCTIONS
|
||||
*/
|
||||
public:
|
||||
/** Default constructor. */
|
||||
QProblem( );
|
||||
|
||||
/** Constructor which takes the QP dimensions only. */
|
||||
QProblem( int _nV, /**< Number of variables. */
|
||||
int _nC /**< Number of constraints. */
|
||||
);
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
QProblem( const QProblem& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~QProblem( );
|
||||
|
||||
/** Assignment operator (deep copy). */
|
||||
QProblem& operator=( const QProblem& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
|
||||
/** Clears all data structures of QProblemB except for QP data.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_RESET_FAILED */
|
||||
returnValue reset( );
|
||||
|
||||
|
||||
/** Initialises a QProblem with given QP data and solves it
|
||||
* using an initial homotopy with empty working set (at most nWSR iterations).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INIT_FAILED \n
|
||||
RET_INIT_FAILED_CHOLESKY \n
|
||||
RET_INIT_FAILED_TQ \n
|
||||
RET_INIT_FAILED_HOTSTART \n
|
||||
RET_INIT_FAILED_INFEASIBILITY \n
|
||||
RET_INIT_FAILED_UNBOUNDEDNESS \n
|
||||
RET_MAX_NWSR_REACHED \n
|
||||
RET_INVALID_ARGUMENTS \n
|
||||
RET_INACCURATE_SOLUTION \n
|
||||
RET_NO_SOLUTION */
|
||||
returnValue init( const real_t* const _H, /**< Hessian matrix. */
|
||||
const real_t* const _g, /**< Gradient vector. */
|
||||
const real_t* const _A, /**< Constraint matrix. */
|
||||
const real_t* const _lb, /**< Lower bound vector (on variables). \n
|
||||
If no lower bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ub, /**< Upper bound vector (on variables). \n
|
||||
If no upper bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _lbA, /**< Lower constraints' bound vector. \n
|
||||
If no lower constraints' bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ubA, /**< Upper constraints' bound vector. \n
|
||||
If no lower constraints' bounds exist, a NULL pointer can be passed. */
|
||||
int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy.
|
||||
Output: Number of performed working set recalculations. */
|
||||
const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */
|
||||
real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */
|
||||
);
|
||||
|
||||
|
||||
/** Initialises a QProblem with given QP data and solves it
|
||||
* using an initial homotopy with empty working set (at most nWSR iterations).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INIT_FAILED \n
|
||||
RET_INIT_FAILED_CHOLESKY \n
|
||||
RET_INIT_FAILED_TQ \n
|
||||
RET_INIT_FAILED_HOTSTART \n
|
||||
RET_INIT_FAILED_INFEASIBILITY \n
|
||||
RET_INIT_FAILED_UNBOUNDEDNESS \n
|
||||
RET_MAX_NWSR_REACHED \n
|
||||
RET_INVALID_ARGUMENTS \n
|
||||
RET_INACCURATE_SOLUTION \n
|
||||
RET_NO_SOLUTION */
|
||||
returnValue init( const real_t* const _H, /**< Hessian matrix. */
|
||||
const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */
|
||||
const real_t* const _g, /**< Gradient vector. */
|
||||
const real_t* const _A, /**< Constraint matrix. */
|
||||
const real_t* const _lb, /**< Lower bound vector (on variables). \n
|
||||
If no lower bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ub, /**< Upper bound vector (on variables). \n
|
||||
If no upper bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _lbA, /**< Lower constraints' bound vector. \n
|
||||
If no lower constraints' bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ubA, /**< Upper constraints' bound vector. \n
|
||||
If no lower constraints' bounds exist, a NULL pointer can be passed. */
|
||||
int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy.
|
||||
Output: Number of performed working set recalculations. */
|
||||
const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */
|
||||
real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */
|
||||
);
|
||||
|
||||
|
||||
/** Solves QProblem using online active set strategy.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_MAX_NWSR_REACHED \n
|
||||
RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
|
||||
RET_HOTSTART_FAILED \n
|
||||
RET_SHIFT_DETERMINATION_FAILED \n
|
||||
RET_STEPDIRECTION_DETERMINATION_FAILED \n
|
||||
RET_STEPLENGTH_DETERMINATION_FAILED \n
|
||||
RET_HOMOTOPY_STEP_FAILED \n
|
||||
RET_HOTSTART_STOPPED_INFEASIBILITY \n
|
||||
RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
|
||||
RET_INACCURATE_SOLUTION \n
|
||||
RET_NO_SOLUTION */
|
||||
returnValue hotstart( const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */
|
||||
const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n
|
||||
If no lower bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n
|
||||
If no upper bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n
|
||||
If no lower constraints' bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n
|
||||
If no upper constraints' bounds exist, a NULL pointer can be passed. */
|
||||
int& nWSR, /**< Input: Maximum number of working set recalculations; \n
|
||||
Output: Number of performed working set recalculations. */
|
||||
real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
|
||||
);
|
||||
|
||||
|
||||
/** Returns constraint matrix of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getA( real_t* const _A /**< Array of appropriate dimension for copying constraint matrix.*/
|
||||
) const;
|
||||
|
||||
/** Returns a single row of constraint matrix of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue getA( int number, /**< Number of entry to be returned. */
|
||||
real_t* const row /**< Array of appropriate dimension for copying (number)th constraint. */
|
||||
) const;
|
||||
|
||||
/** Returns lower constraints' bound vector of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getLBA( real_t* const _lbA /**< Array of appropriate dimension for copying lower constraints' bound vector.*/
|
||||
) const;
|
||||
|
||||
/** Returns single entry of lower constraints' bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue getLBA( int number, /**< Number of entry to be returned. */
|
||||
real_t& value /**< Output: lbA[number].*/
|
||||
) const;
|
||||
|
||||
/** Returns upper constraints' bound vector of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getUBA( real_t* const _ubA /**< Array of appropriate dimension for copying upper constraints' bound vector.*/
|
||||
) const;
|
||||
|
||||
/** Returns single entry of upper constraints' bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue getUBA( int number, /**< Number of entry to be returned. */
|
||||
real_t& value /**< Output: ubA[number].*/
|
||||
) const;
|
||||
|
||||
|
||||
/** Returns current constraints object of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getConstraints( Constraints* const _constraints /** Output: Constraints object. */
|
||||
) const;
|
||||
|
||||
|
||||
/** Returns the number of constraints.
|
||||
* \return Number of constraints. */
|
||||
inline int getNC( ) const;
|
||||
|
||||
/** Returns the number of (implicitly defined) equality constraints.
|
||||
* \return Number of (implicitly defined) equality constraints. */
|
||||
inline int getNEC( ) const;
|
||||
|
||||
/** Returns the number of active constraints.
|
||||
* \return Number of active constraints. */
|
||||
inline int getNAC( );
|
||||
|
||||
/** Returns the number of inactive constraints.
|
||||
* \return Number of inactive constraints. */
|
||||
inline int getNIAC( );
|
||||
|
||||
/** Returns the dimension of null space.
|
||||
* \return Dimension of null space. */
|
||||
int getNZ( );
|
||||
|
||||
|
||||
/** Returns the dual solution vector (deep copy).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_QP_NOT_SOLVED */
|
||||
returnValue getDualSolution( real_t* const yOpt /**< Output: Dual solution vector (if QP has been solved). */
|
||||
) const;
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER FUNCTIONS
|
||||
*/
|
||||
protected:
|
||||
/** Determines type of constraints and bounds (i.e. implicitly fixed, unbounded etc.).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SETUPSUBJECTTOTYPE_FAILED */
|
||||
returnValue setupSubjectToType( );
|
||||
|
||||
/** Computes the Cholesky decomposition R of the projected Hessian (i.e. R^T*R = Z^T*H*Z).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
* RET_INDEXLIST_CORRUPTED */
|
||||
returnValue setupCholeskyDecompositionProjected( );
|
||||
|
||||
/** Initialises TQ factorisation of A (i.e. A*Q = [0 T]) if NO constraint is active.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEXLIST_CORRUPTED */
|
||||
returnValue setupTQfactorisation( );
|
||||
|
||||
|
||||
/** Solves a QProblem whose QP data is assumed to be stored in the member variables.
|
||||
* A guess for its primal/dual optimal solution vectors and the corresponding
|
||||
* working sets of bounds and constraints can be provided.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INIT_FAILED \n
|
||||
RET_INIT_FAILED_CHOLESKY \n
|
||||
RET_INIT_FAILED_TQ \n
|
||||
RET_INIT_FAILED_HOTSTART \n
|
||||
RET_INIT_FAILED_INFEASIBILITY \n
|
||||
RET_INIT_FAILED_UNBOUNDEDNESS \n
|
||||
RET_MAX_NWSR_REACHED */
|
||||
returnValue solveInitialQP( const real_t* const xOpt, /**< Optimal primal solution vector.
|
||||
* A NULL pointer can be passed. */
|
||||
const real_t* const yOpt, /**< Optimal dual solution vector.
|
||||
* A NULL pointer can be passed. */
|
||||
const Bounds* const guessedBounds, /**< Guessed working set of bounds for solution (xOpt,yOpt).
|
||||
* A NULL pointer can be passed. */
|
||||
const Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt).
|
||||
* A NULL pointer can be passed. */
|
||||
int& nWSR, /**< Input: Maximum number of working set recalculations; \n
|
||||
* Output: Number of performed working set recalculations. */
|
||||
real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
|
||||
);
|
||||
|
||||
/** Obtains the desired working set for the auxiliary initial QP in
|
||||
* accordance with the user specifications
|
||||
* (assumes that member AX has already been initialised!)
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_OBTAINING_WORKINGSET_FAILED \n
|
||||
RET_INVALID_ARGUMENTS */
|
||||
returnValue obtainAuxiliaryWorkingSet( const real_t* const xOpt, /**< Optimal primal solution vector.
|
||||
* If a NULL pointer is passed, all entries are assumed to be zero. */
|
||||
const real_t* const yOpt, /**< Optimal dual solution vector.
|
||||
* If a NULL pointer is passed, all entries are assumed to be zero. */
|
||||
const Bounds* const guessedBounds, /**< Guessed working set of bounds for solution (xOpt,yOpt). */
|
||||
const Constraints* const guessedConstraints, /**< Guessed working set for solution (xOpt,yOpt). */
|
||||
Bounds* auxiliaryBounds, /**< Input: Allocated bound object. \n
|
||||
* Ouput: Working set of constraints for auxiliary QP. */
|
||||
Constraints* auxiliaryConstraints /**< Input: Allocated bound object. \n
|
||||
* Ouput: Working set for auxiliary QP. */
|
||||
) const;
|
||||
|
||||
/** Setups bound and constraints data structures according to auxiliaryBounds/Constraints.
|
||||
* (If the working set shall be setup afresh, make sure that
|
||||
* bounds and constraints data structure have been resetted
|
||||
* and the TQ factorisation has been initialised!)
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SETUP_WORKINGSET_FAILED \n
|
||||
RET_INVALID_ARGUMENTS \n
|
||||
RET_UNKNOWN BUG */
|
||||
returnValue setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */
|
||||
const Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */
|
||||
BooleanType setupAfresh /**< Flag indicating if given working set shall be
|
||||
* setup afresh or by updating the current one. */
|
||||
);
|
||||
|
||||
/** Setups the optimal primal/dual solution of the auxiliary initial QP.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue setupAuxiliaryQPsolution( const real_t* const xOpt, /**< Optimal primal solution vector.
|
||||
* If a NULL pointer is passed, all entries are set to zero. */
|
||||
const real_t* const yOpt /**< Optimal dual solution vector.
|
||||
* If a NULL pointer is passed, all entries are set to zero. */
|
||||
);
|
||||
|
||||
/** Setups gradient of the auxiliary initial QP for given
|
||||
* optimal primal/dual solution and given initial working set
|
||||
* (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue setupAuxiliaryQPgradient( );
|
||||
|
||||
/** Setups (constraints') bounds of the auxiliary initial QP for given
|
||||
* optimal primal/dual solution and given initial working set
|
||||
* (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNKNOWN BUG */
|
||||
returnValue setupAuxiliaryQPbounds( const Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */
|
||||
const Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */
|
||||
BooleanType useRelaxation /**< Flag indicating if inactive (constraints') bounds shall be relaxed. */
|
||||
);
|
||||
|
||||
|
||||
/** Adds a constraint to active set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_ADDCONSTRAINT_FAILED \n
|
||||
RET_ADDCONSTRAINT_FAILED_INFEASIBILITY \n
|
||||
RET_ENSURELI_FAILED */
|
||||
returnValue addConstraint( int number, /**< Number of constraint to be added to active set. */
|
||||
SubjectToStatus C_status, /**< Status of new active constraint. */
|
||||
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
|
||||
);
|
||||
|
||||
/** Checks if new active constraint to be added is linearly dependent from
|
||||
* from row of the active constraints matrix.
|
||||
* \return RET_LINEARLY_DEPENDENT \n
|
||||
RET_LINEARLY_INDEPENDENT \n
|
||||
RET_INDEXLIST_CORRUPTED */
|
||||
returnValue addConstraint_checkLI( int number /**< Number of constraint to be added to active set. */
|
||||
);
|
||||
|
||||
/** Ensures linear independence of constraint matrix when a new constraint is added.
|
||||
* To this end a bound or constraint is removed simultaneously if necessary.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_LI_RESOLVED \n
|
||||
RET_ENSURELI_FAILED \n
|
||||
RET_ENSURELI_FAILED_TQ \n
|
||||
RET_ENSURELI_FAILED_NOINDEX \n
|
||||
RET_REMOVE_FROM_ACTIVESET */
|
||||
returnValue addConstraint_ensureLI( int number, /**< Number of constraint to be added to active set. */
|
||||
SubjectToStatus C_status /**< Status of new active bound. */
|
||||
);
|
||||
|
||||
/** Adds a bound to active set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_ADDBOUND_FAILED \n
|
||||
RET_ADDBOUND_FAILED_INFEASIBILITY \n
|
||||
RET_ENSURELI_FAILED */
|
||||
returnValue addBound( int number, /**< Number of bound to be added to active set. */
|
||||
SubjectToStatus B_status, /**< Status of new active bound. */
|
||||
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
|
||||
);
|
||||
|
||||
/** Checks if new active bound to be added is linearly dependent from
|
||||
* from row of the active constraints matrix.
|
||||
* \return RET_LINEARLY_DEPENDENT \n
|
||||
RET_LINEARLY_INDEPENDENT */
|
||||
returnValue addBound_checkLI( int number /**< Number of bound to be added to active set. */
|
||||
);
|
||||
|
||||
/** Ensures linear independence of constraint matrix when a new bound is added.
|
||||
* To this end a bound or constraint is removed simultaneously if necessary.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_LI_RESOLVED \n
|
||||
RET_ENSURELI_FAILED \n
|
||||
RET_ENSURELI_FAILED_TQ \n
|
||||
RET_ENSURELI_FAILED_NOINDEX \n
|
||||
RET_REMOVE_FROM_ACTIVESET */
|
||||
returnValue addBound_ensureLI( int number, /**< Number of bound to be added to active set. */
|
||||
SubjectToStatus B_status /**< Status of new active bound. */
|
||||
);
|
||||
|
||||
/** Removes a constraint from active set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_CONSTRAINT_NOT_ACTIVE \n
|
||||
RET_REMOVECONSTRAINT_FAILED \n
|
||||
RET_HESSIAN_NOT_SPD */
|
||||
returnValue removeConstraint( int number, /**< Number of constraint to be removed from active set. */
|
||||
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
|
||||
);
|
||||
|
||||
/** Removes a bounds from active set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_BOUND_NOT_ACTIVE \n
|
||||
RET_HESSIAN_NOT_SPD \n
|
||||
RET_REMOVEBOUND_FAILED */
|
||||
returnValue removeBound( int number, /**< Number of bound to be removed from active set. */
|
||||
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
|
||||
);
|
||||
|
||||
|
||||
/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_DIV_BY_ZERO */
|
||||
returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */
|
||||
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
|
||||
real_t* const a /**< Output: Solution vector */
|
||||
);
|
||||
|
||||
/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n
|
||||
* Special variant for the case that this function is called from within "removeBound()".
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_DIV_BY_ZERO */
|
||||
returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */
|
||||
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
|
||||
BooleanType removingBound, /**< Indicates if function is called from "removeBound()". */
|
||||
real_t* const a /**< Output: Solution vector */
|
||||
);
|
||||
|
||||
|
||||
/** Solves the system Ta = b or T^Ta = b where T is a reverse upper triangular matrix.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_DIV_BY_ZERO */
|
||||
returnValue backsolveT( const real_t* const b, /**< Right hand side vector. */
|
||||
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
|
||||
real_t* const a /**< Output: Solution vector */
|
||||
);
|
||||
|
||||
|
||||
/** Determines step direction of the shift of the QP data.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue hotstart_determineDataShift(const int* const FX_idx, /**< Index array of fixed variables. */
|
||||
const int* const AC_idx, /**< Index array of active constraints. */
|
||||
const real_t* const g_new, /**< New gradient vector. */
|
||||
const real_t* const lbA_new,/**< New lower constraints' bounds. */
|
||||
const real_t* const ubA_new,/**< New upper constraints' bounds. */
|
||||
const real_t* const lb_new, /**< New lower bounds. */
|
||||
const real_t* const ub_new, /**< New upper bounds. */
|
||||
real_t* const delta_g, /**< Output: Step direction of gradient vector. */
|
||||
real_t* const delta_lbA, /**< Output: Step direction of lower constraints' bounds. */
|
||||
real_t* const delta_ubA, /**< Output: Step direction of upper constraints' bounds. */
|
||||
real_t* const delta_lb, /**< Output: Step direction of lower bounds. */
|
||||
real_t* const delta_ub, /**< Output: Step direction of upper bounds. */
|
||||
BooleanType& Delta_bC_isZero,/**< Output: Indicates if active constraints' bounds are to be shifted. */
|
||||
BooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */
|
||||
);
|
||||
|
||||
/** Determines step direction of the homotopy path.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_STEPDIRECTION_FAILED_TQ \n
|
||||
RET_STEPDIRECTION_FAILED_CHOLESKY */
|
||||
returnValue hotstart_determineStepDirection(const int* const FR_idx, /**< Index array of free variables. */
|
||||
const int* const FX_idx, /**< Index array of fixed variables. */
|
||||
const int* const AC_idx, /**< Index array of active constraints. */
|
||||
const real_t* const delta_g, /**< Step direction of gradient vector. */
|
||||
const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */
|
||||
const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */
|
||||
const real_t* const delta_lb, /**< Step direction of lower bounds. */
|
||||
const real_t* const delta_ub, /**< Step direction of upper bounds. */
|
||||
BooleanType Delta_bC_isZero, /**< Indicates if active constraints' bounds are to be shifted. */
|
||||
BooleanType Delta_bB_isZero, /**< Indicates if active bounds are to be shifted. */
|
||||
real_t* const delta_xFX, /**< Output: Primal homotopy step direction of fixed variables. */
|
||||
real_t* const delta_xFR, /**< Output: Primal homotopy step direction of free variables. */
|
||||
real_t* const delta_yAC, /**< Output: Dual homotopy step direction of active constraints' multiplier. */
|
||||
real_t* const delta_yFX /**< Output: Dual homotopy step direction of fixed variables' multiplier. */
|
||||
);
|
||||
|
||||
/** Determines the maximum possible step length along the homotopy path.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue hotstart_determineStepLength( const int* const FR_idx, /**< Index array of free variables. */
|
||||
const int* const FX_idx, /**< Index array of fixed variables. */
|
||||
const int* const AC_idx, /**< Index array of active constraints. */
|
||||
const int* const IAC_idx, /**< Index array of inactive constraints. */
|
||||
const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */
|
||||
const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */
|
||||
const real_t* const delta_lb, /**< Step direction of lower bounds. */
|
||||
const real_t* const delta_ub, /**< Step direction of upper bounds. */
|
||||
const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */
|
||||
const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */
|
||||
const real_t* const delta_yAC, /**< Dual homotopy step direction of active constraints' multiplier. */
|
||||
const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */
|
||||
real_t* const delta_Ax, /**< Output: Step in vector Ax. */
|
||||
int& BC_idx, /**< Output: Index of blocking constraint. */
|
||||
SubjectToStatus& BC_status, /**< Output: Status of blocking constraint. */
|
||||
BooleanType& BC_isBound /**< Output: Indicates if blocking constraint is a bound. */
|
||||
);
|
||||
|
||||
/** Performs a step along the homotopy path (and updates active set).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_OPTIMAL_SOLUTION_FOUND \n
|
||||
RET_REMOVE_FROM_ACTIVESET_FAILED \n
|
||||
RET_ADD_TO_ACTIVESET_FAILED \n
|
||||
RET_QP_INFEASIBLE */
|
||||
returnValue hotstart_performStep( const int* const FR_idx, /**< Index array of free variables. */
|
||||
const int* const FX_idx, /**< Index array of fixed variables. */
|
||||
const int* const AC_idx, /**< Index array of active constraints. */
|
||||
const int* const IAC_idx, /**< Index array of inactive constraints. */
|
||||
const real_t* const delta_g, /**< Step direction of gradient vector. */
|
||||
const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */
|
||||
const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */
|
||||
const real_t* const delta_lb, /**< Step direction of lower bounds. */
|
||||
const real_t* const delta_ub, /**< Step direction of upper bounds. */
|
||||
const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */
|
||||
const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */
|
||||
const real_t* const delta_yAC, /**< Dual homotopy step direction of active constraints' multiplier. */
|
||||
const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */
|
||||
const real_t* const delta_Ax, /**< Step in vector Ax. */
|
||||
int BC_idx, /**< Index of blocking constraint. */
|
||||
SubjectToStatus BC_status, /**< Status of blocking constraint. */
|
||||
BooleanType BC_isBound /**< Indicates if blocking constraint is a bound. */
|
||||
);
|
||||
|
||||
|
||||
/** Checks if lower/upper (constraints') bounds remain consistent
|
||||
* (i.e. if lb <= ub and lbA <= ubA ) during the current step.
|
||||
* \return BT_TRUE iff (constraints") bounds remain consistent
|
||||
*/
|
||||
BooleanType areBoundsConsistent( const real_t* const delta_lb, /**< Step direction of lower bounds. */
|
||||
const real_t* const delta_ub, /**< Step direction of upper bounds. */
|
||||
const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */
|
||||
const real_t* const delta_ubA /**< Step direction of upper constraints' bounds. */
|
||||
) const;
|
||||
|
||||
|
||||
/** Setups internal QP data.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INVALID_ARGUMENTS */
|
||||
returnValue setupQPdata( const real_t* const _H, /**< Hessian matrix. */
|
||||
const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */
|
||||
const real_t* const _g, /**< Gradient vector. */
|
||||
const real_t* const _A, /**< Constraint matrix. */
|
||||
const real_t* const _lb, /**< Lower bound vector (on variables). \n
|
||||
If no lower bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ub, /**< Upper bound vector (on variables). \n
|
||||
If no upper bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _lbA, /**< Lower constraints' bound vector. \n
|
||||
If no lower constraints' bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ubA /**< Upper constraints' bound vector. \n
|
||||
If no lower constraints' bounds exist, a NULL pointer can be passed. */
|
||||
);
|
||||
|
||||
|
||||
#ifdef PC_DEBUG /* Define print functions only for debugging! */
|
||||
|
||||
/** Prints concise information on the current iteration.
|
||||
* \return SUCCESSFUL_RETURN \n */
|
||||
returnValue printIteration( int iteration, /**< Number of current iteration. */
|
||||
int BC_idx, /**< Index of blocking constraint. */
|
||||
SubjectToStatus BC_status, /**< Status of blocking constraint. */
|
||||
BooleanType BC_isBound /**< Indicates if blocking constraint is a bound. */
|
||||
);
|
||||
|
||||
/** Prints concise information on the current iteration.
|
||||
* NOTE: ONLY DEFINED FOR SUPPRESSING A COMPILER WARNING!!
|
||||
* \return SUCCESSFUL_RETURN \n */
|
||||
returnValue printIteration( int iteration, /**< Number of current iteration. */
|
||||
int BC_idx, /**< Index of blocking bound. */
|
||||
SubjectToStatus BC_status /**< Status of blocking bound. */
|
||||
);
|
||||
|
||||
#endif /* PC_DEBUG */
|
||||
|
||||
|
||||
/** Determines the maximum violation of the KKT optimality conditions
|
||||
* of the current iterate within the QProblem object.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
* RET_INACCURATE_SOLUTION \n
|
||||
* RET_NO_SOLUTION */
|
||||
returnValue checkKKTconditions( );
|
||||
|
||||
|
||||
/** Sets constraint matrix of the QP. \n
|
||||
(Remark: Also internal vector Ax is recomputed!)
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setA( const real_t* const A_new /**< New constraint matrix (with correct dimension!). */
|
||||
);
|
||||
|
||||
/** Changes single row of constraint matrix of the QP. \n
|
||||
(Remark: Also correponding component of internal vector Ax is recomputed!)
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue setA( int number, /**< Number of row to be changed. */
|
||||
const real_t* const value /**< New (number)th constraint (with correct dimension!). */
|
||||
);
|
||||
|
||||
|
||||
/** Sets constraints' lower bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setLBA( const real_t* const lbA_new /**< New constraints' lower bound vector (with correct dimension!). */
|
||||
);
|
||||
|
||||
/** Changes single entry of lower constraints' bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue setLBA( int number, /**< Number of entry to be changed. */
|
||||
real_t value /**< New value for entry of lower constraints' bound vector (with correct dimension!). */
|
||||
);
|
||||
|
||||
/** Sets constraints' upper bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setUBA( const real_t* const ubA_new /**< New constraints' upper bound vector (with correct dimension!). */
|
||||
);
|
||||
|
||||
/** Changes single entry of upper constraints' bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue setUBA( int number, /**< Number of entry to be changed. */
|
||||
real_t value /**< New value for entry of upper constraints' bound vector (with correct dimension!). */
|
||||
);
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER VARIABLES
|
||||
*/
|
||||
protected:
|
||||
real_t A[NCMAX_ALLOC*NVMAX]; /**< Constraint matrix. */
|
||||
real_t lbA[NCMAX_ALLOC]; /**< Lower constraints' bound vector. */
|
||||
real_t ubA[NCMAX_ALLOC]; /**< Upper constraints' bound vector. */
|
||||
|
||||
Constraints constraints; /**< Data structure for problem's constraints. */
|
||||
|
||||
real_t T[NVMAX*NVMAX]; /**< Reverse triangular matrix, A = [0 T]*Q'. */
|
||||
real_t Q[NVMAX*NVMAX]; /**< Orthonormal quadratic matrix, A = [0 T]*Q'. */
|
||||
int sizeT; /**< Matrix T is stored in a (sizeT x sizeT) array. */
|
||||
|
||||
real_t Ax[NCMAX_ALLOC]; /**< Stores the current product A*x (for increased efficiency only). */
|
||||
|
||||
CyclingManager cyclingManager; /**< Data structure for storing (possible) cycling information (NOT YET IMPLEMENTED!). */
|
||||
};
|
||||
|
||||
|
||||
#include <QProblem.ipp>
|
||||
|
||||
#endif /* QPOASES_QPROBLEM_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,628 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/QProblemB.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of the QProblemB class which is able to use the newly
|
||||
* developed online active set strategy for parametric quadratic programming
|
||||
* for problems with (simple) bounds only.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifndef QPOASES_QPROBLEMB_HPP
|
||||
#define QPOASES_QPROBLEMB_HPP
|
||||
|
||||
|
||||
#include <Bounds.hpp>
|
||||
|
||||
|
||||
|
||||
class SolutionAnalysis;
|
||||
|
||||
/** Class for setting up and solving quadratic programs with (simple) bounds only.
|
||||
* The main feature is the possibily to use the newly developed online active set strategy
|
||||
* for parametric quadratic programming.
|
||||
*
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*/
|
||||
class QProblemB
|
||||
{
|
||||
/* allow SolutionAnalysis class to access private members */
|
||||
friend class SolutionAnalysis;
|
||||
|
||||
/*
|
||||
* PUBLIC MEMBER FUNCTIONS
|
||||
*/
|
||||
public:
|
||||
/** Default constructor. */
|
||||
QProblemB( );
|
||||
|
||||
/** Constructor which takes the QP dimension only. */
|
||||
QProblemB( int _nV /**< Number of variables. */
|
||||
);
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
QProblemB( const QProblemB& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~QProblemB( );
|
||||
|
||||
/** Assignment operator (deep copy). */
|
||||
QProblemB& operator=( const QProblemB& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
|
||||
/** Clears all data structures of QProblemB except for QP data.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_RESET_FAILED */
|
||||
returnValue reset( );
|
||||
|
||||
|
||||
/** Initialises a QProblemB with given QP data and solves it
|
||||
* using an initial homotopy with empty working set (at most nWSR iterations).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INIT_FAILED \n
|
||||
RET_INIT_FAILED_CHOLESKY \n
|
||||
RET_INIT_FAILED_HOTSTART \n
|
||||
RET_INIT_FAILED_INFEASIBILITY \n
|
||||
RET_INIT_FAILED_UNBOUNDEDNESS \n
|
||||
RET_MAX_NWSR_REACHED \n
|
||||
RET_INVALID_ARGUMENTS \n
|
||||
RET_INACCURATE_SOLUTION \n
|
||||
RET_NO_SOLUTION */
|
||||
returnValue init( const real_t* const _H, /**< Hessian matrix. */
|
||||
const real_t* const _g, /**< Gradient vector. */
|
||||
const real_t* const _lb, /**< Lower bounds (on variables). \n
|
||||
If no lower bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ub, /**< Upper bounds (on variables). \n
|
||||
If no upper bounds exist, a NULL pointer can be passed. */
|
||||
int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n
|
||||
Output: Number of performed working set recalculations. */
|
||||
const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */
|
||||
real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */
|
||||
);
|
||||
|
||||
|
||||
/** Initialises a QProblemB with given QP data and solves it
|
||||
* using an initial homotopy with empty working set (at most nWSR iterations).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INIT_FAILED \n
|
||||
RET_INIT_FAILED_CHOLESKY \n
|
||||
RET_INIT_FAILED_HOTSTART \n
|
||||
RET_INIT_FAILED_INFEASIBILITY \n
|
||||
RET_INIT_FAILED_UNBOUNDEDNESS \n
|
||||
RET_MAX_NWSR_REACHED \n
|
||||
RET_INVALID_ARGUMENTS \n
|
||||
RET_INACCURATE_SOLUTION \n
|
||||
RET_NO_SOLUTION */
|
||||
returnValue init( const real_t* const _H, /**< Hessian matrix. */
|
||||
const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */
|
||||
const real_t* const _g, /**< Gradient vector. */
|
||||
const real_t* const _lb, /**< Lower bounds (on variables). \n
|
||||
If no lower bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ub, /**< Upper bounds (on variables). \n
|
||||
If no upper bounds exist, a NULL pointer can be passed. */
|
||||
int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n
|
||||
Output: Number of performed working set recalculations. */
|
||||
const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */
|
||||
real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */
|
||||
);
|
||||
|
||||
|
||||
/** Solves an initialised QProblemB using online active set strategy.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_MAX_NWSR_REACHED \n
|
||||
RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
|
||||
RET_HOTSTART_FAILED \n
|
||||
RET_SHIFT_DETERMINATION_FAILED \n
|
||||
RET_STEPDIRECTION_DETERMINATION_FAILED \n
|
||||
RET_STEPLENGTH_DETERMINATION_FAILED \n
|
||||
RET_HOMOTOPY_STEP_FAILED \n
|
||||
RET_HOTSTART_STOPPED_INFEASIBILITY \n
|
||||
RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
|
||||
RET_INACCURATE_SOLUTION \n
|
||||
RET_NO_SOLUTION */
|
||||
returnValue hotstart( const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */
|
||||
const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n
|
||||
If no lower bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n
|
||||
If no upper bounds exist, a NULL pointer can be passed. */
|
||||
int& nWSR, /**< Input: Maximum number of working set recalculations; \n
|
||||
Output: Number of performed working set recalculations. */
|
||||
real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
|
||||
);
|
||||
|
||||
|
||||
/** Returns Hessian matrix of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getH( real_t* const _H /**< Array of appropriate dimension for copying Hessian matrix.*/
|
||||
) const;
|
||||
|
||||
/** Returns gradient vector of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getG( real_t* const _g /**< Array of appropriate dimension for copying gradient vector.*/
|
||||
) const;
|
||||
|
||||
/** Returns lower bound vector of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getLB( real_t* const _lb /**< Array of appropriate dimension for copying lower bound vector.*/
|
||||
) const;
|
||||
|
||||
/** Returns single entry of lower bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue getLB( int number, /**< Number of entry to be returned. */
|
||||
real_t& value /**< Output: lb[number].*/
|
||||
) const;
|
||||
|
||||
/** Returns upper bound vector of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getUB( real_t* const _ub /**< Array of appropriate dimension for copying upper bound vector.*/
|
||||
) const;
|
||||
|
||||
/** Returns single entry of upper bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue getUB( int number, /**< Number of entry to be returned. */
|
||||
real_t& value /**< Output: ub[number].*/
|
||||
) const;
|
||||
|
||||
|
||||
/** Returns current bounds object of the QP (deep copy).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue getBounds( Bounds* const _bounds /** Output: Bounds object. */
|
||||
) const;
|
||||
|
||||
|
||||
/** Returns the number of variables.
|
||||
* \return Number of variables. */
|
||||
inline int getNV( ) const;
|
||||
|
||||
/** Returns the number of free variables.
|
||||
* \return Number of free variables. */
|
||||
inline int getNFR( );
|
||||
|
||||
/** Returns the number of fixed variables.
|
||||
* \return Number of fixed variables. */
|
||||
inline int getNFX( );
|
||||
|
||||
/** Returns the number of implicitly fixed variables.
|
||||
* \return Number of implicitly fixed variables. */
|
||||
inline int getNFV( ) const;
|
||||
|
||||
/** Returns the dimension of null space.
|
||||
* \return Dimension of null space. */
|
||||
int getNZ( );
|
||||
|
||||
|
||||
/** Returns the optimal objective function value.
|
||||
* \return finite value: Optimal objective function value (QP was solved) \n
|
||||
+infinity: QP was not yet solved */
|
||||
real_t getObjVal( ) const;
|
||||
|
||||
/** Returns the objective function value at an arbitrary point x.
|
||||
* \return Objective function value at point x */
|
||||
real_t getObjVal( const real_t* const _x /**< Point at which the objective function shall be evaluated. */
|
||||
) const;
|
||||
|
||||
/** Returns the primal solution vector.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_QP_NOT_SOLVED */
|
||||
returnValue getPrimalSolution( real_t* const xOpt /**< Output: Primal solution vector (if QP has been solved). */
|
||||
) const;
|
||||
|
||||
/** Returns the dual solution vector.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_QP_NOT_SOLVED */
|
||||
returnValue getDualSolution( real_t* const yOpt /**< Output: Dual solution vector (if QP has been solved). */
|
||||
) const;
|
||||
|
||||
|
||||
/** Returns status of the solution process.
|
||||
* \return Status of solution process. */
|
||||
inline QProblemStatus getStatus( ) const;
|
||||
|
||||
|
||||
/** Returns if the QProblem object is initialised.
|
||||
* \return BT_TRUE: QProblemB initialised \n
|
||||
BT_FALSE: QProblemB not initialised */
|
||||
inline BooleanType isInitialised( ) const;
|
||||
|
||||
/** Returns if the QP has been solved.
|
||||
* \return BT_TRUE: QProblemB solved \n
|
||||
BT_FALSE: QProblemB not solved */
|
||||
inline BooleanType isSolved( ) const;
|
||||
|
||||
/** Returns if the QP is infeasible.
|
||||
* \return BT_TRUE: QP infeasible \n
|
||||
BT_FALSE: QP feasible (or not known to be infeasible!) */
|
||||
inline BooleanType isInfeasible( ) const;
|
||||
|
||||
/** Returns if the QP is unbounded.
|
||||
* \return BT_TRUE: QP unbounded \n
|
||||
BT_FALSE: QP unbounded (or not known to be unbounded!) */
|
||||
inline BooleanType isUnbounded( ) const;
|
||||
|
||||
|
||||
/** Returns the print level.
|
||||
* \return Print level. */
|
||||
inline PrintLevel getPrintLevel( ) const;
|
||||
|
||||
/** Changes the print level.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue setPrintLevel( PrintLevel _printlevel /**< New print level. */
|
||||
);
|
||||
|
||||
|
||||
/** Returns Hessian type flag (type is not determined due to this call!).
|
||||
* \return Hessian type. */
|
||||
inline HessianType getHessianType( ) const;
|
||||
|
||||
/** Changes the print level.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setHessianType( HessianType _hessianType /**< New Hessian type. */
|
||||
);
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER FUNCTIONS
|
||||
*/
|
||||
protected:
|
||||
/** Checks if Hessian happens to be the identity matrix,
|
||||
* and sets corresponding status flag (otherwise the flag remains unaltered!).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue checkForIdentityHessian( );
|
||||
|
||||
/** Determines type of constraints and bounds (i.e. implicitly fixed, unbounded etc.).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SETUPSUBJECTTOTYPE_FAILED */
|
||||
returnValue setupSubjectToType( );
|
||||
|
||||
/** Computes the Cholesky decomposition R of the (simply projected) Hessian (i.e. R^T*R = Z^T*H*Z).
|
||||
* It only works in the case where Z is a simple projection matrix!
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
* RET_INDEXLIST_CORRUPTED */
|
||||
returnValue setupCholeskyDecomposition( );
|
||||
|
||||
|
||||
/** Solves a QProblemB whose QP data is assumed to be stored in the member variables.
|
||||
* A guess for its primal/dual optimal solution vectors and the corresponding
|
||||
* optimal working set can be provided.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INIT_FAILED \n
|
||||
RET_INIT_FAILED_CHOLESKY \n
|
||||
RET_INIT_FAILED_HOTSTART \n
|
||||
RET_INIT_FAILED_INFEASIBILITY \n
|
||||
RET_INIT_FAILED_UNBOUNDEDNESS \n
|
||||
RET_MAX_NWSR_REACHED */
|
||||
returnValue solveInitialQP( const real_t* const xOpt, /**< Optimal primal solution vector.
|
||||
* A NULL pointer can be passed. */
|
||||
const real_t* const yOpt, /**< Optimal dual solution vector.
|
||||
* A NULL pointer can be passed. */
|
||||
const Bounds* const guessedBounds, /**< Guessed working set for solution (xOpt,yOpt).
|
||||
* A NULL pointer can be passed. */
|
||||
int& nWSR, /**< Input: Maximum number of working set recalculations; \n
|
||||
* Output: Number of performed working set recalculations. */
|
||||
real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
|
||||
);
|
||||
|
||||
|
||||
/** Obtains the desired working set for the auxiliary initial QP in
|
||||
* accordance with the user specifications
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_OBTAINING_WORKINGSET_FAILED \n
|
||||
RET_INVALID_ARGUMENTS */
|
||||
returnValue obtainAuxiliaryWorkingSet( const real_t* const xOpt, /**< Optimal primal solution vector.
|
||||
* If a NULL pointer is passed, all entries are assumed to be zero. */
|
||||
const real_t* const yOpt, /**< Optimal dual solution vector.
|
||||
* If a NULL pointer is passed, all entries are assumed to be zero. */
|
||||
const Bounds* const guessedBounds, /**< Guessed working set for solution (xOpt,yOpt). */
|
||||
Bounds* auxiliaryBounds /**< Input: Allocated bound object. \n
|
||||
* Ouput: Working set for auxiliary QP. */
|
||||
) const;
|
||||
|
||||
/** Setups bound data structure according to auxiliaryBounds.
|
||||
* (If the working set shall be setup afresh, make sure that
|
||||
* bounds data structure has been resetted!)
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SETUP_WORKINGSET_FAILED \n
|
||||
RET_INVALID_ARGUMENTS \n
|
||||
RET_UNKNOWN BUG */
|
||||
returnValue setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds, /**< Working set for auxiliary QP. */
|
||||
BooleanType setupAfresh /**< Flag indicating if given working set shall be
|
||||
* setup afresh or by updating the current one. */
|
||||
);
|
||||
|
||||
/** Setups the optimal primal/dual solution of the auxiliary initial QP.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue setupAuxiliaryQPsolution( const real_t* const xOpt, /**< Optimal primal solution vector.
|
||||
* If a NULL pointer is passed, all entries are set to zero. */
|
||||
const real_t* const yOpt /**< Optimal dual solution vector.
|
||||
* If a NULL pointer is passed, all entries are set to zero. */
|
||||
);
|
||||
|
||||
/** Setups gradient of the auxiliary initial QP for given
|
||||
* optimal primal/dual solution and given initial working set
|
||||
* (assumes that members X, Y and BOUNDS have already been initialised!).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue setupAuxiliaryQPgradient( );
|
||||
|
||||
/** Setups bounds of the auxiliary initial QP for given
|
||||
* optimal primal/dual solution and given initial working set
|
||||
* (assumes that members X, Y and BOUNDS have already been initialised!).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNKNOWN BUG */
|
||||
returnValue setupAuxiliaryQPbounds( BooleanType useRelaxation /**< Flag indicating if inactive bounds shall be relaxed. */
|
||||
);
|
||||
|
||||
|
||||
/** Adds a bound to active set (specialised version for the case where no constraints exist).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_ADDBOUND_FAILED */
|
||||
returnValue addBound( int number, /**< Number of bound to be added to active set. */
|
||||
SubjectToStatus B_status, /**< Status of new active bound. */
|
||||
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
|
||||
);
|
||||
|
||||
/** Removes a bounds from active set (specialised version for the case where no constraints exist).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_HESSIAN_NOT_SPD \n
|
||||
RET_REMOVEBOUND_FAILED */
|
||||
returnValue removeBound( int number, /**< Number of bound to be removed from active set. */
|
||||
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
|
||||
);
|
||||
|
||||
|
||||
/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_DIV_BY_ZERO */
|
||||
returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */
|
||||
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
|
||||
real_t* const a /**< Output: Solution vector */
|
||||
);
|
||||
|
||||
/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n
|
||||
* Special variant for the case that this function is called from within "removeBound()".
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_DIV_BY_ZERO */
|
||||
returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */
|
||||
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
|
||||
BooleanType removingBound, /**< Indicates if function is called from "removeBound()". */
|
||||
real_t* const a /**< Output: Solution vector */
|
||||
);
|
||||
|
||||
|
||||
/** Determines step direction of the shift of the QP data.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue hotstart_determineDataShift(const int* const FX_idx, /**< Index array of fixed variables. */
|
||||
const real_t* const g_new, /**< New gradient vector. */
|
||||
const real_t* const lb_new, /**< New lower bounds. */
|
||||
const real_t* const ub_new, /**< New upper bounds. */
|
||||
real_t* const delta_g, /**< Output: Step direction of gradient vector. */
|
||||
real_t* const delta_lb, /**< Output: Step direction of lower bounds. */
|
||||
real_t* const delta_ub, /**< Output: Step direction of upper bounds. */
|
||||
BooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */
|
||||
);
|
||||
|
||||
|
||||
/** Checks if lower/upper bounds remain consistent
|
||||
* (i.e. if lb <= ub) during the current step.
|
||||
* \return BT_TRUE iff bounds remain consistent
|
||||
*/
|
||||
BooleanType areBoundsConsistent( const real_t* const delta_lb, /**< Step direction of lower bounds. */
|
||||
const real_t* const delta_ub /**< Step direction of upper bounds. */
|
||||
) const;
|
||||
|
||||
|
||||
/** Setups internal QP data.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INVALID_ARGUMENTS */
|
||||
returnValue setupQPdata( const real_t* const _H, /**< Hessian matrix. */
|
||||
const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */
|
||||
const real_t* const _g, /**< Gradient vector. */
|
||||
const real_t* const _lb, /**< Lower bounds (on variables). \n
|
||||
If no lower bounds exist, a NULL pointer can be passed. */
|
||||
const real_t* const _ub /**< Upper bounds (on variables). \n
|
||||
If no upper bounds exist, a NULL pointer can be passed. */
|
||||
);
|
||||
|
||||
|
||||
/** Sets Hessian matrix of the QP.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setH( const real_t* const H_new /**< New Hessian matrix (with correct dimension!). */
|
||||
);
|
||||
|
||||
/** Changes gradient vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setG( const real_t* const g_new /**< New gradient vector (with correct dimension!). */
|
||||
);
|
||||
|
||||
/** Changes lower bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setLB( const real_t* const lb_new /**< New lower bound vector (with correct dimension!). */
|
||||
);
|
||||
|
||||
/** Changes single entry of lower bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue setLB( int number, /**< Number of entry to be changed. */
|
||||
real_t value /**< New value for entry of lower bound vector. */
|
||||
);
|
||||
|
||||
/** Changes upper bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline returnValue setUB( const real_t* const ub_new /**< New upper bound vector (with correct dimension!). */
|
||||
);
|
||||
|
||||
/** Changes single entry of upper bound vector of the QP.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue setUB( int number, /**< Number of entry to be changed. */
|
||||
real_t value /**< New value for entry of upper bound vector. */
|
||||
);
|
||||
|
||||
|
||||
/** Computes parameters for the Givens matrix G for which [x,y]*G = [z,0]
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline void computeGivens( real_t xold, /**< Matrix entry to be normalised. */
|
||||
real_t yold, /**< Matrix entry to be annihilated. */
|
||||
real_t& xnew, /**< Output: Normalised matrix entry. */
|
||||
real_t& ynew, /**< Output: Annihilated matrix entry. */
|
||||
real_t& c, /**< Output: Cosine entry of Givens matrix. */
|
||||
real_t& s /**< Output: Sine entry of Givens matrix. */
|
||||
) const;
|
||||
|
||||
/** Applies Givens matrix determined by c and s (cf. computeGivens).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
inline void applyGivens( real_t c, /**< Cosine entry of Givens matrix. */
|
||||
real_t s, /**< Sine entry of Givens matrix. */
|
||||
real_t xold, /**< Matrix entry to be transformed corresponding to
|
||||
* the normalised entry of the original matrix. */
|
||||
real_t yold, /**< Matrix entry to be transformed corresponding to
|
||||
* the annihilated entry of the original matrix. */
|
||||
real_t& xnew, /**< Output: Transformed matrix entry corresponding to
|
||||
* the normalised entry of the original matrix. */
|
||||
real_t& ynew /**< Output: Transformed matrix entry corresponding to
|
||||
* the annihilated entry of the original matrix. */
|
||||
) const;
|
||||
|
||||
|
||||
/*
|
||||
* PRIVATE MEMBER FUNCTIONS
|
||||
*/
|
||||
private:
|
||||
/** Determines step direction of the homotopy path.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_STEPDIRECTION_FAILED_CHOLESKY */
|
||||
returnValue hotstart_determineStepDirection(const int* const FR_idx, /**< Index array of free variables. */
|
||||
const int* const FX_idx, /**< Index array of fixed variables. */
|
||||
const real_t* const delta_g, /**< Step direction of gradient vector. */
|
||||
const real_t* const delta_lb, /**< Step direction of lower bounds. */
|
||||
const real_t* const delta_ub, /**< Step direction of upper bounds. */
|
||||
BooleanType Delta_bB_isZero, /**< Indicates if active bounds are to be shifted. */
|
||||
real_t* const delta_xFX, /**< Output: Primal homotopy step direction of fixed variables. */
|
||||
real_t* const delta_xFR, /**< Output: Primal homotopy step direction of free variables. */
|
||||
real_t* const delta_yFX /**< Output: Dual homotopy step direction of fixed variables' multiplier. */
|
||||
);
|
||||
|
||||
/** Determines the maximum possible step length along the homotopy path.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue hotstart_determineStepLength( const int* const FR_idx, /**< Index array of free variables. */
|
||||
const int* const FX_idx, /**< Index array of fixed variables. */
|
||||
const real_t* const delta_lb, /**< Step direction of lower bounds. */
|
||||
const real_t* const delta_ub, /**< Step direction of upper bounds. */
|
||||
const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */
|
||||
const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */
|
||||
int& BC_idx, /**< Output: Index of blocking constraint. */
|
||||
SubjectToStatus& BC_status /**< Output: Status of blocking constraint. */
|
||||
);
|
||||
|
||||
/** Performs a step along the homotopy path (and updates active set).
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_OPTIMAL_SOLUTION_FOUND \n
|
||||
RET_REMOVE_FROM_ACTIVESET_FAILED \n
|
||||
RET_ADD_TO_ACTIVESET_FAILED \n
|
||||
RET_QP_INFEASIBLE */
|
||||
returnValue hotstart_performStep( const int* const FR_idx, /**< Index array of free variables. */
|
||||
const int* const FX_idx, /**< Index array of fixed variables. */
|
||||
const real_t* const delta_g, /**< Step direction of gradient vector. */
|
||||
const real_t* const delta_lb, /**< Step direction of lower bounds. */
|
||||
const real_t* const delta_ub, /**< Step direction of upper bounds. */
|
||||
const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */
|
||||
const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */
|
||||
const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */
|
||||
int BC_idx, /**< Index of blocking constraint. */
|
||||
SubjectToStatus BC_status /**< Status of blocking constraint. */
|
||||
);
|
||||
|
||||
|
||||
#ifdef PC_DEBUG /* Define print functions only for debugging! */
|
||||
|
||||
/** Prints concise information on the current iteration.
|
||||
* \return SUCCESSFUL_RETURN \n */
|
||||
returnValue printIteration( int iteration, /**< Number of current iteration. */
|
||||
int BC_idx, /**< Index of blocking bound. */
|
||||
SubjectToStatus BC_status /**< Status of blocking bound. */
|
||||
);
|
||||
|
||||
#endif /* PC_DEBUG */
|
||||
|
||||
|
||||
/** Determines the maximum violation of the KKT optimality conditions
|
||||
* of the current iterate within the QProblemB object.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
* RET_INACCURATE_SOLUTION \n
|
||||
* RET_NO_SOLUTION */
|
||||
returnValue checkKKTconditions( );
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER VARIABLES
|
||||
*/
|
||||
protected:
|
||||
real_t H[NVMAX*NVMAX]; /**< Hessian matrix. */
|
||||
BooleanType hasHessian; /**< Flag indicating whether H contains Hessian or corresponding Cholesky factor R; \sa init. */
|
||||
|
||||
real_t g[NVMAX]; /**< Gradient. */
|
||||
real_t lb[NVMAX]; /**< Lower bound vector (on variables). */
|
||||
real_t ub[NVMAX]; /**< Upper bound vector (on variables). */
|
||||
|
||||
Bounds bounds; /**< Data structure for problem's bounds. */
|
||||
|
||||
real_t R[NVMAX*NVMAX]; /**< Cholesky decomposition of H (i.e. H = R^T*R). */
|
||||
BooleanType hasCholesky; /**< Flag indicating whether Cholesky decomposition has already been setup. */
|
||||
|
||||
real_t x[NVMAX]; /**< Primal solution vector. */
|
||||
real_t y[NVMAX+NCMAX]; /**< Dual solution vector. */
|
||||
|
||||
real_t tau; /**< Last homotopy step length. */
|
||||
|
||||
QProblemStatus status; /**< Current status of the solution process. */
|
||||
|
||||
BooleanType infeasible; /**< QP infeasible? */
|
||||
BooleanType unbounded; /**< QP unbounded? */
|
||||
|
||||
HessianType hessianType; /**< Type of Hessian matrix. */
|
||||
|
||||
PrintLevel printlevel; /**< Print level. */
|
||||
|
||||
int count; /**< Counts the number of hotstart function calls (internal usage only!). */
|
||||
};
|
||||
|
||||
|
||||
#include <QProblemB.ipp>
|
||||
|
||||
#endif /* QPOASES_QPROBLEMB_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,178 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/SubjectTo.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of the SubjectTo class designed to manage working sets of
|
||||
* constraints and bounds within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_SUBJECTTO_HPP
|
||||
#define QPOASES_SUBJECTTO_HPP
|
||||
|
||||
|
||||
#include <Indexlist.hpp>
|
||||
|
||||
|
||||
|
||||
/** This class manages working sets of constraints and bounds by storing
|
||||
* index sets and other status information.
|
||||
*
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*/
|
||||
class SubjectTo
|
||||
{
|
||||
/*
|
||||
* PUBLIC MEMBER FUNCTIONS
|
||||
*/
|
||||
public:
|
||||
/** Default constructor. */
|
||||
SubjectTo( );
|
||||
|
||||
/** Copy constructor (deep copy). */
|
||||
SubjectTo( const SubjectTo& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
/** Destructor. */
|
||||
~SubjectTo( );
|
||||
|
||||
/** Assignment operator (deep copy). */
|
||||
SubjectTo& operator=( const SubjectTo& rhs /**< Rhs object. */
|
||||
);
|
||||
|
||||
|
||||
/** Pseudo-constructor takes the number of constraints or bounds.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue init( int n /**< Number of constraints or bounds. */
|
||||
);
|
||||
|
||||
|
||||
/** Returns type of (constraints') bound.
|
||||
* \return Type of (constraints') bound \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline SubjectToType getType( int i /**< Number of (constraints') bound. */
|
||||
) const ;
|
||||
|
||||
/** Returns status of (constraints') bound.
|
||||
* \return Status of (constraints') bound \n
|
||||
ST_UNDEFINED */
|
||||
inline SubjectToStatus getStatus( int i /**< Number of (constraints') bound. */
|
||||
) const;
|
||||
|
||||
|
||||
/** Sets type of (constraints') bound.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue setType( int i, /**< Number of (constraints') bound. */
|
||||
SubjectToType value /**< Type of (constraints') bound. */
|
||||
);
|
||||
|
||||
/** Sets status of (constraints') bound.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_INDEX_OUT_OF_BOUNDS */
|
||||
inline returnValue setStatus( int i, /**< Number of (constraints') bound. */
|
||||
SubjectToStatus value /**< Status of (constraints') bound. */
|
||||
);
|
||||
|
||||
|
||||
/** Sets status of lower (constraints') bounds. */
|
||||
inline void setNoLower( BooleanType _status /**< Status of lower (constraints') bounds. */
|
||||
);
|
||||
|
||||
/** Sets status of upper (constraints') bounds. */
|
||||
inline void setNoUpper( BooleanType _status /**< Status of upper (constraints') bounds. */
|
||||
);
|
||||
|
||||
|
||||
/** Returns status of lower (constraints') bounds.
|
||||
* \return BT_TRUE if there is no lower (constraints') bound on any variable. */
|
||||
inline BooleanType isNoLower( ) const;
|
||||
|
||||
/** Returns status of upper bounds.
|
||||
* \return BT_TRUE if there is no upper (constraints') bound on any variable. */
|
||||
inline BooleanType isNoUpper( ) const;
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER FUNCTIONS
|
||||
*/
|
||||
protected:
|
||||
/** Adds the index of a new constraint or bound to index set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_ADDINDEX_FAILED */
|
||||
returnValue addIndex( Indexlist* const indexlist, /**< Index list to which the new index shall be added. */
|
||||
int newnumber, /**< Number of new constraint or bound. */
|
||||
SubjectToStatus newstatus /**< Status of new constraint or bound. */
|
||||
);
|
||||
|
||||
/** Removes the index of a constraint or bound from index set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNKNOWN_BUG */
|
||||
returnValue removeIndex( Indexlist* const indexlist, /**< Index list from which the new index shall be removed. */
|
||||
int removenumber /**< Number of constraint or bound to be removed. */
|
||||
);
|
||||
|
||||
/** Swaps the indices of two constraints or bounds within the index set.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_SWAPINDEX_FAILED */
|
||||
returnValue swapIndex( Indexlist* const indexlist, /**< Index list in which the indices shold be swapped. */
|
||||
int number1, /**< Number of first constraint or bound. */
|
||||
int number2 /**< Number of second constraint or bound. */
|
||||
);
|
||||
|
||||
|
||||
/*
|
||||
* PROTECTED MEMBER VARIABLES
|
||||
*/
|
||||
protected:
|
||||
SubjectToType type[NVMAX+NCMAX]; /**< Type of constraints/bounds. */
|
||||
SubjectToStatus status[NVMAX+NCMAX]; /**< Status of constraints/bounds. */
|
||||
|
||||
BooleanType noLower; /**< This flag indicates if there is no lower bound on any variable. */
|
||||
BooleanType noUpper; /**< This flag indicates if there is no upper bound on any variable. */
|
||||
|
||||
|
||||
/*
|
||||
* PRIVATE MEMBER VARIABLES
|
||||
*/
|
||||
private:
|
||||
int size;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#include <SubjectTo.ipp>
|
||||
|
||||
#endif /* QPOASES_SUBJECTTO_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,131 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/Types.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2008
|
||||
*
|
||||
* Declaration of all non-built-in types (except for classes).
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_TYPES_HPP
|
||||
#define QPOASES_TYPES_HPP
|
||||
|
||||
|
||||
|
||||
/** Define real_t for facilitating switching between double and float. */
|
||||
// typedef double real_t;
|
||||
|
||||
|
||||
/** Summarises all possible logical values. */
|
||||
enum BooleanType
|
||||
{
|
||||
BT_FALSE, /**< Logical value for "false". */
|
||||
BT_TRUE /**< Logical value for "true". */
|
||||
};
|
||||
|
||||
|
||||
/** Summarises all possible print levels. Print levels are used to describe
|
||||
* the desired amount of output during runtime of qpOASES. */
|
||||
enum PrintLevel
|
||||
{
|
||||
PL_NONE, /**< No output. */
|
||||
PL_LOW, /**< Print error messages only. */
|
||||
PL_MEDIUM, /**< Print error and warning messages as well as concise info messages. */
|
||||
PL_HIGH /**< Print all messages with full details. */
|
||||
};
|
||||
|
||||
|
||||
/** Defines visibility status of a message. */
|
||||
enum VisibilityStatus
|
||||
{
|
||||
VS_VISIBLE, /**< Message visible. */
|
||||
VS_HIDDEN /**< Message not visible. */
|
||||
};
|
||||
|
||||
|
||||
/** Summarises all possible states of the (S)QProblem(B) object during the
|
||||
solution process of a QP sequence. */
|
||||
enum QProblemStatus
|
||||
{
|
||||
QPS_NOTINITIALISED, /**< QProblem object is freshly instantiated or reset. */
|
||||
QPS_PREPARINGAUXILIARYQP, /**< An auxiliary problem is currently setup, either at the very beginning
|
||||
* via an initial homotopy or after changing the QP matrices. */
|
||||
QPS_AUXILIARYQPSOLVED, /**< An auxilary problem was solved, either at the very beginning
|
||||
* via an initial homotopy or after changing the QP matrices. */
|
||||
QPS_PERFORMINGHOMOTOPY, /**< A homotopy according to the main idea of the online active
|
||||
* set strategy is performed. */
|
||||
QPS_HOMOTOPYQPSOLVED, /**< An intermediate QP along the homotopy path was solved. */
|
||||
QPS_SOLVED /**< The solution of the actual QP was found. */
|
||||
};
|
||||
|
||||
|
||||
/** Summarises all possible types of bounds and constraints. */
|
||||
enum SubjectToType
|
||||
{
|
||||
ST_UNBOUNDED, /**< Bound/constraint is unbounded. */
|
||||
ST_BOUNDED, /**< Bound/constraint is bounded but not fixed. */
|
||||
ST_EQUALITY, /**< Bound/constraint is fixed (implicit equality bound/constraint). */
|
||||
ST_UNKNOWN /**< Type of bound/constraint unknown. */
|
||||
};
|
||||
|
||||
|
||||
/** Summarises all possible states of bounds and constraints. */
|
||||
enum SubjectToStatus
|
||||
{
|
||||
ST_INACTIVE, /**< Bound/constraint is inactive. */
|
||||
ST_LOWER, /**< Bound/constraint is at its lower bound. */
|
||||
ST_UPPER, /**< Bound/constraint is at its upper bound. */
|
||||
ST_UNDEFINED /**< Status of bound/constraint undefined. */
|
||||
};
|
||||
|
||||
|
||||
/** Summarises all possible cycling states of bounds and constraints. */
|
||||
enum CyclingStatus
|
||||
{
|
||||
CYC_NOT_INVOLVED, /**< Bound/constraint is not involved in current cycling. */
|
||||
CYC_PREV_ADDED, /**< Bound/constraint has previously been added during the current cycling. */
|
||||
CYC_PREV_REMOVED /**< Bound/constraint has previously been removed during the current cycling. */
|
||||
};
|
||||
|
||||
|
||||
/** Summarises all possible types of the QP's Hessian matrix. */
|
||||
enum HessianType
|
||||
{
|
||||
HST_SEMIDEF, /**< Hessian is positive semi-definite. */
|
||||
HST_POSDEF_NULLSPACE, /**< Hessian is positive definite on null space of active bounds/constraints. */
|
||||
HST_POSDEF, /**< Hessian is (strictly) positive definite. */
|
||||
HST_IDENTITY /**< Hessian is identity matrix. */
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif /* QPOASES_TYPES_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,197 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file INCLUDE/Utils.hpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of global utility functions for working with qpOASES.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef QPOASES_UTILS_HPP
|
||||
#define QPOASES_UTILS_HPP
|
||||
|
||||
|
||||
#include <MessageHandling.hpp>
|
||||
|
||||
|
||||
#ifdef PC_DEBUG /* Define print functions only for debugging! */
|
||||
|
||||
/** Prints a vector.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue print( const real_t* const v, /**< Vector to be printed. */
|
||||
int n /**< Length of vector. */
|
||||
);
|
||||
|
||||
/** Prints a permuted vector.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue print( const real_t* const v, /**< Vector to be printed. */
|
||||
int n, /**< Length of vector. */
|
||||
const int* const V_idx /**< Pemutation vector. */
|
||||
);
|
||||
|
||||
/** Prints a named vector.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue print( const real_t* const v, /**< Vector to be printed. */
|
||||
int n, /**< Length of vector. */
|
||||
const char* name /** Name of vector. */
|
||||
);
|
||||
|
||||
/** Prints a matrix.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue print( const real_t* const M, /**< Matrix to be printed. */
|
||||
int nrow, /**< Row number of matrix. */
|
||||
int ncol /**< Column number of matrix. */
|
||||
);
|
||||
|
||||
/** Prints a permuted matrix.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue print( const real_t* const M, /**< Matrix to be printed. */
|
||||
int nrow, /**< Row number of matrix. */
|
||||
int ncol , /**< Column number of matrix. */
|
||||
const int* const ROW_idx, /**< Row pemutation vector. */
|
||||
const int* const COL_idx /**< Column pemutation vector. */
|
||||
);
|
||||
|
||||
/** Prints a named matrix.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue print( const real_t* const M, /**< Matrix to be printed. */
|
||||
int nrow, /**< Row number of matrix. */
|
||||
int ncol, /**< Column number of matrix. */
|
||||
const char* name /** Name of matrix. */
|
||||
);
|
||||
|
||||
/** Prints an index array.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue print( const int* const index, /**< Index array to be printed. */
|
||||
int n /**< Length of index array. */
|
||||
);
|
||||
|
||||
/** Prints a named index array.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue print( const int* const index, /**< Index array to be printed. */
|
||||
int n, /**< Length of index array. */
|
||||
const char* name /**< Name of index array. */
|
||||
);
|
||||
|
||||
|
||||
/** Prints a string to desired output target (useful also for MATLAB output!).
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue myPrintf( const char* s /**< String to be written. */
|
||||
);
|
||||
|
||||
|
||||
/** Prints qpOASES copyright notice.
|
||||
* \return SUCCESSFUL_RETURN */
|
||||
returnValue printCopyrightNotice( );
|
||||
|
||||
|
||||
/** Reads a real_t matrix from file.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNABLE_TO_OPEN_FILE \n
|
||||
RET_UNABLE_TO_READ_FILE */
|
||||
returnValue readFromFile( real_t* data, /**< Matrix to be read from file. */
|
||||
int nrow, /**< Row number of matrix. */
|
||||
int ncol, /**< Column number of matrix. */
|
||||
const char* datafilename /**< Data file name. */
|
||||
);
|
||||
|
||||
/** Reads a real_t vector from file.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNABLE_TO_OPEN_FILE \n
|
||||
RET_UNABLE_TO_READ_FILE */
|
||||
returnValue readFromFile( real_t* data, /**< Vector to be read from file. */
|
||||
int n, /**< Length of vector. */
|
||||
const char* datafilename /**< Data file name. */
|
||||
);
|
||||
|
||||
/** Reads an integer (column) vector from file.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNABLE_TO_OPEN_FILE \n
|
||||
RET_UNABLE_TO_READ_FILE */
|
||||
returnValue readFromFile( int* data, /**< Vector to be read from file. */
|
||||
int n, /**< Length of vector. */
|
||||
const char* datafilename /**< Data file name. */
|
||||
);
|
||||
|
||||
|
||||
/** Writes a real_t matrix into a file.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNABLE_TO_OPEN_FILE */
|
||||
returnValue writeIntoFile( const real_t* const data, /**< Matrix to be written into file. */
|
||||
int nrow, /**< Row number of matrix. */
|
||||
int ncol, /**< Column number of matrix. */
|
||||
const char* datafilename, /**< Data file name. */
|
||||
BooleanType append /**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */
|
||||
);
|
||||
|
||||
/** Writes a real_t vector into a file.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNABLE_TO_OPEN_FILE */
|
||||
returnValue writeIntoFile( const real_t* const data, /**< Vector to be written into file. */
|
||||
int n, /**< Length of vector. */
|
||||
const char* datafilename, /**< Data file name. */
|
||||
BooleanType append /**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */
|
||||
);
|
||||
|
||||
/** Writes an integer (column) vector into a file.
|
||||
* \return SUCCESSFUL_RETURN \n
|
||||
RET_UNABLE_TO_OPEN_FILE */
|
||||
returnValue writeIntoFile( const int* const integer, /**< Integer vector to be written into file. */
|
||||
int n, /**< Length of vector. */
|
||||
const char* datafilename, /**< Data file name. */
|
||||
BooleanType append /**< Indicates if integer shall be appended if the file already exists (otherwise it is overwritten). */
|
||||
);
|
||||
|
||||
#endif /* PC_DEBUG */
|
||||
|
||||
|
||||
/** Returns the current system time.
|
||||
* \return current system time */
|
||||
real_t getCPUtime( );
|
||||
|
||||
|
||||
/** Returns the Euclidean norm of a vector.
|
||||
* \return 0: successful */
|
||||
real_t getNorm( const real_t* const v, /**< Vector. */
|
||||
int n /**< Vector's dimension. */
|
||||
);
|
||||
|
||||
/** Returns the absolute value of a real_t.
|
||||
* \return Absolute value of a real_t */
|
||||
inline real_t getAbs( real_t x /**< Input argument. */
|
||||
);
|
||||
|
||||
|
||||
|
||||
#include <Utils.ipp>
|
||||
|
||||
#endif /* QPOASES_UTILS_HPP */
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
Import('env', 'interface_dir')
|
||||
|
||||
qp_files = [
|
||||
Glob("SRC/*.cpp"),
|
||||
Glob("SRC/EXTRAS/*.cpp"),
|
||||
]
|
||||
|
||||
cpp_path = [
|
||||
".",
|
||||
"INCLUDE",
|
||||
"INCLUDE/EXTRAS",
|
||||
"SRC/",
|
||||
interface_dir,
|
||||
]
|
||||
|
||||
env.Library('qpoases', qp_files, CPPPATH=cpp_path)
|
||||
|
|
@ -0,0 +1,252 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/Bounds.cpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of the Bounds class designed to manage working sets of
|
||||
* bounds within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
#include <Bounds.hpp>
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/*
|
||||
* B o u n d s
|
||||
*/
|
||||
Bounds::Bounds( ) : SubjectTo( ),
|
||||
nV( 0 ),
|
||||
nFV( 0 ),
|
||||
nBV( 0 ),
|
||||
nUV( 0 )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* B o u n d s
|
||||
*/
|
||||
Bounds::Bounds( const Bounds& rhs ) : SubjectTo( rhs ),
|
||||
nV( rhs.nV ),
|
||||
nFV( rhs.nFV ),
|
||||
nBV( rhs.nBV ),
|
||||
nUV( rhs.nUV )
|
||||
{
|
||||
free = rhs.free;
|
||||
fixed = rhs.fixed;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* ~ B o u n d s
|
||||
*/
|
||||
Bounds::~Bounds( )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* o p e r a t o r =
|
||||
*/
|
||||
Bounds& Bounds::operator=( const Bounds& rhs )
|
||||
{
|
||||
if ( this != &rhs )
|
||||
{
|
||||
SubjectTo::operator=( rhs );
|
||||
|
||||
nV = rhs.nV;
|
||||
nFV = rhs.nFV;
|
||||
nBV = rhs.nBV;
|
||||
nUV = rhs.nUV;
|
||||
|
||||
free = rhs.free;
|
||||
fixed = rhs.fixed;
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i n i t
|
||||
*/
|
||||
returnValue Bounds::init( int n )
|
||||
{
|
||||
nV = n;
|
||||
nFV = 0;
|
||||
nBV = 0;
|
||||
nUV = 0;
|
||||
|
||||
free.init( );
|
||||
fixed.init( );
|
||||
|
||||
return SubjectTo::init( n );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t u p B o u n d
|
||||
*/
|
||||
returnValue Bounds::setupBound( int _number, SubjectToStatus _status
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( ( _number < 0 ) || ( _number >= getNV( ) ) )
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
|
||||
/* Add bound index to respective index list. */
|
||||
switch ( _status )
|
||||
{
|
||||
case ST_INACTIVE:
|
||||
if ( this->addIndex( this->getFree( ),_number,_status ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_BOUND_FAILED );
|
||||
break;
|
||||
|
||||
case ST_LOWER:
|
||||
if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_BOUND_FAILED );
|
||||
break;
|
||||
|
||||
case ST_UPPER:
|
||||
if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_BOUND_FAILED );
|
||||
break;
|
||||
|
||||
default:
|
||||
return THROWERROR( RET_INVALID_ARGUMENTS );
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t u p A l l F r e e
|
||||
*/
|
||||
returnValue Bounds::setupAllFree( )
|
||||
{
|
||||
int i;
|
||||
|
||||
/* 1) Place unbounded variables at the beginning of the index list of free variables. */
|
||||
for( i=0; i<nV; ++i )
|
||||
{
|
||||
if ( getType( i ) == ST_UNBOUNDED )
|
||||
{
|
||||
if ( setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_BOUND_FAILED );
|
||||
}
|
||||
}
|
||||
|
||||
/* 2) Add remaining (i.e. bounded but possibly free) variables to the index list of free variables. */
|
||||
for( i=0; i<nV; ++i )
|
||||
{
|
||||
if ( getType( i ) == ST_BOUNDED )
|
||||
{
|
||||
if ( setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_BOUND_FAILED );
|
||||
}
|
||||
}
|
||||
|
||||
/* 3) Place implicitly fixed variables at the end of the index list of free variables. */
|
||||
for( i=0; i<nV; ++i )
|
||||
{
|
||||
if ( getType( i ) == ST_EQUALITY )
|
||||
{
|
||||
if ( setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_BOUND_FAILED );
|
||||
}
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* m o v e F i x e d T o F r e e
|
||||
*/
|
||||
returnValue Bounds::moveFixedToFree( int _number )
|
||||
{
|
||||
/* consistency check */
|
||||
if ( ( _number < 0 ) || ( _number >= getNV( ) ) )
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
|
||||
/* Move index from indexlist of fixed variables to that of free ones. */
|
||||
if ( this->removeIndex( this->getFixed( ),_number ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_MOVING_BOUND_FAILED );
|
||||
|
||||
if ( this->addIndex( this->getFree( ),_number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_MOVING_BOUND_FAILED );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* m o v e F r e e T o F i x e d
|
||||
*/
|
||||
returnValue Bounds::moveFreeToFixed( int _number, SubjectToStatus _status
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( ( _number < 0 ) || ( _number >= getNV( ) ) )
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
|
||||
/* Move index from indexlist of free variables to that of fixed ones. */
|
||||
if ( this->removeIndex( this->getFree( ),_number ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_MOVING_BOUND_FAILED );
|
||||
|
||||
if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_MOVING_BOUND_FAILED );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s w a p F r e e
|
||||
*/
|
||||
returnValue Bounds::swapFree( int number1, int number2
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( ( number1 < 0 ) || ( number1 >= getNV( ) ) || ( number2 < 0 ) || ( number2 >= getNV( ) ) )
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
|
||||
/* Swap index within indexlist of free variables. */
|
||||
return this->swapIndex( this->getFree( ),number1,number2 );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,144 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/Bounds.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of inlined member functions of the Bounds class designed
|
||||
* to manage working sets of bounds within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* g e t N V
|
||||
*/
|
||||
inline int Bounds::getNV( ) const
|
||||
{
|
||||
return nV;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N F X
|
||||
*/
|
||||
inline int Bounds::getNFV( ) const
|
||||
{
|
||||
return nFV;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N B V
|
||||
*/
|
||||
inline int Bounds::getNBV( ) const
|
||||
{
|
||||
return nBV;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N U V
|
||||
*/
|
||||
inline int Bounds::getNUV( ) const
|
||||
{
|
||||
return nUV;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* s e t N F X
|
||||
*/
|
||||
inline returnValue Bounds::setNFV( int n )
|
||||
{
|
||||
nFV = n;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t N B V
|
||||
*/
|
||||
inline returnValue Bounds::setNBV( int n )
|
||||
{
|
||||
nBV = n;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t N U V
|
||||
*/
|
||||
inline returnValue Bounds::setNUV( int n )
|
||||
{
|
||||
nUV = n;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N F R
|
||||
*/
|
||||
inline int Bounds::getNFR( )
|
||||
{
|
||||
return free.getLength( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N F X
|
||||
*/
|
||||
inline int Bounds::getNFX( )
|
||||
{
|
||||
return fixed.getLength( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t F r e e
|
||||
*/
|
||||
inline Indexlist* Bounds::getFree( )
|
||||
{
|
||||
return &free;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t F i x e d
|
||||
*/
|
||||
inline Indexlist* Bounds::getFixed( )
|
||||
{
|
||||
return &fixed;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,248 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/Constraints.cpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of the Constraints class designed to manage working sets of
|
||||
* constraints within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
#include <Constraints.hpp>
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/*
|
||||
* C o n s t r a i n t s
|
||||
*/
|
||||
Constraints::Constraints( ) : SubjectTo( ),
|
||||
nC( 0 ),
|
||||
nEC( 0 ),
|
||||
nIC( 0 ),
|
||||
nUC( 0 )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* C o n s t r a i n t s
|
||||
*/
|
||||
Constraints::Constraints( const Constraints& rhs ) : SubjectTo( rhs ),
|
||||
nC( rhs.nC ),
|
||||
nEC( rhs.nEC ),
|
||||
nIC( rhs.nIC ),
|
||||
nUC( rhs.nUC )
|
||||
{
|
||||
active = rhs.active;
|
||||
inactive = rhs.inactive;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* ~ C o n s t r a i n t s
|
||||
*/
|
||||
Constraints::~Constraints( )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* o p e r a t o r =
|
||||
*/
|
||||
Constraints& Constraints::operator=( const Constraints& rhs )
|
||||
{
|
||||
if ( this != &rhs )
|
||||
{
|
||||
SubjectTo::operator=( rhs );
|
||||
|
||||
nC = rhs.nC;
|
||||
nEC = rhs.nEC;
|
||||
nIC = rhs.nIC;
|
||||
nUC = rhs.nUC;
|
||||
|
||||
active = rhs.active;
|
||||
inactive = rhs.inactive;
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i n i t
|
||||
*/
|
||||
returnValue Constraints::init( int n )
|
||||
{
|
||||
nC = n;
|
||||
nEC = 0;
|
||||
nIC = 0;
|
||||
nUC = 0;
|
||||
|
||||
active.init( );
|
||||
inactive.init( );
|
||||
|
||||
return SubjectTo::init( n );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t u p C o n s t r a i n t
|
||||
*/
|
||||
returnValue Constraints::setupConstraint( int _number, SubjectToStatus _status
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( ( _number < 0 ) || ( _number >= getNC( ) ) )
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
|
||||
/* Add constraint index to respective index list. */
|
||||
switch ( _status )
|
||||
{
|
||||
case ST_INACTIVE:
|
||||
if ( this->addIndex( this->getInactive( ),_number,_status ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
|
||||
break;
|
||||
|
||||
case ST_LOWER:
|
||||
if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
|
||||
break;
|
||||
|
||||
case ST_UPPER:
|
||||
if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
|
||||
break;
|
||||
|
||||
default:
|
||||
return THROWERROR( RET_INVALID_ARGUMENTS );
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t u p A l l I n a c t i v e
|
||||
*/
|
||||
returnValue Constraints::setupAllInactive( )
|
||||
{
|
||||
int i;
|
||||
|
||||
|
||||
/* 1) Place unbounded constraints at the beginning of the index list of inactive constraints. */
|
||||
for( i=0; i<nC; ++i )
|
||||
{
|
||||
if ( getType( i ) == ST_UNBOUNDED )
|
||||
{
|
||||
if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
|
||||
}
|
||||
}
|
||||
|
||||
/* 2) Add remaining (i.e. "real" inequality) constraints to the index list of inactive constraints. */
|
||||
for( i=0; i<nC; ++i )
|
||||
{
|
||||
if ( getType( i ) == ST_BOUNDED )
|
||||
{
|
||||
if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
|
||||
}
|
||||
}
|
||||
|
||||
/* 3) Place implicit equality constraints at the end of the index list of inactive constraints. */
|
||||
for( i=0; i<nC; ++i )
|
||||
{
|
||||
if ( getType( i ) == ST_EQUALITY )
|
||||
{
|
||||
if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
|
||||
}
|
||||
}
|
||||
|
||||
/* 4) Moreover, add all constraints of unknown type. */
|
||||
for( i=0; i<nC; ++i )
|
||||
{
|
||||
if ( getType( i ) == ST_UNKNOWN )
|
||||
{
|
||||
if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
|
||||
}
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* m o v e A c t i v e T o I n a c t i v e
|
||||
*/
|
||||
returnValue Constraints::moveActiveToInactive( int _number )
|
||||
{
|
||||
/* consistency check */
|
||||
if ( ( _number < 0 ) || ( _number >= getNC( ) ) )
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
|
||||
/* Move index from indexlist of active constraints to that of inactive ones. */
|
||||
if ( this->removeIndex( this->getActive( ),_number ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_MOVING_BOUND_FAILED );
|
||||
|
||||
if ( this->addIndex( this->getInactive( ),_number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_MOVING_BOUND_FAILED );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* m o v e I n a c t i v e T o A c t i v e
|
||||
*/
|
||||
returnValue Constraints::moveInactiveToActive( int _number, SubjectToStatus _status
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( ( _number < 0 ) || ( _number >= getNC( ) ) )
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
|
||||
/* Move index from indexlist of inactive constraints to that of active ones. */
|
||||
if ( this->removeIndex( this->getInactive( ),_number ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_MOVING_BOUND_FAILED );
|
||||
|
||||
if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_MOVING_BOUND_FAILED );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,144 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/Constraints.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Declaration of inlined member functions of the Constraints class designed
|
||||
* to manage working sets of constraints within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* g e t N C
|
||||
*/
|
||||
inline int Constraints::getNC( ) const
|
||||
{
|
||||
return nC;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N E C
|
||||
*/
|
||||
inline int Constraints::getNEC( ) const
|
||||
{
|
||||
return nEC;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N I C
|
||||
*/
|
||||
inline int Constraints::getNIC( ) const
|
||||
{
|
||||
return nIC;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N U C
|
||||
*/
|
||||
inline int Constraints::getNUC( ) const
|
||||
{
|
||||
return nUC;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t N E C
|
||||
*/
|
||||
inline returnValue Constraints::setNEC( int n )
|
||||
{
|
||||
nEC = n;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t N I C
|
||||
*/
|
||||
inline returnValue Constraints::setNIC( int n )
|
||||
{
|
||||
nIC = n;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t N U C
|
||||
*/
|
||||
inline returnValue Constraints::setNUC( int n )
|
||||
{
|
||||
nUC = n;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N A C
|
||||
*/
|
||||
inline int Constraints::getNAC( )
|
||||
{
|
||||
return active.getLength( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N I A C
|
||||
*/
|
||||
inline int Constraints::getNIAC( )
|
||||
{
|
||||
return inactive.getLength( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t A c t i v e
|
||||
*/
|
||||
inline Indexlist* Constraints::getActive( )
|
||||
{
|
||||
return &active;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t I n a c t i v e
|
||||
*/
|
||||
inline Indexlist* Constraints::getInactive( )
|
||||
{
|
||||
return &inactive;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,188 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/CyclingManager.cpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of the CyclingManager class designed to detect
|
||||
* and handle possible cycling during QP iterations.
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#include <CyclingManager.hpp>
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/*
|
||||
* C y c l i n g M a n a g e r
|
||||
*/
|
||||
CyclingManager::CyclingManager( ) : nV( 0 ),
|
||||
nC( 0 )
|
||||
{
|
||||
cyclingDetected = BT_FALSE;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* C y c l i n g M a n a g e r
|
||||
*/
|
||||
CyclingManager::CyclingManager( const CyclingManager& rhs ) : nV( rhs.nV ),
|
||||
nC( rhs.nC ),
|
||||
cyclingDetected( rhs.cyclingDetected )
|
||||
{
|
||||
int i;
|
||||
|
||||
for( i=0; i<nV+nC; ++i )
|
||||
status[i] = rhs.status[i];
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* ~ C y c l i n g M a n a g e r
|
||||
*/
|
||||
CyclingManager::~CyclingManager( )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* o p e r a t o r =
|
||||
*/
|
||||
CyclingManager& CyclingManager::operator=( const CyclingManager& rhs )
|
||||
{
|
||||
int i;
|
||||
|
||||
if ( this != &rhs )
|
||||
{
|
||||
nV = rhs.nV;
|
||||
nC = rhs.nC;
|
||||
|
||||
for( i=0; i<nV+nC; ++i )
|
||||
status[i] = rhs.status[i];
|
||||
|
||||
cyclingDetected = rhs.cyclingDetected;
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* i n i t
|
||||
*/
|
||||
returnValue CyclingManager::init( int _nV, int _nC )
|
||||
{
|
||||
nV = _nV;
|
||||
nC = _nC;
|
||||
|
||||
cyclingDetected = BT_FALSE;
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* s e t C y c l i n g S t a t u s
|
||||
*/
|
||||
returnValue CyclingManager::setCyclingStatus( int number,
|
||||
BooleanType isBound, CyclingStatus _status
|
||||
)
|
||||
{
|
||||
if ( isBound == BT_TRUE )
|
||||
{
|
||||
/* Set cycling status of a bound. */
|
||||
if ( ( number >= 0 ) && ( number < nV ) )
|
||||
{
|
||||
status[number] = _status;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Set cycling status of a constraint. */
|
||||
if ( ( number >= 0 ) && ( number < nC ) )
|
||||
{
|
||||
status[nV+number] = _status;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t C y c l i n g S t a t u s
|
||||
*/
|
||||
CyclingStatus CyclingManager::getCyclingStatus( int number, BooleanType isBound ) const
|
||||
{
|
||||
if ( isBound == BT_TRUE )
|
||||
{
|
||||
/* Return cycling status of a bound. */
|
||||
if ( ( number >= 0 ) && ( number < nV ) )
|
||||
return status[number];
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Return cycling status of a constraint. */
|
||||
if ( ( number >= 0 ) && ( number < nC ) )
|
||||
return status[nV+number];
|
||||
}
|
||||
|
||||
return CYC_NOT_INVOLVED;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* c l e a r C y c l i n g D a t a
|
||||
*/
|
||||
returnValue CyclingManager::clearCyclingData( )
|
||||
{
|
||||
int i;
|
||||
|
||||
/* Reset all status values ... */
|
||||
for( i=0; i<nV+nC; ++i )
|
||||
status[i] = CYC_NOT_INVOLVED;
|
||||
|
||||
/* ... and the main cycling flag. */
|
||||
cyclingDetected = BT_FALSE;
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,51 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/CyclingManager.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of inlined member functions of the CyclingManager class
|
||||
* designed to detect and handle possible cycling during QP iterations.
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* i s C y c l i n g D e t e c t e d
|
||||
*/
|
||||
inline BooleanType CyclingManager::isCyclingDetected( ) const
|
||||
{
|
||||
return cyclingDetected;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,434 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/EXTRAS/SolutionAnalysis.cpp
|
||||
* \author Milan Vukov, Boris Houska, Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2012
|
||||
*
|
||||
* Solution analysis class, based on a class in the standard version of the qpOASES
|
||||
*/
|
||||
|
||||
#include <EXTRAS/SolutionAnalysis.hpp>
|
||||
|
||||
/*
|
||||
* S o l u t i o n A n a l y s i s
|
||||
*/
|
||||
SolutionAnalysis::SolutionAnalysis( )
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
* S o l u t i o n A n a l y s i s
|
||||
*/
|
||||
SolutionAnalysis::SolutionAnalysis( const SolutionAnalysis& rhs )
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
* ~ S o l u t i o n A n a l y s i s
|
||||
*/
|
||||
SolutionAnalysis::~SolutionAnalysis( )
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
* o p e r a t o r =
|
||||
*/
|
||||
SolutionAnalysis& SolutionAnalysis::operator=( const SolutionAnalysis& rhs )
|
||||
{
|
||||
if ( this != &rhs )
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
/*
|
||||
* g e t H e s s i a n I n v e r s e
|
||||
*/
|
||||
returnValue SolutionAnalysis::getHessianInverse( QProblem* qp, real_t* hessianInverse )
|
||||
{
|
||||
returnValue returnvalue; /* the return value */
|
||||
BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */
|
||||
BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
|
||||
|
||||
register int run1, run2, run3;
|
||||
|
||||
register int nFR, nFX;
|
||||
|
||||
/* Ask for the number of free and fixed variables, assumes that active set
|
||||
* is constant for the covariance evaluation */
|
||||
nFR = qp->getNFR( );
|
||||
nFX = qp->getNFX( );
|
||||
|
||||
/* Ask for the corresponding index arrays: */
|
||||
if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_HOTSTART_FAILED );
|
||||
|
||||
if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_HOTSTART_FAILED );
|
||||
|
||||
if ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_HOTSTART_FAILED );
|
||||
|
||||
/* Initialization: */
|
||||
for( run1 = 0; run1 < NVMAX; run1++ )
|
||||
delta_g_cov[ run1 ] = 0.0;
|
||||
|
||||
for( run1 = 0; run1 < NVMAX; run1++ )
|
||||
delta_lb_cov[ run1 ] = 0.0;
|
||||
|
||||
for( run1 = 0; run1 < NVMAX; run1++ )
|
||||
delta_ub_cov[ run1 ] = 0.0;
|
||||
|
||||
for( run1 = 0; run1 < NCMAX; run1++ )
|
||||
delta_lbA_cov[ run1 ] = 0.0;
|
||||
|
||||
for( run1 = 0; run1 < NCMAX; run1++ )
|
||||
delta_ubA_cov[ run1 ] = 0.0;
|
||||
|
||||
/* The following loop solves the following:
|
||||
*
|
||||
* KKT * x =
|
||||
* [delta_g_cov', delta_lbA_cov', delta_ubA_cov', delta_lb_cov', delta_ub_cov]'
|
||||
*
|
||||
* for the first NVMAX (negative) elementary vectors in order to get
|
||||
* transposed inverse of the Hessian. Assuming that the Hessian is
|
||||
* symmetric, the function will return transposed inverse, instead of the
|
||||
* true inverse.
|
||||
*
|
||||
* Note, that we use negative elementary vectors due because internal
|
||||
* implementation of the function hotstart_determineStepDirection requires
|
||||
* so.
|
||||
*
|
||||
* */
|
||||
|
||||
for( run3 = 0; run3 < NVMAX; run3++ )
|
||||
{
|
||||
/* Line wise loading of the corresponding (negative) elementary vector: */
|
||||
delta_g_cov[ run3 ] = -1.0;
|
||||
|
||||
/* Evaluation of the step: */
|
||||
returnvalue = qp->hotstart_determineStepDirection(
|
||||
FR_idx, FX_idx, AC_idx,
|
||||
delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
|
||||
Delta_bC_isZero, Delta_bB_isZero,
|
||||
delta_xFX, delta_xFR, delta_yAC, delta_yFX
|
||||
);
|
||||
if ( returnvalue != SUCCESSFUL_RETURN )
|
||||
{
|
||||
return returnvalue;
|
||||
}
|
||||
|
||||
/* Line wise storage of the QP reaction: */
|
||||
for( run1 = 0; run1 < nFR; run1++ )
|
||||
{
|
||||
run2 = FR_idx[ run1 ];
|
||||
|
||||
hessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ];
|
||||
}
|
||||
|
||||
for( run1 = 0; run1 < nFX; run1++ )
|
||||
{
|
||||
run2 = FX_idx[ run1 ];
|
||||
|
||||
hessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ];
|
||||
}
|
||||
|
||||
/* Prepare for the next iteration */
|
||||
delta_g_cov[ run3 ] = 0.0;
|
||||
}
|
||||
|
||||
// TODO: Perform the transpose of the inverse of the Hessian matrix
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
/*
|
||||
* g e t H e s s i a n I n v e r s e
|
||||
*/
|
||||
returnValue SolutionAnalysis::getHessianInverse( QProblemB* qp, real_t* hessianInverse )
|
||||
{
|
||||
returnValue returnvalue; /* the return value */
|
||||
BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
|
||||
|
||||
register int run1, run2, run3;
|
||||
|
||||
register int nFR, nFX;
|
||||
|
||||
/* Ask for the number of free and fixed variables, assumes that active set
|
||||
* is constant for the covariance evaluation */
|
||||
nFR = qp->getNFR( );
|
||||
nFX = qp->getNFX( );
|
||||
|
||||
/* Ask for the corresponding index arrays: */
|
||||
if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_HOTSTART_FAILED );
|
||||
|
||||
if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_HOTSTART_FAILED );
|
||||
|
||||
/* Initialization: */
|
||||
for( run1 = 0; run1 < NVMAX; run1++ )
|
||||
delta_g_cov[ run1 ] = 0.0;
|
||||
|
||||
for( run1 = 0; run1 < NVMAX; run1++ )
|
||||
delta_lb_cov[ run1 ] = 0.0;
|
||||
|
||||
for( run1 = 0; run1 < NVMAX; run1++ )
|
||||
delta_ub_cov[ run1 ] = 0.0;
|
||||
|
||||
/* The following loop solves the following:
|
||||
*
|
||||
* KKT * x =
|
||||
* [delta_g_cov', delta_lb_cov', delta_ub_cov']'
|
||||
*
|
||||
* for the first NVMAX (negative) elementary vectors in order to get
|
||||
* transposed inverse of the Hessian. Assuming that the Hessian is
|
||||
* symmetric, the function will return transposed inverse, instead of the
|
||||
* true inverse.
|
||||
*
|
||||
* Note, that we use negative elementary vectors due because internal
|
||||
* implementation of the function hotstart_determineStepDirection requires
|
||||
* so.
|
||||
*
|
||||
* */
|
||||
|
||||
for( run3 = 0; run3 < NVMAX; run3++ )
|
||||
{
|
||||
/* Line wise loading of the corresponding (negative) elementary vector: */
|
||||
delta_g_cov[ run3 ] = -1.0;
|
||||
|
||||
/* Evaluation of the step: */
|
||||
returnvalue = qp->hotstart_determineStepDirection(
|
||||
FR_idx, FX_idx,
|
||||
delta_g_cov, delta_lb_cov, delta_ub_cov,
|
||||
Delta_bB_isZero,
|
||||
delta_xFX, delta_xFR, delta_yFX
|
||||
);
|
||||
if ( returnvalue != SUCCESSFUL_RETURN )
|
||||
{
|
||||
return returnvalue;
|
||||
}
|
||||
|
||||
/* Line wise storage of the QP reaction: */
|
||||
for( run1 = 0; run1 < nFR; run1++ )
|
||||
{
|
||||
run2 = FR_idx[ run1 ];
|
||||
|
||||
hessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ];
|
||||
}
|
||||
|
||||
for( run1 = 0; run1 < nFX; run1++ )
|
||||
{
|
||||
run2 = FX_idx[ run1 ];
|
||||
|
||||
hessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ];
|
||||
}
|
||||
|
||||
/* Prepare for the next iteration */
|
||||
delta_g_cov[ run3 ] = 0.0;
|
||||
}
|
||||
|
||||
// TODO: Perform the transpose of the inverse of the Hessian matrix
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
/*
|
||||
* g e t V a r i a n c e C o v a r i a n c e
|
||||
*/
|
||||
|
||||
#if QPOASES_USE_OLD_VERSION
|
||||
|
||||
returnValue SolutionAnalysis::getVarianceCovariance( QProblem* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR )
|
||||
{
|
||||
int run1, run2, run3; /* simple run variables (for loops). */
|
||||
|
||||
returnValue returnvalue; /* the return value */
|
||||
BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */
|
||||
BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
|
||||
|
||||
/* ASK FOR THE NUMBER OF FREE AND FIXED VARIABLES:
|
||||
* (ASSUMES THAT ACTIVE SET IS CONSTANT FOR THE
|
||||
* VARIANCE-COVARIANCE EVALUATION)
|
||||
* ----------------------------------------------- */
|
||||
int nFR, nFX, nAC;
|
||||
|
||||
nFR = qp->getNFR( );
|
||||
nFX = qp->getNFX( );
|
||||
nAC = qp->getNAC( );
|
||||
|
||||
if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_HOTSTART_FAILED );
|
||||
|
||||
if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_HOTSTART_FAILED );
|
||||
|
||||
if ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_HOTSTART_FAILED );
|
||||
|
||||
/* SOME INITIALIZATIONS:
|
||||
* --------------------- */
|
||||
for( run1 = 0; run1 < KKT_DIM * KKT_DIM; run1++ )
|
||||
{
|
||||
K [run1] = 0.0;
|
||||
Primal_Dual_VAR[run1] = 0.0;
|
||||
}
|
||||
|
||||
/* ================================================================= */
|
||||
|
||||
/* FIRST MATRIX MULTIPLICATION (OBTAINS THE INTERMEDIATE RESULT
|
||||
* K := [ ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * g_b_bA_VAR ]^T )
|
||||
* THE EVALUATION OF THE INVERSE OF THE KKT-MATRIX OF THE QP
|
||||
* WITH RESPECT TO THE CURRENT ACTIVE SET
|
||||
* USES THE EXISTING CHOLESKY AND TQ-DECOMPOSITIONS. FOR DETAILS
|
||||
* cf. THE (protected) FUNCTION determineStepDirection. */
|
||||
|
||||
for( run3 = 0; run3 < KKT_DIM; run3++ )
|
||||
{
|
||||
|
||||
for( run1 = 0; run1 < NVMAX; run1++ )
|
||||
{
|
||||
delta_g_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+run1];
|
||||
delta_lb_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /* LINE-WISE LOADING OF THE INPUT */
|
||||
delta_ub_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /* VARIANCE-COVARIANCE */
|
||||
}
|
||||
for( run1 = 0; run1 < NCMAX; run1++ )
|
||||
{
|
||||
delta_lbA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1];
|
||||
delta_ubA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1];
|
||||
}
|
||||
|
||||
/* EVALUATION OF THE STEP:
|
||||
* ------------------------------------------------------------------------------ */
|
||||
|
||||
returnvalue = qp->hotstart_determineStepDirection(
|
||||
FR_idx, FX_idx, AC_idx,
|
||||
delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
|
||||
Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
|
||||
delta_yAC,delta_yFX );
|
||||
|
||||
/* ------------------------------------------------------------------------------ */
|
||||
|
||||
/* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
|
||||
* ------------------------------------------------------ */
|
||||
if ( returnvalue != SUCCESSFUL_RETURN )
|
||||
{
|
||||
return returnvalue;
|
||||
}
|
||||
|
||||
/* LINE WISE */
|
||||
/* STORAGE OF THE QP-REACTION */
|
||||
/* (uses the index list) */
|
||||
|
||||
for( run1=0; run1<nFR; run1++ )
|
||||
{
|
||||
run2 = FR_idx[run1];
|
||||
K[run3*KKT_DIM+run2] = delta_xFR[run1];
|
||||
}
|
||||
for( run1=0; run1<nFX; run1++ )
|
||||
{
|
||||
run2 = FX_idx[run1];
|
||||
K[run3*KKT_DIM+run2] = delta_xFX[run1];
|
||||
K[run3*KKT_DIM+NVMAX+run2] = delta_yFX[run1];
|
||||
}
|
||||
for( run1=0; run1<nAC; run1++ )
|
||||
{
|
||||
run2 = AC_idx[run1];
|
||||
K[run3*KKT_DIM+2*NVMAX+run2] = delta_yAC[run1];
|
||||
}
|
||||
}
|
||||
|
||||
/* ================================================================= */
|
||||
|
||||
/* SECOND MATRIX MULTIPLICATION (OBTAINS THE FINAL RESULT
|
||||
* Primal_Dual_VAR := ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * K )
|
||||
* THE APPLICATION OF THE KKT-INVERSE IS AGAIN REALIZED
|
||||
* BY USING THE PROTECTED FUNCTION
|
||||
* determineStepDirection */
|
||||
|
||||
for( run3 = 0; run3 < KKT_DIM; run3++ )
|
||||
{
|
||||
|
||||
for( run1 = 0; run1 < NVMAX; run1++ )
|
||||
{
|
||||
delta_g_cov [run1] = K[run3+ run1*KKT_DIM];
|
||||
delta_lb_cov [run1] = K[run3+(NVMAX+run1)*KKT_DIM]; /* ROW WISE LOADING OF THE */
|
||||
delta_ub_cov [run1] = K[run3+(NVMAX+run1)*KKT_DIM]; /* INTERMEDIATE RESULT K */
|
||||
}
|
||||
for( run1 = 0; run1 < NCMAX; run1++ )
|
||||
{
|
||||
delta_lbA_cov [run1] = K[run3+(2*NVMAX+run1)*KKT_DIM];
|
||||
delta_ubA_cov [run1] = K[run3+(2*NVMAX+run1)*KKT_DIM];
|
||||
}
|
||||
|
||||
/* EVALUATION OF THE STEP:
|
||||
* ------------------------------------------------------------------------------ */
|
||||
|
||||
returnvalue = qp->hotstart_determineStepDirection(
|
||||
FR_idx, FX_idx, AC_idx,
|
||||
delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
|
||||
Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
|
||||
delta_yAC,delta_yFX );
|
||||
|
||||
/* ------------------------------------------------------------------------------ */
|
||||
|
||||
/* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
|
||||
* ------------------------------------------------------ */
|
||||
if ( returnvalue != SUCCESSFUL_RETURN )
|
||||
{
|
||||
return returnvalue;
|
||||
}
|
||||
|
||||
/* ROW-WISE STORAGE */
|
||||
/* OF THE RESULT. */
|
||||
|
||||
for( run1=0; run1<nFR; run1++ )
|
||||
{
|
||||
run2 = FR_idx[run1];
|
||||
Primal_Dual_VAR[run3+run2*KKT_DIM] = delta_xFR[run1];
|
||||
}
|
||||
for( run1=0; run1<nFX; run1++ )
|
||||
{
|
||||
run2 = FX_idx[run1];
|
||||
Primal_Dual_VAR[run3+run2*KKT_DIM ] = delta_xFX[run1];
|
||||
Primal_Dual_VAR[run3+(NVMAX+run2)*KKT_DIM] = delta_yFX[run1];
|
||||
}
|
||||
for( run1=0; run1<nAC; run1++ )
|
||||
{
|
||||
run2 = AC_idx[run1];
|
||||
Primal_Dual_VAR[run3+(2*NVMAX+run2)*KKT_DIM] = delta_yAC[run1];
|
||||
}
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,342 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/Indexlist.cpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of the Indexlist class designed to manage index lists of
|
||||
* constraints and bounds within a QProblem_SubjectTo.
|
||||
*/
|
||||
|
||||
|
||||
#include <Indexlist.hpp>
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/*
|
||||
* I n d e x l i s t
|
||||
*/
|
||||
Indexlist::Indexlist( ) : length( 0 ),
|
||||
first( -1 ),
|
||||
last( -1 ),
|
||||
lastusedindex( -1 ),
|
||||
physicallength( INDEXLISTFACTOR*(NVMAX+NCMAX) )
|
||||
{
|
||||
int i;
|
||||
|
||||
for( i=0; i<physicallength; ++i )
|
||||
{
|
||||
number[i] = -1;
|
||||
next[i] = -1;
|
||||
previous[i] = -1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* I n d e x l i s t
|
||||
*/
|
||||
Indexlist::Indexlist( const Indexlist& rhs ) : length( rhs.length ),
|
||||
first( rhs.first ),
|
||||
last( rhs.last ),
|
||||
lastusedindex( rhs.lastusedindex ),
|
||||
physicallength( rhs.physicallength )
|
||||
{
|
||||
int i;
|
||||
|
||||
for( i=0; i<physicallength; ++i )
|
||||
{
|
||||
number[i] = rhs.number[i];
|
||||
next[i] = rhs.next[i];
|
||||
previous[i] = rhs.previous[i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* ~ I n d e x l i s t
|
||||
*/
|
||||
Indexlist::~Indexlist( )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* o p e r a t o r =
|
||||
*/
|
||||
Indexlist& Indexlist::operator=( const Indexlist& rhs )
|
||||
{
|
||||
int i;
|
||||
|
||||
if ( this != &rhs )
|
||||
{
|
||||
length = rhs.length;
|
||||
first = rhs.first;
|
||||
last = rhs.last;
|
||||
lastusedindex = rhs.lastusedindex;
|
||||
physicallength = rhs.physicallength;
|
||||
|
||||
for( i=0; i<physicallength; ++i )
|
||||
{
|
||||
number[i] = rhs.number[i];
|
||||
next[i] = rhs.next[i];
|
||||
previous[i] = rhs.previous[i];
|
||||
}
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i n i t
|
||||
*/
|
||||
returnValue Indexlist::init( )
|
||||
{
|
||||
int i;
|
||||
|
||||
length = 0;
|
||||
first = -1;
|
||||
last = -1;
|
||||
lastusedindex = -1;
|
||||
physicallength = INDEXLISTFACTOR*(NVMAX+NCMAX);
|
||||
|
||||
for( i=0; i<physicallength; ++i )
|
||||
{
|
||||
number[i] = -1;
|
||||
next[i] = -1;
|
||||
previous[i] = -1;
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N u m b e r A r r a y
|
||||
*/
|
||||
returnValue Indexlist::getNumberArray( int* const numberarray ) const
|
||||
{
|
||||
int i;
|
||||
int n = first;
|
||||
|
||||
/* Run trough indexlist and store numbers in numberarray. */
|
||||
for( i=0; i<length; ++i )
|
||||
{
|
||||
if ( ( n >= 0 ) && ( number[n] >= 0 ) )
|
||||
numberarray[i] = number[n];
|
||||
else
|
||||
return THROWERROR( RET_INDEXLIST_CORRUPTED );
|
||||
|
||||
n = next[n];
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t I n d e x
|
||||
*/
|
||||
int Indexlist::getIndex( int givennumber ) const
|
||||
{
|
||||
int i;
|
||||
int n = first;
|
||||
int index = -1; /* return -1 by default */
|
||||
|
||||
/* Run trough indexlist until number is found, if so return it index. */
|
||||
for ( i=0; i<length; ++i )
|
||||
{
|
||||
if ( number[n] == givennumber )
|
||||
{
|
||||
index = i;
|
||||
break;
|
||||
}
|
||||
|
||||
n = next[n];
|
||||
}
|
||||
|
||||
return index;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t P h y s i c a l I n d e x
|
||||
*/
|
||||
int Indexlist::getPhysicalIndex( int givennumber ) const
|
||||
{
|
||||
int i;
|
||||
int n = first;
|
||||
int index = -1; /* return -1 by default */
|
||||
|
||||
/* Run trough indexlist until number is found, if so return it physicalindex. */
|
||||
for ( i=0; i<length; ++i )
|
||||
{
|
||||
if ( number[n] == givennumber )
|
||||
{
|
||||
index = n;
|
||||
break;
|
||||
}
|
||||
|
||||
n = next[n];
|
||||
}
|
||||
|
||||
return index;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* a d d N u m b e r
|
||||
*/
|
||||
returnValue Indexlist::addNumber( int addnumber )
|
||||
{
|
||||
int i;
|
||||
|
||||
if ( lastusedindex+1 < physicallength )
|
||||
{
|
||||
/* If there is enough storage, add number to indexlist. */
|
||||
++lastusedindex;
|
||||
number[lastusedindex] = addnumber;
|
||||
next[lastusedindex] = 0;
|
||||
|
||||
if ( length == 0 )
|
||||
{
|
||||
first = lastusedindex;
|
||||
previous[lastusedindex] = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
next[last] = lastusedindex;
|
||||
previous[lastusedindex] = last;
|
||||
}
|
||||
|
||||
last = lastusedindex;
|
||||
++length;
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Rearrangement of index list necessary! */
|
||||
if ( length == physicallength )
|
||||
return THROWERROR( RET_INDEXLIST_EXCEEDS_MAX_LENGTH );
|
||||
else
|
||||
{
|
||||
int numberArray[NVMAX+NCMAX];
|
||||
getNumberArray( numberArray );
|
||||
|
||||
/* copy existing elements */
|
||||
for ( i=0; i<length; ++i )
|
||||
{
|
||||
number[i] = numberArray[i];
|
||||
next[i] = i+1;
|
||||
previous[i] = i-1;
|
||||
}
|
||||
|
||||
/* add new number at end of list */
|
||||
number[length] = addnumber;
|
||||
next[length] = -1;
|
||||
previous[length] = length-1;
|
||||
|
||||
/* and set remaining entries to empty */
|
||||
for ( i=length+1; i<physicallength; ++i )
|
||||
{
|
||||
number[i] = -1;
|
||||
next[i] = -1;
|
||||
previous[i] = -1;
|
||||
}
|
||||
|
||||
first = 0;
|
||||
last = length;
|
||||
lastusedindex = length;
|
||||
++length;
|
||||
|
||||
return THROWWARNING( RET_INDEXLIST_MUST_BE_REORDERD );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* r e m o v e N u m b e r
|
||||
*/
|
||||
returnValue Indexlist::removeNumber( int removenumber )
|
||||
{
|
||||
int i = getPhysicalIndex( removenumber );
|
||||
|
||||
/* nothing to be done if number is not contained in index set */
|
||||
if ( i < 0 )
|
||||
return SUCCESSFUL_RETURN;
|
||||
|
||||
int p = previous[i];
|
||||
int n = next[i];
|
||||
|
||||
if ( i == last )
|
||||
last = p;
|
||||
else
|
||||
previous[n] = p;
|
||||
|
||||
if ( i == first )
|
||||
first = n;
|
||||
else
|
||||
next[p] = n;
|
||||
|
||||
number[i] = -1;
|
||||
next[i] = -1;
|
||||
previous[i] = -1;
|
||||
--length;
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s w a p N u m b e r s
|
||||
*/
|
||||
returnValue Indexlist::swapNumbers( int number1, int number2 )
|
||||
{
|
||||
int index1 = getPhysicalIndex( number1 );
|
||||
int index2 = getPhysicalIndex( number2 );
|
||||
|
||||
/* consistency check */
|
||||
if ( ( index1 < 0 ) || ( index2 < 0 ) )
|
||||
return THROWERROR( RET_INDEXLIST_CORRUPTED );
|
||||
|
||||
int tmp = number[index1];
|
||||
number[index1] = number[index2];
|
||||
number[index2] = tmp;
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,85 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/Indexlist.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of inlined member functions of the Indexlist class designed
|
||||
* to manage index lists of constraints and bounds within a QProblem_SubjectTo.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* g e t N u m b e r
|
||||
*/
|
||||
inline int Indexlist::getNumber( int physicalindex ) const
|
||||
{
|
||||
/* consistency check */
|
||||
if ( ( physicalindex < 0 ) || ( physicalindex > length ) )
|
||||
return -RET_INDEXLIST_OUTOFBOUNDS;
|
||||
|
||||
return number[physicalindex];
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t L e n g t h
|
||||
*/
|
||||
inline int Indexlist::getLength( )
|
||||
{
|
||||
return length;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t L a s t N u m b e r
|
||||
*/
|
||||
inline int Indexlist::getLastNumber( ) const
|
||||
{
|
||||
return number[last];
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t L a s t N u m b e r
|
||||
*/
|
||||
inline BooleanType Indexlist::isMember( int _number ) const
|
||||
{
|
||||
if ( getIndex( _number ) >= 0 )
|
||||
return BT_TRUE;
|
||||
else
|
||||
return BT_FALSE;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,529 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/MessageHandling.cpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of the MessageHandling class including global return values.
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include <MessageHandling.hpp>
|
||||
#include <Utils.hpp>
|
||||
|
||||
|
||||
|
||||
|
||||
/** Defines pairs of global return values and messages. */
|
||||
MessageHandling::ReturnValueList returnValueList[] =
|
||||
{
|
||||
/* miscellaneous */
|
||||
{ SUCCESSFUL_RETURN, "Successful return", VS_VISIBLE },
|
||||
{ RET_DIV_BY_ZERO, "Division by zero", VS_VISIBLE },
|
||||
{ RET_INDEX_OUT_OF_BOUNDS, "Index out of bounds", VS_VISIBLE },
|
||||
{ RET_INVALID_ARGUMENTS, "At least one of the arguments is invalid", VS_VISIBLE },
|
||||
{ RET_ERROR_UNDEFINED, "Error number undefined", VS_VISIBLE },
|
||||
{ RET_WARNING_UNDEFINED, "Warning number undefined", VS_VISIBLE },
|
||||
{ RET_INFO_UNDEFINED, "Info number undefined", VS_VISIBLE },
|
||||
{ RET_EWI_UNDEFINED, "Error/warning/info number undefined", VS_VISIBLE },
|
||||
{ RET_AVAILABLE_WITH_LINUX_ONLY, "This function is available under Linux only", VS_HIDDEN },
|
||||
{ RET_UNKNOWN_BUG, "The error occured is not yet known", VS_VISIBLE },
|
||||
{ RET_PRINTLEVEL_CHANGED, "Print level changed", VS_VISIBLE },
|
||||
{ RET_NOT_YET_IMPLEMENTED, "Requested function is not yet implemented.", VS_VISIBLE },
|
||||
/* Indexlist */
|
||||
{ RET_INDEXLIST_MUST_BE_REORDERD, "Index list has to be reordered", VS_VISIBLE },
|
||||
{ RET_INDEXLIST_EXCEEDS_MAX_LENGTH, "Index list exceeds its maximal physical length", VS_VISIBLE },
|
||||
{ RET_INDEXLIST_CORRUPTED, "Index list corrupted", VS_VISIBLE },
|
||||
{ RET_INDEXLIST_OUTOFBOUNDS, "Physical index is out of bounds", VS_VISIBLE },
|
||||
{ RET_INDEXLIST_ADD_FAILED, "Adding indices from another index set failed", VS_VISIBLE },
|
||||
{ RET_INDEXLIST_INTERSECT_FAILED, "Intersection with another index set failed", VS_VISIBLE },
|
||||
/* SubjectTo / Bounds / Constraints */
|
||||
{ RET_INDEX_ALREADY_OF_DESIRED_STATUS, "Index is already of desired status", VS_VISIBLE },
|
||||
{ RET_SWAPINDEX_FAILED, "Cannot swap between different indexsets", VS_VISIBLE },
|
||||
{ RET_ADDINDEX_FAILED, "Adding index to index set failed", VS_VISIBLE },
|
||||
{ RET_NOTHING_TO_DO, "Nothing to do", VS_VISIBLE },
|
||||
{ RET_SETUP_BOUND_FAILED, "Setting up bound index failed", VS_VISIBLE },
|
||||
{ RET_SETUP_CONSTRAINT_FAILED, "Setting up constraint index failed", VS_VISIBLE },
|
||||
{ RET_MOVING_BOUND_FAILED, "Moving bound between index sets failed", VS_VISIBLE },
|
||||
{ RET_MOVING_CONSTRAINT_FAILED, "Moving constraint between index sets failed", VS_VISIBLE },
|
||||
/* QProblem */
|
||||
{ RET_QP_ALREADY_INITIALISED, "QProblem has already been initialised", VS_VISIBLE },
|
||||
{ RET_NO_INIT_WITH_STANDARD_SOLVER, "Initialisation via extern QP solver is not yet implemented", VS_VISIBLE },
|
||||
{ RET_RESET_FAILED, "Reset failed", VS_VISIBLE },
|
||||
{ RET_INIT_FAILED, "Initialisation failed", VS_VISIBLE },
|
||||
{ RET_INIT_FAILED_TQ, "Initialisation failed due to TQ factorisation", VS_VISIBLE },
|
||||
{ RET_INIT_FAILED_CHOLESKY, "Initialisation failed due to Cholesky decomposition", VS_VISIBLE },
|
||||
{ RET_INIT_FAILED_HOTSTART, "Initialisation failed! QP could not be solved!", VS_VISIBLE },
|
||||
{ RET_INIT_FAILED_INFEASIBILITY, "Initial QP could not be solved due to infeasibility!", VS_VISIBLE },
|
||||
{ RET_INIT_FAILED_UNBOUNDEDNESS, "Initial QP could not be solved due to unboundedness!", VS_VISIBLE },
|
||||
{ RET_INIT_SUCCESSFUL, "Initialisation done", VS_VISIBLE },
|
||||
{ RET_OBTAINING_WORKINGSET_FAILED, "Failed to obtain working set for auxiliary QP", VS_VISIBLE },
|
||||
{ RET_SETUP_WORKINGSET_FAILED, "Failed to setup working set for auxiliary QP", VS_VISIBLE },
|
||||
{ RET_SETUP_AUXILIARYQP_FAILED, "Failed to setup auxiliary QP for initialised homotopy", VS_VISIBLE },
|
||||
{ RET_NO_EXTERN_SOLVER, "No extern QP solver available", VS_VISIBLE },
|
||||
{ RET_QP_UNBOUNDED, "QP is unbounded", VS_VISIBLE },
|
||||
{ RET_QP_INFEASIBLE, "QP is infeasible", VS_VISIBLE },
|
||||
{ RET_QP_NOT_SOLVED, "Problems occured while solving QP with standard solver", VS_VISIBLE },
|
||||
{ RET_QP_SOLVED, "QP successfully solved", VS_VISIBLE },
|
||||
{ RET_UNABLE_TO_SOLVE_QP, "Problems occured while solving QP", VS_VISIBLE },
|
||||
{ RET_INITIALISATION_STARTED, "Starting problem initialisation...", VS_VISIBLE },
|
||||
{ RET_HOTSTART_FAILED, "Unable to perform homotopy due to internal error", VS_VISIBLE },
|
||||
{ RET_HOTSTART_FAILED_TO_INIT, "Unable to initialise problem", VS_VISIBLE },
|
||||
{ RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED, "Unable to perform homotopy as previous QP is not solved", VS_VISIBLE },
|
||||
{ RET_ITERATION_STARTED, "Iteration", VS_VISIBLE },
|
||||
{ RET_SHIFT_DETERMINATION_FAILED, "Determination of shift of the QP data failed", VS_VISIBLE },
|
||||
{ RET_STEPDIRECTION_DETERMINATION_FAILED, "Determination of step direction failed", VS_VISIBLE },
|
||||
{ RET_STEPLENGTH_DETERMINATION_FAILED, "Determination of step direction failed", VS_VISIBLE },
|
||||
{ RET_OPTIMAL_SOLUTION_FOUND, "Optimal solution of neighbouring QP found", VS_VISIBLE },
|
||||
{ RET_HOMOTOPY_STEP_FAILED, "Unable to perform homotopy step", VS_VISIBLE },
|
||||
{ RET_HOTSTART_STOPPED_INFEASIBILITY, "Premature homotopy termination because QP is infeasible", VS_VISIBLE },
|
||||
{ RET_HOTSTART_STOPPED_UNBOUNDEDNESS, "Premature homotopy termination because QP is unbounded", VS_VISIBLE },
|
||||
{ RET_WORKINGSET_UPDATE_FAILED, "Unable to update working sets according to initial guesses", VS_VISIBLE },
|
||||
{ RET_MAX_NWSR_REACHED, "Maximum number of working set recalculations performed", VS_VISIBLE },
|
||||
{ RET_CONSTRAINTS_NOT_SPECIFIED, "Problem does comprise constraints! You have to specify new constraints' bounds", VS_VISIBLE },
|
||||
{ RET_INVALID_FACTORISATION_FLAG, "Invalid factorisation flag", VS_VISIBLE },
|
||||
{ RET_UNABLE_TO_SAVE_QPDATA, "Unable to save QP data", VS_VISIBLE },
|
||||
{ RET_STEPDIRECTION_FAILED_TQ, "Abnormal termination due to TQ factorisation", VS_VISIBLE },
|
||||
{ RET_STEPDIRECTION_FAILED_CHOLESKY, "Abnormal termination due to Cholesky factorisation", VS_VISIBLE },
|
||||
{ RET_CYCLING_DETECTED, "Cycling detected", VS_VISIBLE },
|
||||
{ RET_CYCLING_NOT_RESOLVED, "Cycling cannot be resolved, QP is probably infeasible", VS_VISIBLE },
|
||||
{ RET_CYCLING_RESOLVED, "Cycling probably resolved", VS_VISIBLE },
|
||||
{ RET_STEPSIZE, "", VS_VISIBLE },
|
||||
{ RET_STEPSIZE_NONPOSITIVE, "", VS_VISIBLE },
|
||||
{ RET_SETUPSUBJECTTOTYPE_FAILED, "Setup of SubjectToTypes failed", VS_VISIBLE },
|
||||
{ RET_ADDCONSTRAINT_FAILED, "Addition of constraint to working set failed", VS_VISIBLE },
|
||||
{ RET_ADDCONSTRAINT_FAILED_INFEASIBILITY, "Addition of constraint to working set failed", VS_VISIBLE },
|
||||
{ RET_ADDBOUND_FAILED, "Addition of bound to working set failed", VS_VISIBLE },
|
||||
{ RET_ADDBOUND_FAILED_INFEASIBILITY, "Addition of bound to working set failed", VS_VISIBLE },
|
||||
{ RET_REMOVECONSTRAINT_FAILED, "Removal of constraint from working set failed", VS_VISIBLE },
|
||||
{ RET_REMOVEBOUND_FAILED, "Removal of bound from working set failed", VS_VISIBLE },
|
||||
{ RET_REMOVE_FROM_ACTIVESET, "Removing from active set:", VS_VISIBLE },
|
||||
{ RET_ADD_TO_ACTIVESET, "Adding to active set:", VS_VISIBLE },
|
||||
{ RET_REMOVE_FROM_ACTIVESET_FAILED, "Removing from active set failed", VS_VISIBLE },
|
||||
{ RET_ADD_TO_ACTIVESET_FAILED, "Adding to active set failed", VS_VISIBLE },
|
||||
{ RET_CONSTRAINT_ALREADY_ACTIVE, "Constraint is already active", VS_VISIBLE },
|
||||
{ RET_ALL_CONSTRAINTS_ACTIVE, "All constraints are active, no further constraint can be added", VS_VISIBLE },
|
||||
{ RET_LINEARLY_DEPENDENT, "New bound/constraint is linearly dependent", VS_VISIBLE },
|
||||
{ RET_LINEARLY_INDEPENDENT, "New bound/constraint is linearly independent", VS_VISIBLE },
|
||||
{ RET_LI_RESOLVED, "Linear independence of active contraint matrix successfully resolved", VS_VISIBLE },
|
||||
{ RET_ENSURELI_FAILED, "Failed to ensure linear indepence of active contraint matrix", VS_VISIBLE },
|
||||
{ RET_ENSURELI_FAILED_TQ, "Abnormal termination due to TQ factorisation", VS_VISIBLE },
|
||||
{ RET_ENSURELI_FAILED_NOINDEX, "No index found, QP is probably infeasible", VS_VISIBLE },
|
||||
{ RET_ENSURELI_FAILED_CYCLING, "Cycling detected, QP is probably infeasible", VS_VISIBLE },
|
||||
{ RET_BOUND_ALREADY_ACTIVE, "Bound is already active", VS_VISIBLE },
|
||||
{ RET_ALL_BOUNDS_ACTIVE, "All bounds are active, no further bound can be added", VS_VISIBLE },
|
||||
{ RET_CONSTRAINT_NOT_ACTIVE, "Constraint is not active", VS_VISIBLE },
|
||||
{ RET_BOUND_NOT_ACTIVE, "Bound is not active", VS_VISIBLE },
|
||||
{ RET_HESSIAN_NOT_SPD, "Projected Hessian matrix not positive definite", VS_VISIBLE },
|
||||
{ RET_MATRIX_SHIFT_FAILED, "Unable to update matrices or to transform vectors", VS_VISIBLE },
|
||||
{ RET_MATRIX_FACTORISATION_FAILED, "Unable to calculate new matrix factorisations", VS_VISIBLE },
|
||||
{ RET_PRINT_ITERATION_FAILED, "Unable to print information on current iteration", VS_VISIBLE },
|
||||
{ RET_NO_GLOBAL_MESSAGE_OUTPUTFILE, "No global message output file initialised", VS_VISIBLE },
|
||||
/* Utils */
|
||||
{ RET_UNABLE_TO_OPEN_FILE, "Unable to open file", VS_VISIBLE },
|
||||
{ RET_UNABLE_TO_WRITE_FILE, "Unable to write into file", VS_VISIBLE },
|
||||
{ RET_UNABLE_TO_READ_FILE, "Unable to read from file", VS_VISIBLE },
|
||||
{ RET_FILEDATA_INCONSISTENT, "File contains inconsistent data", VS_VISIBLE },
|
||||
/* SolutionAnalysis */
|
||||
{ RET_NO_SOLUTION, "QP solution does not satisfy KKT optimality conditions", VS_VISIBLE },
|
||||
{ RET_INACCURATE_SOLUTION, "KKT optimality conditions not satisfied to sufficient accuracy", VS_VISIBLE },
|
||||
{ TERMINAL_LIST_ELEMENT, "", VS_HIDDEN } /* IMPORTANT: Terminal list element! */
|
||||
};
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/*
|
||||
* M e s s a g e H a n d l i n g
|
||||
*/
|
||||
MessageHandling::MessageHandling( ) : errorVisibility( VS_VISIBLE ),
|
||||
warningVisibility( VS_VISIBLE ),
|
||||
infoVisibility( VS_VISIBLE ),
|
||||
outputFile( myStdout ),
|
||||
errorCount( 0 )
|
||||
{
|
||||
}
|
||||
|
||||
/*
|
||||
* M e s s a g e H a n d l i n g
|
||||
*/
|
||||
MessageHandling::MessageHandling( myFILE* _outputFile ) :
|
||||
errorVisibility( VS_VISIBLE ),
|
||||
warningVisibility( VS_VISIBLE ),
|
||||
infoVisibility( VS_VISIBLE ),
|
||||
outputFile( _outputFile ),
|
||||
errorCount( 0 )
|
||||
{
|
||||
}
|
||||
|
||||
/*
|
||||
* M e s s a g e H a n d l i n g
|
||||
*/
|
||||
MessageHandling::MessageHandling( VisibilityStatus _errorVisibility,
|
||||
VisibilityStatus _warningVisibility,
|
||||
VisibilityStatus _infoVisibility
|
||||
) :
|
||||
errorVisibility( _errorVisibility ),
|
||||
warningVisibility( _warningVisibility ),
|
||||
infoVisibility( _infoVisibility ),
|
||||
outputFile( myStderr ),
|
||||
errorCount( 0 )
|
||||
{
|
||||
}
|
||||
|
||||
/*
|
||||
* M e s s a g e H a n d l i n g
|
||||
*/
|
||||
MessageHandling::MessageHandling( myFILE* _outputFile,
|
||||
VisibilityStatus _errorVisibility,
|
||||
VisibilityStatus _warningVisibility,
|
||||
VisibilityStatus _infoVisibility
|
||||
) :
|
||||
errorVisibility( _errorVisibility ),
|
||||
warningVisibility( _warningVisibility ),
|
||||
infoVisibility( _infoVisibility ),
|
||||
outputFile( _outputFile ),
|
||||
errorCount( 0 )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* M e s s a g e H a n d l i n g
|
||||
*/
|
||||
MessageHandling::MessageHandling( const MessageHandling& rhs ) :
|
||||
errorVisibility( rhs.errorVisibility ),
|
||||
warningVisibility( rhs.warningVisibility ),
|
||||
infoVisibility( rhs.infoVisibility ),
|
||||
outputFile( rhs.outputFile ),
|
||||
errorCount( rhs.errorCount )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* ~ M e s s a g e H a n d l i n g
|
||||
*/
|
||||
MessageHandling::~MessageHandling( )
|
||||
{
|
||||
#ifdef PC_DEBUG
|
||||
if ( outputFile != 0 )
|
||||
fclose( outputFile );
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* o p e r a t o r =
|
||||
*/
|
||||
MessageHandling& MessageHandling::operator=( const MessageHandling& rhs )
|
||||
{
|
||||
if ( this != &rhs )
|
||||
{
|
||||
errorVisibility = rhs.errorVisibility;
|
||||
warningVisibility = rhs.warningVisibility;
|
||||
infoVisibility = rhs.infoVisibility;
|
||||
outputFile = rhs.outputFile;
|
||||
errorCount = rhs.errorCount;
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* t h r o w E r r o r
|
||||
*/
|
||||
returnValue MessageHandling::throwError(
|
||||
returnValue Enumber,
|
||||
const char* additionaltext,
|
||||
const char* functionname,
|
||||
const char* filename,
|
||||
const unsigned long linenumber,
|
||||
VisibilityStatus localVisibilityStatus
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( Enumber <= SUCCESSFUL_RETURN )
|
||||
return throwError( RET_ERROR_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
|
||||
/* Call to common throwMessage function if error shall be displayed. */
|
||||
if ( errorVisibility == VS_VISIBLE )
|
||||
return throwMessage( Enumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"ERROR" );
|
||||
else
|
||||
return Enumber;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* t h r o w W a r n i n g
|
||||
*/
|
||||
returnValue MessageHandling::throwWarning(
|
||||
returnValue Wnumber,
|
||||
const char* additionaltext,
|
||||
const char* functionname,
|
||||
const char* filename,
|
||||
const unsigned long linenumber,
|
||||
VisibilityStatus localVisibilityStatus
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( Wnumber <= SUCCESSFUL_RETURN )
|
||||
return throwError( RET_WARNING_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
|
||||
/* Call to common throwMessage function if warning shall be displayed. */
|
||||
if ( warningVisibility == VS_VISIBLE )
|
||||
return throwMessage( Wnumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"WARNING" );
|
||||
else
|
||||
return Wnumber;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* t h r o w I n f o
|
||||
*/
|
||||
returnValue MessageHandling::throwInfo(
|
||||
returnValue Inumber,
|
||||
const char* additionaltext,
|
||||
const char* functionname,
|
||||
const char* filename,
|
||||
const unsigned long linenumber,
|
||||
VisibilityStatus localVisibilityStatus
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( Inumber < SUCCESSFUL_RETURN )
|
||||
return throwError( RET_INFO_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
|
||||
/* Call to common throwMessage function if info shall be displayed. */
|
||||
if ( infoVisibility == VS_VISIBLE )
|
||||
return throwMessage( Inumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"INFO" );
|
||||
else
|
||||
return Inumber;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* r e s e t
|
||||
*/
|
||||
returnValue MessageHandling::reset( )
|
||||
{
|
||||
setErrorVisibilityStatus( VS_VISIBLE );
|
||||
setWarningVisibilityStatus( VS_VISIBLE );
|
||||
setInfoVisibilityStatus( VS_VISIBLE );
|
||||
|
||||
setOutputFile( myStderr );
|
||||
setErrorCount( 0 );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* l i s t A l l M e s s a g e s
|
||||
*/
|
||||
returnValue MessageHandling::listAllMessages( )
|
||||
{
|
||||
#ifdef PC_DEBUG
|
||||
int keypos = 0;
|
||||
char myPrintfString[160];
|
||||
|
||||
/* Run through whole returnValueList and print each item. */
|
||||
while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
|
||||
{
|
||||
sprintf( myPrintfString," %d - %s \n",keypos,returnValueList[keypos].data );
|
||||
myPrintf( myPrintfString );
|
||||
|
||||
++keypos;
|
||||
}
|
||||
#endif
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P R O T E C T E D *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
#ifdef PC_DEBUG /* Re-define throwMessage function for embedded code! */
|
||||
|
||||
/*
|
||||
* t h r o w M e s s a g e
|
||||
*/
|
||||
returnValue MessageHandling::throwMessage(
|
||||
returnValue RETnumber,
|
||||
const char* additionaltext,
|
||||
const char* functionname,
|
||||
const char* filename,
|
||||
const unsigned long linenumber,
|
||||
VisibilityStatus localVisibilityStatus,
|
||||
const char* RETstring
|
||||
)
|
||||
{
|
||||
int i;
|
||||
|
||||
int keypos = 0;
|
||||
char myPrintfString[160];
|
||||
|
||||
/* 1) Determine number of whitespace for output. */
|
||||
char whitespaces[41];
|
||||
int numberOfWhitespaces = (errorCount-1)*2;
|
||||
|
||||
if ( numberOfWhitespaces < 0 )
|
||||
numberOfWhitespaces = 0;
|
||||
|
||||
if ( numberOfWhitespaces > 40 )
|
||||
numberOfWhitespaces = 40;
|
||||
|
||||
for( i=0; i<numberOfWhitespaces; ++i )
|
||||
whitespaces[i] = ' ';
|
||||
whitespaces[numberOfWhitespaces] = '\0';
|
||||
|
||||
/* 2) Find error/warning/info in list. */
|
||||
while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
|
||||
{
|
||||
if ( returnValueList[keypos].key == RETnumber )
|
||||
break;
|
||||
else
|
||||
++keypos;
|
||||
}
|
||||
|
||||
if ( returnValueList[keypos].key == TERMINAL_LIST_ELEMENT )
|
||||
{
|
||||
throwError( RET_EWI_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
return RETnumber;
|
||||
}
|
||||
|
||||
/* 3) Print error/warning/info. */
|
||||
if ( ( returnValueList[keypos].globalVisibilityStatus == VS_VISIBLE ) && ( localVisibilityStatus == VS_VISIBLE ) )
|
||||
{
|
||||
if ( errorCount > 0 )
|
||||
{
|
||||
sprintf( myPrintfString,"%s->", whitespaces );
|
||||
myPrintf( myPrintfString );
|
||||
}
|
||||
|
||||
if ( additionaltext == 0 )
|
||||
{
|
||||
sprintf( myPrintfString,"%s (%s, %s:%d): \t%s\n",
|
||||
RETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data
|
||||
);
|
||||
myPrintf( myPrintfString );
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf( myPrintfString,"%s (%s, %s:%d): \t%s %s\n",
|
||||
RETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data,additionaltext
|
||||
);
|
||||
myPrintf( myPrintfString );
|
||||
}
|
||||
|
||||
/* take care of proper indention for subsequent error messages */
|
||||
if ( RETstring[0] == 'E' )
|
||||
{
|
||||
++errorCount;
|
||||
}
|
||||
else
|
||||
{
|
||||
if ( errorCount > 0 )
|
||||
myPrintf( "\n" );
|
||||
errorCount = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return RETnumber;
|
||||
}
|
||||
|
||||
#else /* = PC_DEBUG not defined */
|
||||
|
||||
/*
|
||||
* t h r o w M e s s a g e
|
||||
*/
|
||||
returnValue MessageHandling::throwMessage(
|
||||
returnValue RETnumber,
|
||||
const char* additionaltext,
|
||||
const char* functionname,
|
||||
const char* filename,
|
||||
const unsigned long linenumber,
|
||||
VisibilityStatus localVisibilityStatus,
|
||||
const char* RETstring
|
||||
)
|
||||
{
|
||||
/* DUMMY CODE FOR PRETENDING USE OF ARGUMENTS
|
||||
* FOR SUPPRESSING COMPILER WARNINGS! */
|
||||
int i = 0;
|
||||
if ( additionaltext == 0 ) i++;
|
||||
if ( functionname == 0 ) i++;
|
||||
if ( filename == 0 ) i++;
|
||||
if ( linenumber == 0 ) i++;
|
||||
if ( localVisibilityStatus == VS_VISIBLE ) i++;
|
||||
if ( RETstring == 0 ) i++;
|
||||
/* END OF DUMMY CODE */
|
||||
|
||||
return RETnumber;
|
||||
}
|
||||
|
||||
#endif /* PC_DEBUG */
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* G L O B A L M E S S A G E H A N D L E R *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/** Global message handler for all qpOASES modules.*/
|
||||
MessageHandling globalMessageHandler( myStderr,VS_VISIBLE,VS_VISIBLE,VS_VISIBLE );
|
||||
|
||||
|
||||
/*
|
||||
* g e t G l o b a l M e s s a g e H a n d l e r
|
||||
*/
|
||||
MessageHandling* getGlobalMessageHandler( )
|
||||
{
|
||||
return &globalMessageHandler;
|
||||
}
|
||||
|
||||
const char* MessageHandling::getErrorString(int error)
|
||||
{
|
||||
return returnValueList[ error ].data;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,137 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/MessageHandling.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of inlined member functions of the MessageHandling class.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* g e t E r r o r V i s i b i l i t y S t a t u s
|
||||
*/
|
||||
inline VisibilityStatus MessageHandling::getErrorVisibilityStatus( ) const
|
||||
{
|
||||
return errorVisibility;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t W a r n i n g V i s i b i l i t y S t a t u s
|
||||
*/
|
||||
inline VisibilityStatus MessageHandling::getWarningVisibilityStatus( ) const
|
||||
{
|
||||
return warningVisibility;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t I n f o V i s i b i l i t y S t a t u s
|
||||
*/
|
||||
inline VisibilityStatus MessageHandling::getInfoVisibilityStatus( ) const
|
||||
{
|
||||
return infoVisibility;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t O u t p u t F i l e
|
||||
*/
|
||||
inline myFILE* MessageHandling::getOutputFile( ) const
|
||||
{
|
||||
return outputFile;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t E r r o r C o u n t
|
||||
*/
|
||||
inline int MessageHandling::getErrorCount( ) const
|
||||
{
|
||||
return errorCount;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t E r r o r V i s i b i l i t y S t a t u s
|
||||
*/
|
||||
inline void MessageHandling::setErrorVisibilityStatus( VisibilityStatus _errorVisibility )
|
||||
{
|
||||
errorVisibility = _errorVisibility;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t W a r n i n g V i s i b i l i t y S t a t u s
|
||||
*/
|
||||
inline void MessageHandling::setWarningVisibilityStatus( VisibilityStatus _warningVisibility )
|
||||
{
|
||||
warningVisibility = _warningVisibility;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t I n f o V i s i b i l i t y S t a t u s
|
||||
*/
|
||||
inline void MessageHandling::setInfoVisibilityStatus( VisibilityStatus _infoVisibility )
|
||||
{
|
||||
infoVisibility = _infoVisibility;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t O u t p u t F i l e
|
||||
*/
|
||||
inline void MessageHandling::setOutputFile( myFILE* _outputFile )
|
||||
{
|
||||
outputFile = _outputFile;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t E r r o r C o u n t
|
||||
*/
|
||||
inline returnValue MessageHandling::setErrorCount( int _errorCount )
|
||||
{
|
||||
if ( _errorCount >= 0 )
|
||||
{
|
||||
errorCount = _errorCount;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return RET_INVALID_ARGUMENTS;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,299 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/QProblem.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of inlined member functions of the QProblem class which
|
||||
* is able to use the newly developed online active set strategy for
|
||||
* parametric quadratic programming.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* g e t A
|
||||
*/
|
||||
inline returnValue QProblem::getA( real_t* const _A ) const
|
||||
{
|
||||
int i;
|
||||
|
||||
for ( i=0; i<getNV( )*getNC( ); ++i )
|
||||
_A[i] = A[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t A
|
||||
*/
|
||||
inline returnValue QProblem::getA( int number, real_t* const row ) const
|
||||
{
|
||||
int nV = getNV( );
|
||||
|
||||
if ( ( number >= 0 ) && ( number < getNC( ) ) )
|
||||
{
|
||||
for ( int i=0; i<nV; ++i )
|
||||
row[i] = A[number*NVMAX + i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t L B A
|
||||
*/
|
||||
inline returnValue QProblem::getLBA( real_t* const _lbA ) const
|
||||
{
|
||||
int i;
|
||||
|
||||
for ( i=0; i<getNC( ); ++i )
|
||||
_lbA[i] = lbA[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t L B A
|
||||
*/
|
||||
inline returnValue QProblem::getLBA( int number, real_t& value ) const
|
||||
{
|
||||
if ( ( number >= 0 ) && ( number < getNC( ) ) )
|
||||
{
|
||||
value = lbA[number];
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t U B A
|
||||
*/
|
||||
inline returnValue QProblem::getUBA( real_t* const _ubA ) const
|
||||
{
|
||||
int i;
|
||||
|
||||
for ( i=0; i<getNC( ); ++i )
|
||||
_ubA[i] = ubA[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t U B A
|
||||
*/
|
||||
inline returnValue QProblem::getUBA( int number, real_t& value ) const
|
||||
{
|
||||
if ( ( number >= 0 ) && ( number < getNC( ) ) )
|
||||
{
|
||||
value = ubA[number];
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t C o n s t r a i n t s
|
||||
*/
|
||||
inline returnValue QProblem::getConstraints( Constraints* const _constraints ) const
|
||||
{
|
||||
*_constraints = constraints;
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* g e t N C
|
||||
*/
|
||||
inline int QProblem::getNC( ) const
|
||||
{
|
||||
return constraints.getNC( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N E C
|
||||
*/
|
||||
inline int QProblem::getNEC( ) const
|
||||
{
|
||||
return constraints.getNEC( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N A C
|
||||
*/
|
||||
inline int QProblem::getNAC( )
|
||||
{
|
||||
return constraints.getNAC( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N I A C
|
||||
*/
|
||||
inline int QProblem::getNIAC( )
|
||||
{
|
||||
return constraints.getNIAC( );
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P R O T E C T E D *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/*
|
||||
* s e t A
|
||||
*/
|
||||
inline returnValue QProblem::setA( const real_t* const A_new )
|
||||
{
|
||||
int i, j;
|
||||
int nV = getNV( );
|
||||
int nC = getNC( );
|
||||
|
||||
/* Set constraint matrix AND update member AX. */
|
||||
for( j=0; j<nC; ++j )
|
||||
{
|
||||
Ax[j] = 0.0;
|
||||
|
||||
for( i=0; i<nV; ++i )
|
||||
{
|
||||
A[j*NVMAX + i] = A_new[j*nV + i];
|
||||
Ax[j] += A[j*NVMAX + i] * x[i];
|
||||
}
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t A
|
||||
*/
|
||||
inline returnValue QProblem::setA( int number, const real_t* const row )
|
||||
{
|
||||
int i;
|
||||
int nV = getNV( );
|
||||
|
||||
/* Set constraint matrix AND update member AX. */
|
||||
if ( ( number >= 0 ) && ( number < getNC( ) ) )
|
||||
{
|
||||
Ax[number] = 0.0;
|
||||
|
||||
for( i=0; i<nV; ++i )
|
||||
{
|
||||
A[number*NVMAX + i] = row[i];
|
||||
Ax[number] += A[number*NVMAX + i] * x[i];
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t L B A
|
||||
*/
|
||||
inline returnValue QProblem::setLBA( const real_t* const lbA_new )
|
||||
{
|
||||
int i;
|
||||
int nC = getNC();
|
||||
|
||||
for( i=0; i<nC; ++i )
|
||||
lbA[i] = lbA_new[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t L B A
|
||||
*/
|
||||
inline returnValue QProblem::setLBA( int number, real_t value )
|
||||
{
|
||||
if ( ( number >= 0 ) && ( number < getNC( ) ) )
|
||||
{
|
||||
lbA[number] = value;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t U B A
|
||||
*/
|
||||
inline returnValue QProblem::setUBA( const real_t* const ubA_new )
|
||||
{
|
||||
int i;
|
||||
int nC = getNC();
|
||||
|
||||
for( i=0; i<nC; ++i )
|
||||
ubA[i] = ubA_new[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t U B A
|
||||
*/
|
||||
inline returnValue QProblem::setUBA( int number, real_t value )
|
||||
{
|
||||
if ( ( number >= 0 ) && ( number < getNC( ) ) )
|
||||
{
|
||||
ubA[number] = value;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,425 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/QProblemB.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of inlined member functions of the QProblemB class which
|
||||
* is able to use the newly developed online active set strategy for
|
||||
* parametric quadratic programming.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include <math.h>
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* g e t H
|
||||
*/
|
||||
inline returnValue QProblemB::getH( real_t* const _H ) const
|
||||
{
|
||||
int i;
|
||||
|
||||
for ( i=0; i<getNV( )*getNV( ); ++i )
|
||||
_H[i] = H[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t G
|
||||
*/
|
||||
inline returnValue QProblemB::getG( real_t* const _g ) const
|
||||
{
|
||||
int i;
|
||||
|
||||
for ( i=0; i<getNV( ); ++i )
|
||||
_g[i] = g[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t L B
|
||||
*/
|
||||
inline returnValue QProblemB::getLB( real_t* const _lb ) const
|
||||
{
|
||||
int i;
|
||||
|
||||
for ( i=0; i<getNV( ); ++i )
|
||||
_lb[i] = lb[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t L B
|
||||
*/
|
||||
inline returnValue QProblemB::getLB( int number, real_t& value ) const
|
||||
{
|
||||
if ( ( number >= 0 ) && ( number < getNV( ) ) )
|
||||
{
|
||||
value = lb[number];
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
{
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t U B
|
||||
*/
|
||||
inline returnValue QProblemB::getUB( real_t* const _ub ) const
|
||||
{
|
||||
int i;
|
||||
|
||||
for ( i=0; i<getNV( ); ++i )
|
||||
_ub[i] = ub[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t U B
|
||||
*/
|
||||
inline returnValue QProblemB::getUB( int number, real_t& value ) const
|
||||
{
|
||||
if ( ( number >= 0 ) && ( number < getNV( ) ) )
|
||||
{
|
||||
value = ub[number];
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
{
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t B o u n d s
|
||||
*/
|
||||
inline returnValue QProblemB::getBounds( Bounds* const _bounds ) const
|
||||
{
|
||||
*_bounds = bounds;
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N V
|
||||
*/
|
||||
inline int QProblemB::getNV( ) const
|
||||
{
|
||||
return bounds.getNV( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N F R
|
||||
*/
|
||||
inline int QProblemB::getNFR( )
|
||||
{
|
||||
return bounds.getNFR( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N F X
|
||||
*/
|
||||
inline int QProblemB::getNFX( )
|
||||
{
|
||||
return bounds.getNFX( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N F V
|
||||
*/
|
||||
inline int QProblemB::getNFV( ) const
|
||||
{
|
||||
return bounds.getNFV( );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t S t a t u s
|
||||
*/
|
||||
inline QProblemStatus QProblemB::getStatus( ) const
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i s I n i t i a l i s e d
|
||||
*/
|
||||
inline BooleanType QProblemB::isInitialised( ) const
|
||||
{
|
||||
if ( status == QPS_NOTINITIALISED )
|
||||
return BT_FALSE;
|
||||
else
|
||||
return BT_TRUE;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i s S o l v e d
|
||||
*/
|
||||
inline BooleanType QProblemB::isSolved( ) const
|
||||
{
|
||||
if ( status == QPS_SOLVED )
|
||||
return BT_TRUE;
|
||||
else
|
||||
return BT_FALSE;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i s I n f e a s i b l e
|
||||
*/
|
||||
inline BooleanType QProblemB::isInfeasible( ) const
|
||||
{
|
||||
return infeasible;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i s U n b o u n d e d
|
||||
*/
|
||||
inline BooleanType QProblemB::isUnbounded( ) const
|
||||
{
|
||||
return unbounded;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t P r i n t L e v e l
|
||||
*/
|
||||
inline PrintLevel QProblemB::getPrintLevel( ) const
|
||||
{
|
||||
return printlevel;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t H e s s i a n T y p e
|
||||
*/
|
||||
inline HessianType QProblemB::getHessianType( ) const
|
||||
{
|
||||
return hessianType;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t H e s s i a n T y p e
|
||||
*/
|
||||
inline returnValue QProblemB::setHessianType( HessianType _hessianType )
|
||||
{
|
||||
hessianType = _hessianType;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P R O T E C T E D *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* s e t H
|
||||
*/
|
||||
inline returnValue QProblemB::setH( const real_t* const H_new )
|
||||
{
|
||||
int i, j;
|
||||
|
||||
int nV = getNV();
|
||||
|
||||
for( i=0; i<nV; ++i )
|
||||
for( j=0; j<nV; ++j )
|
||||
H[i*NVMAX + j] = H_new[i*nV + j];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t G
|
||||
*/
|
||||
inline returnValue QProblemB::setG( const real_t* const g_new )
|
||||
{
|
||||
int i;
|
||||
|
||||
int nV = getNV();
|
||||
|
||||
for( i=0; i<nV; ++i )
|
||||
g[i] = g_new[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t L B
|
||||
*/
|
||||
inline returnValue QProblemB::setLB( const real_t* const lb_new )
|
||||
{
|
||||
int i;
|
||||
|
||||
int nV = getNV();
|
||||
|
||||
for( i=0; i<nV; ++i )
|
||||
lb[i] = lb_new[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t L B
|
||||
*/
|
||||
inline returnValue QProblemB::setLB( int number, real_t value )
|
||||
{
|
||||
if ( ( number >= 0 ) && ( number < getNV( ) ) )
|
||||
{
|
||||
lb[number] = value;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
{
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t U B
|
||||
*/
|
||||
inline returnValue QProblemB::setUB( const real_t* const ub_new )
|
||||
{
|
||||
int i;
|
||||
|
||||
int nV = getNV();
|
||||
|
||||
for( i=0; i<nV; ++i )
|
||||
ub[i] = ub_new[i];
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t U B
|
||||
*/
|
||||
inline returnValue QProblemB::setUB( int number, real_t value )
|
||||
{
|
||||
if ( ( number >= 0 ) && ( number < getNV( ) ) )
|
||||
{
|
||||
ub[number] = value;
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
{
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* c o m p u t e G i v e n s
|
||||
*/
|
||||
inline void QProblemB::computeGivens( real_t xold, real_t yold, real_t& xnew, real_t& ynew,
|
||||
real_t& c, real_t& s
|
||||
) const
|
||||
{
|
||||
if ( getAbs( yold ) <= ZERO )
|
||||
{
|
||||
c = 1.0;
|
||||
s = 0.0;
|
||||
|
||||
xnew = xold;
|
||||
ynew = yold;
|
||||
}
|
||||
else
|
||||
{
|
||||
real_t t, mu;
|
||||
|
||||
mu = getAbs( xold );
|
||||
if ( getAbs( yold ) > mu )
|
||||
mu = getAbs( yold );
|
||||
|
||||
t = mu * sqrt( (xold/mu)*(xold/mu) + (yold/mu)*(yold/mu) );
|
||||
|
||||
if ( xold < 0.0 )
|
||||
t = -t;
|
||||
|
||||
c = xold/t;
|
||||
s = yold/t;
|
||||
xnew = t;
|
||||
ynew = 0.0;
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* a p p l y G i v e n s
|
||||
*/
|
||||
inline void QProblemB::applyGivens( real_t c, real_t s, real_t xold, real_t yold,
|
||||
real_t& xnew, real_t& ynew
|
||||
) const
|
||||
{
|
||||
/* Usual Givens plane rotation requiring four multiplications. */
|
||||
xnew = c*xold + s*yold;
|
||||
ynew = -s*xold + c*yold;
|
||||
// double nu = s/(1.0+c);
|
||||
//
|
||||
// xnew = xold*c + yold*s;
|
||||
// ynew = (xnew+xold)*nu - yold;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,200 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/SubjectTo.cpp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of the SubjectTo class designed to manage working sets of
|
||||
* constraints and bounds within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
#include <SubjectTo.hpp>
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/*
|
||||
* S u b j e c t T o
|
||||
*/
|
||||
SubjectTo::SubjectTo( ) : noLower( BT_TRUE ),
|
||||
noUpper( BT_TRUE ),
|
||||
size( 0 )
|
||||
{
|
||||
int i;
|
||||
|
||||
for( i=0; i<size; ++i )
|
||||
{
|
||||
type[i] = ST_UNKNOWN;
|
||||
status[i] = ST_UNDEFINED;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* S u b j e c t T o
|
||||
*/
|
||||
SubjectTo::SubjectTo( const SubjectTo& rhs ) : noLower( rhs.noLower ),
|
||||
noUpper( rhs.noUpper ),
|
||||
size( rhs.size )
|
||||
{
|
||||
int i;
|
||||
|
||||
for( i=0; i<size; ++i )
|
||||
{
|
||||
type[i] = rhs.type[i];
|
||||
status[i] = rhs.status[i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* ~ S u b j e c t T o
|
||||
*/
|
||||
SubjectTo::~SubjectTo( )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* o p e r a t o r =
|
||||
*/
|
||||
SubjectTo& SubjectTo::operator=( const SubjectTo& rhs )
|
||||
{
|
||||
int i;
|
||||
|
||||
if ( this != &rhs )
|
||||
{
|
||||
size = rhs.size;
|
||||
|
||||
for( i=0; i<size; ++i )
|
||||
{
|
||||
type[i] = rhs.type[i];
|
||||
status[i] = rhs.status[i];
|
||||
}
|
||||
|
||||
noLower = rhs.noLower;
|
||||
noUpper = rhs.noUpper;
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* i n i t
|
||||
*/
|
||||
returnValue SubjectTo::init( int n )
|
||||
{
|
||||
int i;
|
||||
|
||||
size = n;
|
||||
|
||||
noLower = BT_TRUE;
|
||||
noUpper = BT_TRUE;
|
||||
|
||||
for( i=0; i<size; ++i )
|
||||
{
|
||||
type[i] = ST_UNKNOWN;
|
||||
status[i] = ST_UNDEFINED;
|
||||
}
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P R O T E C T E D *
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* a d d I n d e x
|
||||
*/
|
||||
returnValue SubjectTo::addIndex( Indexlist* const indexlist,
|
||||
int newnumber, SubjectToStatus newstatus
|
||||
)
|
||||
{
|
||||
/* consistency check */
|
||||
if ( status[newnumber] == newstatus )
|
||||
return THROWERROR( RET_INDEX_ALREADY_OF_DESIRED_STATUS );
|
||||
|
||||
status[newnumber] = newstatus;
|
||||
|
||||
if ( indexlist->addNumber( newnumber ) == RET_INDEXLIST_EXCEEDS_MAX_LENGTH )
|
||||
return THROWERROR( RET_ADDINDEX_FAILED );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* r e m o v e I n d e x
|
||||
*/
|
||||
returnValue SubjectTo::removeIndex( Indexlist* const indexlist,
|
||||
int removenumber
|
||||
)
|
||||
{
|
||||
status[removenumber] = ST_UNDEFINED;
|
||||
|
||||
if ( indexlist->removeNumber( removenumber ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_UNKNOWN_BUG );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s w a p I n d e x
|
||||
*/
|
||||
returnValue SubjectTo::swapIndex( Indexlist* const indexlist,
|
||||
int number1, int number2
|
||||
)
|
||||
{
|
||||
/* consistency checks */
|
||||
if ( status[number1] != status[number2] )
|
||||
return THROWERROR( RET_SWAPINDEX_FAILED );
|
||||
|
||||
if ( number1 == number2 )
|
||||
{
|
||||
THROWWARNING( RET_NOTHING_TO_DO );
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
if ( indexlist->swapNumbers( number1,number2 ) != SUCCESSFUL_RETURN )
|
||||
return THROWERROR( RET_SWAPINDEX_FAILED );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,132 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/SubjectTo.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of the inlined member functions of the SubjectTo class
|
||||
* designed to manage working sets of constraints and bounds within a QProblem.
|
||||
*/
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* P U B L I C *
|
||||
*****************************************************************************/
|
||||
|
||||
|
||||
/*
|
||||
* g e t T y p e
|
||||
*/
|
||||
inline SubjectToType SubjectTo::getType( int i ) const
|
||||
{
|
||||
if ( ( i >= 0 ) && ( i < size ) )
|
||||
return type[i];
|
||||
else
|
||||
return ST_UNKNOWN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t S t a t u s
|
||||
*/
|
||||
inline SubjectToStatus SubjectTo::getStatus( int i ) const
|
||||
{
|
||||
if ( ( i >= 0 ) && ( i < size ) )
|
||||
return status[i];
|
||||
else
|
||||
return ST_UNDEFINED;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t T y p e
|
||||
*/
|
||||
inline returnValue SubjectTo::setType( int i, SubjectToType value )
|
||||
{
|
||||
if ( ( i >= 0 ) && ( i < size ) )
|
||||
{
|
||||
type[i] = value;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t S t a t u s
|
||||
*/
|
||||
inline returnValue SubjectTo::setStatus( int i, SubjectToStatus value )
|
||||
{
|
||||
if ( ( i >= 0 ) && ( i < size ) )
|
||||
{
|
||||
status[i] = value;
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
else
|
||||
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t N o L o w e r
|
||||
*/
|
||||
inline void SubjectTo::setNoLower( BooleanType _status )
|
||||
{
|
||||
noLower = _status;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* s e t N o U p p e r
|
||||
*/
|
||||
inline void SubjectTo::setNoUpper( BooleanType _status )
|
||||
{
|
||||
noUpper = _status;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i s N o L o w e r
|
||||
*/
|
||||
inline BooleanType SubjectTo::isNoLower( ) const
|
||||
{
|
||||
return noLower;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* i s N o L o w e r
|
||||
*/
|
||||
inline BooleanType SubjectTo::isNoUpper( ) const
|
||||
{
|
||||
return noUpper;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,471 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/Utils.cpp
|
||||
* \author Hans Joachim Ferreau, Eckhard Arnold
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of some inlined utilities for working with the different QProblem
|
||||
* classes.
|
||||
*/
|
||||
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#if defined(__WIN32__) || defined(WIN32)
|
||||
#include <windows.h>
|
||||
#elif defined(LINUX)
|
||||
#include <sys/stat.h>
|
||||
#include <sys/time.h>
|
||||
#endif
|
||||
|
||||
#ifdef __MATLAB__
|
||||
#include <mex.h>
|
||||
#endif
|
||||
|
||||
|
||||
#include <Utils.hpp>
|
||||
|
||||
|
||||
|
||||
#ifdef PC_DEBUG /* Define print functions only for debugging! */
|
||||
/*
|
||||
* p r i n t
|
||||
*/
|
||||
returnValue print( const real_t* const v, int n )
|
||||
{
|
||||
int i;
|
||||
char myPrintfString[160];
|
||||
|
||||
/* Print a vector. */
|
||||
myPrintf( "[\t" );
|
||||
for( i=0; i<n; ++i )
|
||||
{
|
||||
sprintf( myPrintfString," %.16e\t", v[i] );
|
||||
myPrintf( myPrintfString );
|
||||
}
|
||||
myPrintf( "]\n" );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* p r i n t
|
||||
*/
|
||||
returnValue print( const real_t* const v, int n,
|
||||
const int* const V_idx
|
||||
)
|
||||
{
|
||||
int i;
|
||||
char myPrintfString[160];
|
||||
|
||||
/* Print a permuted vector. */
|
||||
myPrintf( "[\t" );
|
||||
for( i=0; i<n; ++i )
|
||||
{
|
||||
sprintf( myPrintfString," %.16e\t", v[ V_idx[i] ] );
|
||||
myPrintf( myPrintfString );
|
||||
}
|
||||
myPrintf( "]\n" );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* p r i n t
|
||||
*/
|
||||
returnValue print( const real_t* const v, int n,
|
||||
const char* name
|
||||
)
|
||||
{
|
||||
char myPrintfString[160];
|
||||
|
||||
/* Print vector name ... */
|
||||
sprintf( myPrintfString,"%s = ", name );
|
||||
myPrintf( myPrintfString );
|
||||
|
||||
/* ... and the vector itself. */
|
||||
return print( v, n );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* p r i n t
|
||||
*/
|
||||
returnValue print( const real_t* const M, int nrow, int ncol )
|
||||
{
|
||||
int i;
|
||||
|
||||
/* Print a matrix as a collection of row vectors. */
|
||||
for( i=0; i<nrow; ++i )
|
||||
print( &(M[i*ncol]), ncol );
|
||||
myPrintf( "\n" );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* p r i n t
|
||||
*/
|
||||
returnValue print( const real_t* const M, int nrow, int ncol,
|
||||
const int* const ROW_idx, const int* const COL_idx
|
||||
)
|
||||
{
|
||||
int i;
|
||||
|
||||
/* Print a permuted matrix as a collection of permuted row vectors. */
|
||||
for( i=0; i<nrow; ++i )
|
||||
print( &( M[ ROW_idx[i]*ncol ] ), ncol, COL_idx );
|
||||
myPrintf( "\n" );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* p r i n t
|
||||
*/
|
||||
returnValue print( const real_t* const M, int nrow, int ncol,
|
||||
const char* name
|
||||
)
|
||||
{
|
||||
char myPrintfString[160];
|
||||
|
||||
/* Print matrix name ... */
|
||||
sprintf( myPrintfString,"%s = ", name );
|
||||
myPrintf( myPrintfString );
|
||||
|
||||
/* ... and the matrix itself. */
|
||||
return print( M, nrow, ncol );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* p r i n t
|
||||
*/
|
||||
returnValue print( const int* const index, int n )
|
||||
{
|
||||
int i;
|
||||
char myPrintfString[160];
|
||||
|
||||
/* Print a indexlist. */
|
||||
myPrintf( "[\t" );
|
||||
for( i=0; i<n; ++i )
|
||||
{
|
||||
sprintf( myPrintfString," %d\t", index[i] );
|
||||
myPrintf( myPrintfString );
|
||||
}
|
||||
myPrintf( "]\n" );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* p r i n t
|
||||
*/
|
||||
returnValue print( const int* const index, int n,
|
||||
const char* name
|
||||
)
|
||||
{
|
||||
char myPrintfString[160];
|
||||
|
||||
/* Print indexlist name ... */
|
||||
sprintf( myPrintfString,"%s = ", name );
|
||||
myPrintf( myPrintfString );
|
||||
|
||||
/* ... and the indexlist itself. */
|
||||
return print( index, n );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* m y P r i n t f
|
||||
*/
|
||||
returnValue myPrintf( const char* s )
|
||||
{
|
||||
#ifdef __MATLAB__
|
||||
mexPrintf( s );
|
||||
#else
|
||||
myFILE* outputfile = getGlobalMessageHandler( )->getOutputFile( );
|
||||
if ( outputfile == 0 )
|
||||
return THROWERROR( RET_NO_GLOBAL_MESSAGE_OUTPUTFILE );
|
||||
|
||||
fprintf( outputfile, "%s", s );
|
||||
#endif
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* p r i n t C o p y r i g h t N o t i c e
|
||||
*/
|
||||
returnValue printCopyrightNotice( )
|
||||
{
|
||||
return myPrintf( "\nqpOASES -- An Implementation of the Online Active Set Strategy.\nCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\n\nqpOASES is distributed under the terms of the \nGNU Lesser General Public License 2.1 in the hope that it will be \nuseful, but WITHOUT ANY WARRANTY; without even the implied warranty \nof MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. \nSee the GNU Lesser General Public License for more details.\n\n" );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* r e a d F r o m F i l e
|
||||
*/
|
||||
returnValue readFromFile( real_t* data, int nrow, int ncol,
|
||||
const char* datafilename
|
||||
)
|
||||
{
|
||||
int i, j;
|
||||
float float_data;
|
||||
myFILE* datafile;
|
||||
|
||||
/* 1) Open file. */
|
||||
if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
|
||||
{
|
||||
char errstr[80];
|
||||
sprintf( errstr,"(%s)",datafilename );
|
||||
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
}
|
||||
|
||||
/* 2) Read data from file. */
|
||||
for( i=0; i<nrow; ++i )
|
||||
{
|
||||
for( j=0; j<ncol; ++j )
|
||||
{
|
||||
if ( fscanf( datafile, "%f ", &float_data ) == 0 )
|
||||
{
|
||||
fclose( datafile );
|
||||
char errstr[80];
|
||||
sprintf( errstr,"(%s)",datafilename );
|
||||
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
}
|
||||
data[i*ncol + j] = ( (real_t) float_data );
|
||||
}
|
||||
}
|
||||
|
||||
/* 3) Close file. */
|
||||
fclose( datafile );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* r e a d F r o m F i l e
|
||||
*/
|
||||
returnValue readFromFile( real_t* data, int n,
|
||||
const char* datafilename
|
||||
)
|
||||
{
|
||||
return readFromFile( data, n, 1, datafilename );
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* r e a d F r o m F i l e
|
||||
*/
|
||||
returnValue readFromFile( int* data, int n,
|
||||
const char* datafilename
|
||||
)
|
||||
{
|
||||
int i;
|
||||
myFILE* datafile;
|
||||
|
||||
/* 1) Open file. */
|
||||
if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
|
||||
{
|
||||
char errstr[80];
|
||||
sprintf( errstr,"(%s)",datafilename );
|
||||
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
}
|
||||
|
||||
/* 2) Read data from file. */
|
||||
for( i=0; i<n; ++i )
|
||||
{
|
||||
if ( fscanf( datafile, "%d\n", &(data[i]) ) == 0 )
|
||||
{
|
||||
fclose( datafile );
|
||||
char errstr[80];
|
||||
sprintf( errstr,"(%s)",datafilename );
|
||||
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
}
|
||||
}
|
||||
|
||||
/* 3) Close file. */
|
||||
fclose( datafile );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* w r i t e I n t o F i l e
|
||||
*/
|
||||
returnValue writeIntoFile( const real_t* const data, int nrow, int ncol,
|
||||
const char* datafilename, BooleanType append
|
||||
)
|
||||
{
|
||||
int i, j;
|
||||
myFILE* datafile;
|
||||
|
||||
/* 1) Open file. */
|
||||
if ( append == BT_TRUE )
|
||||
{
|
||||
/* append data */
|
||||
if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
|
||||
{
|
||||
char errstr[80];
|
||||
sprintf( errstr,"(%s)",datafilename );
|
||||
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
/* do not append data */
|
||||
if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
|
||||
{
|
||||
char errstr[80];
|
||||
sprintf( errstr,"(%s)",datafilename );
|
||||
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
}
|
||||
}
|
||||
|
||||
/* 2) Write data into file. */
|
||||
for( i=0; i<nrow; ++i )
|
||||
{
|
||||
for( j=0; j<ncol; ++j )
|
||||
fprintf( datafile, "%.16e ", data[i*ncol+j] );
|
||||
|
||||
fprintf( datafile, "\n" );
|
||||
}
|
||||
|
||||
/* 3) Close file. */
|
||||
fclose( datafile );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* w r i t e I n t o F i l e
|
||||
*/
|
||||
returnValue writeIntoFile( const real_t* const data, int n,
|
||||
const char* datafilename, BooleanType append
|
||||
)
|
||||
{
|
||||
return writeIntoFile( data,1,n,datafilename,append );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* w r i t e I n t o F i l e
|
||||
*/
|
||||
returnValue writeIntoFile( const int* const data, int n,
|
||||
const char* datafilename, BooleanType append
|
||||
)
|
||||
{
|
||||
int i;
|
||||
|
||||
myFILE* datafile;
|
||||
|
||||
/* 1) Open file. */
|
||||
if ( append == BT_TRUE )
|
||||
{
|
||||
/* append data */
|
||||
if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
|
||||
{
|
||||
char errstr[80];
|
||||
sprintf( errstr,"(%s)",datafilename );
|
||||
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
/* do not append data */
|
||||
if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
|
||||
{
|
||||
char errstr[80];
|
||||
sprintf( errstr,"(%s)",datafilename );
|
||||
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
|
||||
}
|
||||
}
|
||||
|
||||
/* 2) Write data into file. */
|
||||
for( i=0; i<n; ++i )
|
||||
fprintf( datafile, "%d\n", data[i] );
|
||||
|
||||
/* 3) Close file. */
|
||||
fclose( datafile );
|
||||
|
||||
return SUCCESSFUL_RETURN;
|
||||
}
|
||||
#endif /* PC_DEBUG */
|
||||
|
||||
|
||||
/*
|
||||
* g e t C P U t i m e
|
||||
*/
|
||||
real_t getCPUtime( )
|
||||
{
|
||||
real_t current_time = -1.0;
|
||||
|
||||
#if defined(__WIN32__) || defined(WIN32)
|
||||
LARGE_INTEGER counter, frequency;
|
||||
QueryPerformanceFrequency(&frequency);
|
||||
QueryPerformanceCounter(&counter);
|
||||
current_time = ((real_t) counter.QuadPart) / ((real_t) frequency.QuadPart);
|
||||
#elif defined(LINUX)
|
||||
struct timeval theclock;
|
||||
gettimeofday( &theclock,0 );
|
||||
current_time = 1.0*theclock.tv_sec + 1.0e-6*theclock.tv_usec;
|
||||
#endif
|
||||
|
||||
return current_time;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* g e t N o r m
|
||||
*/
|
||||
real_t getNorm( const real_t* const v, int n )
|
||||
{
|
||||
int i;
|
||||
|
||||
real_t norm = 0.0;
|
||||
|
||||
for( i=0; i<n; ++i )
|
||||
norm += v[i]*v[i];
|
||||
|
||||
return sqrt( norm );
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -0,0 +1,51 @@
|
|||
/*
|
||||
* This file is part of qpOASES.
|
||||
*
|
||||
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
||||
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
|
||||
*
|
||||
* qpOASES is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* qpOASES is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with qpOASES; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* \file SRC/Utils.ipp
|
||||
* \author Hans Joachim Ferreau
|
||||
* \version 1.3embedded
|
||||
* \date 2007-2008
|
||||
*
|
||||
* Implementation of some inlined utilities for working with the different QProblem
|
||||
* classes.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* g e t A b s
|
||||
*/
|
||||
inline real_t getAbs( real_t x )
|
||||
{
|
||||
if ( x < 0.0 )
|
||||
return -x;
|
||||
else
|
||||
return x;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* end of file
|
||||
*/
|
||||
|
|
@ -30,6 +30,11 @@ constexpr int LEAD_MHP_VALS = LEAD_PRED_DIM*LEAD_TRAJ_LEN;
|
|||
constexpr int LEAD_MHP_SELECTION = 3;
|
||||
constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION);
|
||||
|
||||
constexpr int STOP_LINE_MHP_N = 3;
|
||||
constexpr int STOP_LINE_PRED_DIM = 8;
|
||||
constexpr int STOP_LINE_MHP_SELECTION = 1;
|
||||
constexpr int STOP_LINE_MHP_GROUP_SIZE = (2*STOP_LINE_PRED_DIM + STOP_LINE_MHP_SELECTION);
|
||||
|
||||
constexpr int POSE_SIZE = 12;
|
||||
|
||||
constexpr int PLAN_IDX = 0;
|
||||
|
|
@ -38,7 +43,9 @@ constexpr int LL_PROB_IDX = LL_IDX + 4*2*2*33;
|
|||
constexpr int RE_IDX = LL_PROB_IDX + 8;
|
||||
constexpr int LEAD_IDX = RE_IDX + 2*2*2*33;
|
||||
constexpr int LEAD_PROB_IDX = LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE);
|
||||
constexpr int DESIRE_STATE_IDX = LEAD_PROB_IDX + 3;
|
||||
constexpr int STOP_LINE_IDX = LEAD_PROB_IDX + 3;
|
||||
constexpr int STOP_LINE_PROB_IDX = STOP_LINE_IDX + STOP_LINE_MHP_N*STOP_LINE_MHP_GROUP_SIZE;
|
||||
constexpr int DESIRE_STATE_IDX = STOP_LINE_PROB_IDX + 1;
|
||||
constexpr int META_IDX = DESIRE_STATE_IDX + DESIRE_LEN;
|
||||
constexpr int POSE_IDX = META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE;
|
||||
constexpr int OUTPUT_SIZE = POSE_IDX + POSE_SIZE;
|
||||
|
|
@ -117,6 +124,8 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int heigh
|
|||
net_outputs.road_edges = &s->output[RE_IDX];
|
||||
net_outputs.lead = &s->output[LEAD_IDX];
|
||||
net_outputs.lead_prob = &s->output[LEAD_PROB_IDX];
|
||||
net_outputs.stop_line = &s->output[STOP_LINE_IDX];
|
||||
net_outputs.stop_line_prob = &s->output[STOP_LINE_PROB_IDX];
|
||||
net_outputs.meta = &s->output[DESIRE_STATE_IDX];
|
||||
net_outputs.pose = &s->output[POSE_IDX];
|
||||
return net_outputs;
|
||||
|
|
@ -145,6 +154,10 @@ static const float *get_lead_data(const float *lead, int t_offset) {
|
|||
return get_best_data(lead, LEAD_MHP_N, LEAD_MHP_GROUP_SIZE, t_offset - LEAD_MHP_SELECTION);
|
||||
}
|
||||
|
||||
static const float *get_stop_line_data(const float *stop_line) {
|
||||
return get_best_data(stop_line, STOP_LINE_MHP_N, STOP_LINE_MHP_GROUP_SIZE, -1);
|
||||
}
|
||||
|
||||
|
||||
void fill_sigmoid(const float *input, float *output, int len, int stride) {
|
||||
for (int i=0; i<len; i++) {
|
||||
|
|
@ -186,6 +199,29 @@ void fill_lead_v3(cereal::ModelDataV2::LeadDataV3::Builder lead, const float *le
|
|||
lead.setAStd(a_stds_arr);
|
||||
}
|
||||
|
||||
void fill_stop_line(cereal::ModelDataV2::StopLineData::Builder stop_line, const float *stop_line_data, const float *prob) {
|
||||
const float *data = get_stop_line_data(stop_line_data);
|
||||
stop_line.setProb(prob[0]);
|
||||
|
||||
stop_line.setX(data[0]);
|
||||
stop_line.setY(data[1]);
|
||||
stop_line.setZ(data[2]);
|
||||
stop_line.setRoll(data[3]);
|
||||
stop_line.setPitch(data[4]);
|
||||
stop_line.setYaw(data[5]);
|
||||
stop_line.setSpeedAtLine(data[6]);
|
||||
stop_line.setSecondsUntilLine(data[7]);
|
||||
|
||||
stop_line.setXStd(data[STOP_LINE_PRED_DIM+0]);
|
||||
stop_line.setYStd(data[STOP_LINE_PRED_DIM+1]);
|
||||
stop_line.setZStd(data[STOP_LINE_PRED_DIM+2]);
|
||||
stop_line.setRollStd(data[STOP_LINE_PRED_DIM+3]);
|
||||
stop_line.setPitchStd(data[STOP_LINE_PRED_DIM+4]);
|
||||
stop_line.setYawStd(data[STOP_LINE_PRED_DIM+5]);
|
||||
stop_line.setSpeedAtLineStd(data[STOP_LINE_PRED_DIM+6]);
|
||||
stop_line.setSecondsUntilLineStd(data[STOP_LINE_PRED_DIM+7]);
|
||||
}
|
||||
|
||||
void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_data) {
|
||||
float desire_state_softmax[DESIRE_LEN];
|
||||
float desire_pred_softmax[4*DESIRE_LEN];
|
||||
|
|
@ -328,6 +364,9 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
|
|||
// meta
|
||||
fill_meta(framed.initMeta(), net_outputs.meta);
|
||||
|
||||
// stop line
|
||||
fill_stop_line(framed.initStopLine(), net_outputs.stop_line, net_outputs.stop_line_prob);
|
||||
|
||||
// leads
|
||||
auto leads = framed.initLeadsV3(LEAD_MHP_SELECTION);
|
||||
float t_offsets[LEAD_MHP_SELECTION] = {0.0, 2.0, 4.0};
|
||||
|
|
|
|||
|
|
@ -25,6 +25,8 @@ struct ModelDataRaw {
|
|||
float *road_edges;
|
||||
float *lead;
|
||||
float *lead_prob;
|
||||
float *stop_line;
|
||||
float *stop_line_prob;
|
||||
float *desire_state;
|
||||
float *meta;
|
||||
float *desire_pred;
|
||||
|
|
|
|||
Loading…
Reference in New Issue