You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
425 lines
6.7 KiB
425 lines
6.7 KiB
/*
|
|
* 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
|
|
*/
|
|
|