Exemplo n.º 1
0
		Bounds::Ptr expand(const Bounds &bounds) const {
			Bounds::Ptr pBounds;

			switch (bounds.getType()) {
			case Bounds::TYPE_PLANE:
				pBounds = expand(dynamic_cast<const BoundingPlane&>(bounds));
				break;
			case Bounds::TYPE_SPHERE:
				pBounds = expand(dynamic_cast<const BoundingSphere&>(bounds));
				break;
			case Bounds::TYPE_CYLINDER:
				pBounds = expand(dynamic_cast<const BoundingCylinder&>(bounds));
				break;
			case Bounds::TYPE_BOX:
				pBounds = expand(dynamic_cast<const BoundingBox&>(bounds));
				break;
			case Bounds::TYPE_CONVEX_MESH:
				pBounds = expand(dynamic_cast<const BoundingConvexMesh&>(bounds));
				break;
			default:
				break;
			}

			return pBounds;
		}
Exemplo n.º 2
0
/*
 *	s e t u p A u x i l i a r y Q P
 */
returnValue SQProblem::setupAuxiliaryQP ( SymmetricMatrix *H_new, Matrix *A_new,
    const real_t *lb_new, const real_t *ub_new, const real_t *lbA_new, const real_t *ubA_new
)
{

	int i;
	int nV = getNV( );
	int nC = getNC( );
	returnValue returnvalue;

	if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
		 ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
		 ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
	{
		return THROWERROR( RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED );
	}

	status = QPS_PREPARINGAUXILIARYQP;


	/* I) SETUP NEW QP MATRICES AND VECTORS: */
	/* 1) Shift constraints' bounds vectors by (A_new - A)'*x_opt to ensure
	 *    that old optimal solution remains feasible for new QP data. */
	/*    Firstly, shift by -A'*x_opt and ... */
	if ( nC > 0 )
	{
		if ( A_new == 0 )
			return THROWERROR( RET_INVALID_ARGUMENTS );

		for ( i=0; i<nC; ++i )
		{
			lbA[i] = -Ax_l[i];
			ubA[i] =  Ax_u[i];
		}

		/* Set constraint matrix as well as ... */
		setA( A_new );

		/* ... secondly, shift by +A_new'*x_opt. */
		for ( i=0; i<nC; ++i )
		{
			lbA[i] += Ax[i];
			ubA[i] += Ax[i];
		}

		/* update constraint products. */
		for ( i=0; i<nC; ++i )
		{
			Ax_u[i] = ubA[i] - Ax[i];
			Ax_l[i] = Ax[i] - lbA[i];
		}
	}

	/* 2) Set new Hessian matrix, determine Hessian type and
	 *    regularise new Hessian matrix if necessary. */
	/* a) Setup new Hessian matrix and determine its type. */
	if ( H_new != 0 )
	{
		setH( H_new );

		hessianType = HST_UNKNOWN;
		if ( determineHessianType( ) != SUCCESSFUL_RETURN )
			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );

		/* b) Regularise new Hessian if necessary. */
		if ( ( hessianType == HST_ZERO ) ||
			 ( hessianType == HST_SEMIDEF ) ||
			 ( usingRegularisation( ) == BT_TRUE ) )
		{
			regVal = 0.0; /* reset previous regularisation */

			if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
				return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
		}
	}
	else
	{
		if ( H != 0 )
			return THROWERROR( RET_NO_HESSIAN_SPECIFIED );
	}

	/* 3) Setup QP gradient. */
	if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
		return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );


	/* II) SETUP WORKING SETS AND MATRIX FACTORISATIONS: */
	/* 1) Make a copy of current bounds/constraints ... */
	Bounds      oldBounds      = bounds;
	Constraints oldConstraints = constraints;

    /* we're trying to find an active set with positive definite null
     * space Hessian twice:
     * - first for the current active set including all equalities
     * - second after moving all inactive variables to a bound
     *   (depending on Options). This creates an empty null space and
     *   is guaranteed to succeed. Thus this loop will exit after n_try=1.
     */
    int n_try;
    for (n_try = 0; n_try < 2; ++n_try) {

        if (n_try > 0) {
            // the current active set leaves an indefinite null space Hessian
            // move all inactive variables to a bound, creating an empty null space
            for (int ii = 0; ii < nV; ++ii)
                if (oldBounds.getStatus (ii) == ST_INACTIVE)
                    oldBounds.setStatus (ii, options.initialStatusBounds);
        }

        /*    ... reset them ... */
        bounds.init( nV );
        constraints.init( nC );

        /*    ... and set them up afresh. */
        if ( setupSubjectToType(lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );

        if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );

        if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );

        /* 2) Setup TQ factorisation. */
        if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );

		// check for equalities that have become bounds ...
		for (int ii = 0; ii < nC; ++ii) {
			if (oldConstraints.getType (ii) == ST_EQUALITY && constraints.getType (ii) == ST_BOUNDED) {
				if (oldConstraints.getStatus (ii) == ST_LOWER && y[nV+ii] < 0.0)
					oldConstraints.setStatus (ii, ST_UPPER);
				else if (oldConstraints.getStatus (ii) == ST_UPPER && y[nV+ii] > 0.0)
					oldConstraints.setStatus (ii, ST_LOWER);
			}
		}

		// ... and do the same also for the bounds!
		for (int ii = 0; ii < nV; ++ii) {
			if (oldBounds.getType(ii) == ST_EQUALITY
					&& bounds.getType(ii) == ST_BOUNDED) {
				if (oldBounds.getStatus(ii) == ST_LOWER && y[ii] < 0.0)
					oldBounds.setStatus(ii, ST_UPPER);
				else if (oldBounds.getStatus(ii) == ST_UPPER && y[ii] > 0.0)
					oldBounds.setStatus(ii, ST_LOWER);
			}
		}

        /* 3) Setup old working sets afresh (updating TQ factorisation). */
        if ( setupAuxiliaryWorkingSet( &oldBounds,&oldConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );

        /* Factorise projected Hessian
        * this now handles all special cases (no active bounds/constraints, no nullspace) */
        returnvalue = computeProjectedCholesky( );

        /* leave the loop if decomposition was successful, i.e. we have
         * found an active set with positive definite null space Hessian */
        if ( returnvalue == SUCCESSFUL_RETURN )
            break;
    }

    /* adjust lb/ub if we changed the old active set in the second try
     */
    if (n_try > 0) {
		// as per setupAuxiliaryQPbounds assumptions ... oh the troubles
		for (int ii = 0; ii < nC; ++ii)
			Ax_l[ii] = Ax_u[ii] = Ax[ii];
        setupAuxiliaryQPbounds (&bounds, &constraints, BT_FALSE);
	}

	status = QPS_AUXILIARYQPSOLVED;

	return SUCCESSFUL_RETURN;
}