ipopt-src 0.2.3+3.14.16

// Copyright (C) 2004, 2010 International Business Machines and others.
// All Rights Reserved.
// This code is published under the Eclipse Public License.
//
// Authors:  Carl Laird, Andreas Waechter     IBM    2004-08-13

#include "IpRestoIpoptNLP.hpp"
#include "IpIdentityMatrix.hpp"
#include "IpSumSymMatrix.hpp"
#include "IpSumMatrix.hpp"
#include "IpNLPScaling.hpp"
#include "IpLowRankUpdateSymMatrix.hpp"
#include "IpIpoptData.hpp"
#include "IpIpoptCalculatedQuantities.hpp"

#include <cmath>
#include <cassert>

namespace Ipopt
{
#if IPOPT_VERBOSITY > 0
static const Index dbg_verbosity = 0;
#endif

RestoIpoptNLP::RestoIpoptNLP(
   IpoptNLP&                  orig_ip_nlp,
   IpoptData&                 orig_ip_data,
   IpoptCalculatedQuantities& orig_ip_cq
)
   : IpoptNLP(new NoNLPScalingObject()),
     orig_ip_nlp_(&orig_ip_nlp),
     orig_ip_data_(&orig_ip_data),
     orig_ip_cq_(&orig_ip_cq),
     eta_mu_exponent_(0.5)
{ }

RestoIpoptNLP::~RestoIpoptNLP()
{ }

void RestoIpoptNLP::RegisterOptions(
   SmartPtr<RegisteredOptions> roptions
)
{
   roptions->AddBoolOption(
      "evaluate_orig_obj_at_resto_trial",
      "Determines if the original objective function should be evaluated at restoration phase trial points.",
      true,
      "Enabling this option makes the restoration phase algorithm evaluate the objective function "
      "of the original problem at every trial point encountered during the restoration phase, "
      "even if this value is not required.  "
      "In this way, it is guaranteed that the original objective function can be evaluated without error "
      "at all accepted iterates; "
      "otherwise the algorithm might fail at a point where the restoration phase accepts an iterate that is good "
      "for the restoration phase problem, but not the original problem. "
      "On the other hand, if the evaluation of the original objective is expensive, this might be costly.");
   roptions->AddLowerBoundedNumberOption(
      "resto_penalty_parameter",
      "Penalty parameter in the restoration phase objective function.",
      0., true,
      1e3,
      "This is the parameter rho in equation (31a) in the Ipopt implementation paper.",
      true);
   roptions->AddLowerBoundedNumberOption(
      "resto_proximity_weight",
      "Weighting factor for the proximity term in restoration phase objective.",
      0., false,
      1.,
      "This determines how the parameter zeta in equation (29a) in the implementation paper is computed. "
      "zeta here is resto_proximity_weight*sqrt(mu), where mu is the current barrier parameter.",
      true);
}

bool RestoIpoptNLP::Initialize(
   const Journalist&  jnlst,
   const OptionsList& options,
   const std::string& prefix
)
{
   options.GetBoolValue("evaluate_orig_obj_at_resto_trial", evaluate_orig_obj_at_resto_trial_, prefix);
   options.GetNumericValue("resto_penalty_parameter", rho_, prefix);
   Index enum_int;
   options.GetEnumValue("hessian_approximation", enum_int, prefix);
   hessian_approximation_ = HessianApproximationType(enum_int);
   options.GetNumericValue("resto_proximity_weight", eta_factor_, prefix);

   initialized_ = true;
   return IpoptNLP::Initialize(jnlst, options, prefix);
}

bool RestoIpoptNLP::InitializeStructures(
   SmartPtr<Vector>& x,
   bool              init_x,
   SmartPtr<Vector>& y_c,
   bool              init_y_c,
   SmartPtr<Vector>& y_d,
   bool              init_y_d,
   SmartPtr<Vector>& z_L,
   bool              init_z_L,
   SmartPtr<Vector>& z_U,
   bool              init_z_U,
   SmartPtr<Vector>& v_L,
   SmartPtr<Vector>& v_U)
{
   DBG_START_METH("RestoIpoptNLP::InitializeStructures", 0);
   DBG_ASSERT(initialized_);
   ///////////////////////////////////////////////////////////
   // Get the vector/matrix spaces for the original problem //
   ///////////////////////////////////////////////////////////

   SmartPtr<const VectorSpace> orig_x_space;
   SmartPtr<const VectorSpace> orig_c_space;
   SmartPtr<const VectorSpace> orig_d_space;
   SmartPtr<const VectorSpace> orig_x_l_space;
   SmartPtr<const MatrixSpace> orig_px_l_space;
   SmartPtr<const VectorSpace> orig_x_u_space;
   SmartPtr<const MatrixSpace> orig_px_u_space;
   SmartPtr<const VectorSpace> orig_d_l_space;
   SmartPtr<const MatrixSpace> orig_pd_l_space;
   SmartPtr<const VectorSpace> orig_d_u_space;
   SmartPtr<const MatrixSpace> orig_pd_u_space;
   SmartPtr<const MatrixSpace> orig_jac_c_space;
   SmartPtr<const MatrixSpace> orig_jac_d_space;
   SmartPtr<const SymMatrixSpace> orig_h_space;

   orig_ip_nlp_->GetSpaces(orig_x_space, orig_c_space, orig_d_space, orig_x_l_space, orig_px_l_space, orig_x_u_space,
                           orig_px_u_space, orig_d_l_space, orig_pd_l_space, orig_d_u_space, orig_pd_u_space, orig_jac_c_space,
                           orig_jac_d_space, orig_h_space);

   // Create the restoration phase problem vector/matrix spaces, based
   // on the original spaces (pretty inconvenient with all the
   // matrix spaces, isn't it?!?)
   DBG_PRINT((1, "Creating the x_space_\n"));
   // vector x
   Index total_dim = orig_x_space->Dim() + 2 * orig_c_space->Dim() + 2 * orig_d_space->Dim();
   x_space_ = new CompoundVectorSpace(5, total_dim);
   x_space_->SetCompSpace(0, *orig_x_space);
   x_space_->SetCompSpace(1, *orig_c_space); // n_c
   x_space_->SetCompSpace(2, *orig_c_space); // p_c
   x_space_->SetCompSpace(3, *orig_d_space); // n_d
   x_space_->SetCompSpace(4, *orig_d_space); // p_d

   DBG_PRINT((1, "Setting the c_space_\n"));
   // vector c
   //c_space_ = orig_c_space;
   c_space_ = new CompoundVectorSpace(1, orig_c_space->Dim());
   c_space_->SetCompSpace(0, *orig_c_space);

   DBG_PRINT((1, "Setting the d_space_\n"));
   // vector d
   //d_space_ = orig_d_space;
   d_space_ = new CompoundVectorSpace(1, orig_d_space->Dim());
   d_space_->SetCompSpace(0, *orig_d_space);

   DBG_PRINT((1, "Creating the x_l_space_\n"));
   // vector x_L
   total_dim = orig_x_l_space->Dim() + 2 * orig_c_space->Dim() + 2 * orig_d_space->Dim();
   x_l_space_ = new CompoundVectorSpace(5, total_dim);
   x_l_space_->SetCompSpace(0, *orig_x_l_space);
   x_l_space_->SetCompSpace(1, *orig_c_space); // n_c >=0
   x_l_space_->SetCompSpace(2, *orig_c_space); // p_c >=0
   x_l_space_->SetCompSpace(3, *orig_d_space); // n_d >=0
   x_l_space_->SetCompSpace(4, *orig_d_space); // p_d >=0

   DBG_PRINT((1, "Setting the x_u_space_\n"));
   // vector x_U
   x_u_space_ = new CompoundVectorSpace(1, orig_x_u_space->Dim());
   x_u_space_->SetCompSpace(0, *orig_x_u_space);

   DBG_PRINT((1, "Creating the px_l_space_\n"));
   // matrix px_l
   Index total_rows = orig_x_space->Dim() + 2 * orig_c_space->Dim() + 2 * orig_d_space->Dim();
   Index total_cols = orig_x_l_space->Dim() + 2 * orig_c_space->Dim() + 2 * orig_d_space->Dim();
   px_l_space_ = new CompoundMatrixSpace(5, 5, total_rows, total_cols);
   px_l_space_->SetBlockRows(0, orig_x_space->Dim());
   px_l_space_->SetBlockRows(1, orig_c_space->Dim());
   px_l_space_->SetBlockRows(2, orig_c_space->Dim());
   px_l_space_->SetBlockRows(3, orig_d_space->Dim());
   px_l_space_->SetBlockRows(4, orig_d_space->Dim());
   px_l_space_->SetBlockCols(0, orig_x_l_space->Dim());
   px_l_space_->SetBlockCols(1, orig_c_space->Dim());
   px_l_space_->SetBlockCols(2, orig_c_space->Dim());
   px_l_space_->SetBlockCols(3, orig_d_space->Dim());
   px_l_space_->SetBlockCols(4, orig_d_space->Dim());

   px_l_space_->SetCompSpace(0, 0, *orig_px_l_space);
   // now setup the identity matrix
   // This could be changed to be something like...
   // px_l_space_->SetBlockToIdentity(1,1,1.0);
   // px_l_space_->SetBlockToIdentity(2,2,other_factor);
   // ... etc with some simple changes to the CompoundMatrixSpace
   // to allow this (space should auto create the matrices)
   //
   // for now, we use the new feature and set the true flag for this block
   // to say that the matrices should be auto_allocated
   SmartPtr<const MatrixSpace> identity_mat_space_nc = new IdentityMatrixSpace(orig_c_space->Dim());
   px_l_space_->SetCompSpace(1, 1, *identity_mat_space_nc, true);
   px_l_space_->SetCompSpace(2, 2, *identity_mat_space_nc, true);
   SmartPtr<const MatrixSpace> identity_mat_space_nd = new IdentityMatrixSpace(orig_d_space->Dim());
   px_l_space_->SetCompSpace(3, 3, *identity_mat_space_nd, true);
   px_l_space_->SetCompSpace(4, 4, *identity_mat_space_nd, true);

   DBG_PRINT((1, "Creating the px_u_space_\n"));
   // matrix px_u
   total_rows = orig_x_space->Dim() + 2 * orig_c_space->Dim() + 2 * orig_d_space->Dim();
   total_cols = orig_x_u_space->Dim();
   DBG_PRINT((1, "total_rows = %" IPOPT_INDEX_FORMAT ", total_cols = %" IPOPT_INDEX_FORMAT "\n", total_rows, total_cols));
   px_u_space_ = new CompoundMatrixSpace(5, 1, total_rows, total_cols);
   px_u_space_->SetBlockRows(0, orig_x_space->Dim());
   px_u_space_->SetBlockRows(1, orig_c_space->Dim());
   px_u_space_->SetBlockRows(2, orig_c_space->Dim());
   px_u_space_->SetBlockRows(3, orig_d_space->Dim());
   px_u_space_->SetBlockRows(4, orig_d_space->Dim());
   px_u_space_->SetBlockCols(0, orig_x_u_space->Dim());

   px_u_space_->SetCompSpace(0, 0, *orig_px_u_space);
   // other matrices are zero'ed out

   // vector d_L
   //d_l_space_ = orig_d_l_space;
   d_l_space_ = new CompoundVectorSpace(1, orig_d_l_space->Dim());
   d_l_space_->SetCompSpace(0, *orig_d_l_space);

   // vector d_U
   //d_u_space_ = orig_d_u_space;
   d_u_space_ = new CompoundVectorSpace(1, orig_d_u_space->Dim());
   d_u_space_->SetCompSpace(0, *orig_d_u_space);

   // matrix pd_L
   //pd_l_space_ = orig_pd_l_space;
   pd_l_space_ = new CompoundMatrixSpace(1, 1, orig_pd_l_space->NRows(), orig_pd_l_space->NCols());
   pd_l_space_->SetBlockRows(0, orig_pd_l_space->NRows());
   pd_l_space_->SetBlockCols(0, orig_pd_l_space->NCols());
   pd_l_space_->SetCompSpace(0, 0, *orig_pd_l_space);

   // matrix pd_U
   //pd_u_space_ = orig_pd_u_space;
   pd_u_space_ = new CompoundMatrixSpace(1, 1, orig_pd_u_space->NRows(), orig_pd_u_space->NCols());
   pd_u_space_->SetBlockRows(0, orig_pd_u_space->NRows());
   pd_u_space_->SetBlockCols(0, orig_pd_u_space->NCols());
   pd_u_space_->SetCompSpace(0, 0, *orig_pd_u_space);

   DBG_PRINT((1, "Creating the jac_c_space_\n"));
   // matrix jac_c
   total_rows = orig_c_space->Dim();
   total_cols = orig_x_space->Dim() + 2 * orig_c_space->Dim() + 2 * orig_d_space->Dim();
   jac_c_space_ = new CompoundMatrixSpace(1, 5, total_rows, total_cols);
   jac_c_space_->SetBlockRows(0, orig_c_space->Dim());
   jac_c_space_->SetBlockCols(0, orig_x_space->Dim());
   jac_c_space_->SetBlockCols(1, orig_c_space->Dim());
   jac_c_space_->SetBlockCols(2, orig_c_space->Dim());
   jac_c_space_->SetBlockCols(3, orig_d_space->Dim());
   jac_c_space_->SetBlockCols(4, orig_d_space->Dim());

   jac_c_space_->SetCompSpace(0, 0, *orig_jac_c_space);
   // **NOTE: By placing "flat" identity matrices here, we are creating
   //         potential issues for linalg operations that arise when the original
   //         NLP has a "compound" c_space. To avoid problems like this,
   //         we place all unmodified component spaces in trivial (size 1)
   //         "compound" spaces.
   jac_c_space_->SetCompSpace(0, 1, *identity_mat_space_nc, true);
   jac_c_space_->SetCompSpace(0, 2, *identity_mat_space_nc, true);
   // remaining blocks are zero'ed

   DBG_PRINT((1, "Creating the jac_d_space_\n"));
   // matrix jac_d
   total_rows = orig_d_space->Dim();
   total_cols = orig_x_space->Dim() + 2 * orig_c_space->Dim() + 2 * orig_d_space->Dim();
   jac_d_space_ = new CompoundMatrixSpace(1, 5, total_rows, total_cols);
   jac_d_space_->SetBlockRows(0, orig_d_space->Dim());
   jac_d_space_->SetBlockCols(0, orig_x_space->Dim());
   jac_d_space_->SetBlockCols(1, orig_c_space->Dim());
   jac_d_space_->SetBlockCols(2, orig_c_space->Dim());
   jac_d_space_->SetBlockCols(3, orig_d_space->Dim());
   jac_d_space_->SetBlockCols(4, orig_d_space->Dim());

   jac_d_space_->SetCompSpace(0, 0, *orig_jac_d_space);
   DBG_PRINT((1, "orig_jac_d_space = %p\n", GetRawPtr(orig_jac_d_space)))
   // Blocks (0,1) and (0,2) are zero'ed out
   // **NOTE: By placing "flat" identity matrices here, we are creating
   //         potential issues for linalg operations that arise when the original
   //         NLP has a "compound" d_space. To avoid problems like this,
   //         we place all unmodified component spaces in trivial (size 1)
   //         "compound" spaces.
   jac_d_space_->SetCompSpace(0, 3, *identity_mat_space_nd, true);
   jac_d_space_->SetCompSpace(0, 4, *identity_mat_space_nd, true);

   DBG_PRINT((1, "Creating the h_space_\n"));
   // matrix h
   total_dim = orig_x_space->Dim() + 2 * orig_c_space->Dim() + 2 * orig_d_space->Dim();
   h_space_ = new CompoundSymMatrixSpace(5, total_dim);
   h_space_->SetBlockDim(0, orig_x_space->Dim());
   h_space_->SetBlockDim(1, orig_c_space->Dim());
   h_space_->SetBlockDim(2, orig_c_space->Dim());
   h_space_->SetBlockDim(3, orig_d_space->Dim());
   h_space_->SetBlockDim(4, orig_d_space->Dim());

   SmartPtr<DiagMatrixSpace> DR2_x_space = new DiagMatrixSpace(orig_x_space->Dim());
   if( hessian_approximation_ == LIMITED_MEMORY )
   {
      const LowRankUpdateSymMatrixSpace* LR_h_space = static_cast<const LowRankUpdateSymMatrixSpace*>(GetRawPtr(
            orig_h_space));
      DBG_ASSERT(LR_h_space);
      SmartPtr<LowRankUpdateSymMatrixSpace> new_orig_h_space = new LowRankUpdateSymMatrixSpace(LR_h_space->Dim(),
         NULL, orig_x_space, false);
      h_space_->SetCompSpace(0, 0, *new_orig_h_space, true);
   }
   else
   {
      SmartPtr<SumSymMatrixSpace> sumsym_mat_space = new SumSymMatrixSpace(orig_x_space->Dim(), 2);
      sumsym_mat_space->SetTermSpace(0, *orig_h_space);
      sumsym_mat_space->SetTermSpace(1, *DR2_x_space);
      h_space_->SetCompSpace(0, 0, *sumsym_mat_space, true);
      // All remaining blocks are zero'ed out
   }

   ///////////////////////////
   // Create the bound data //
   ///////////////////////////

   // x_L
   x_L_ = x_l_space_->MakeNewCompoundVector();
   x_L_->SetComp(0, *orig_ip_nlp_->x_L()); // x >= x_L
   x_L_->GetCompNonConst(1)->Set(0.0); // n_c >= 0
   x_L_->GetCompNonConst(2)->Set(0.0); // p_c >= 0
   x_L_->GetCompNonConst(3)->Set(0.0); // n_d >= 0
   x_L_->GetCompNonConst(4)->Set(0.0); // p_d >= 0
   DBG_PRINT_VECTOR(2, "resto_x_L", *x_L_);

   // x_U
   x_U_ = x_u_space_->MakeNewCompoundVector();
   x_U_->SetComp(0, *orig_ip_nlp_->x_U());

   // d_L
   d_L_ = d_l_space_->MakeNewCompoundVector();
   d_L_->SetComp(0, *orig_ip_nlp_->d_L());

   // d_U
   d_U_ = d_u_space_->MakeNewCompoundVector();
   d_U_->SetComp(0, *orig_ip_nlp_->d_U());

   // Px_L
   Px_L_ = px_l_space_->MakeNewCompoundMatrix();
   Px_L_->SetComp(0, 0, *orig_ip_nlp_->Px_L());
   // Identities are auto-created (true flag passed into SetCompSpace)

   // Px_U
   Px_U_ = px_u_space_->MakeNewCompoundMatrix();
   Px_U_->SetComp(0, 0, *orig_ip_nlp_->Px_U());
   // Remaining matrices will be zero'ed out

   // Pd_L
   //Pd_L_ = orig_ip_nlp_->Pd_L();
   Pd_L_ = pd_l_space_->MakeNewCompoundMatrix();
   Pd_L_->SetComp(0, 0, *orig_ip_nlp_->Pd_L());

   // Pd_U
   //Pd_U_ = orig_ip_nlp_->Pd_U();
   Pd_U_ = pd_u_space_->MakeNewCompoundMatrix();
   Pd_U_->SetComp(0, 0, *orig_ip_nlp_->Pd_U());

   // Getting the NLP scaling

   SmartPtr<const MatrixSpace> scaled_jac_c_space;
   SmartPtr<const MatrixSpace> scaled_jac_d_space;
   SmartPtr<const SymMatrixSpace> scaled_h_space;
   NLP_scaling()->DetermineScaling(GetRawPtr(x_space_), c_space_, d_space_, GetRawPtr(jac_c_space_),
                                   GetRawPtr(jac_d_space_), GetRawPtr(h_space_), scaled_jac_c_space, scaled_jac_d_space, scaled_h_space, *Px_L_,
                                   *x_L_, *Px_U_, *x_U_);
   // For now we assume that no scaling is done inside the NLP_Scaling
   DBG_ASSERT(scaled_jac_c_space == jac_c_space_);
   DBG_ASSERT(scaled_jac_d_space == jac_d_space_);
   DBG_ASSERT(scaled_h_space == h_space_);

   /////////////////////////////////////////////////////////////////////////
   // Create and initialize the vectors for the restoration phase problem //
   /////////////////////////////////////////////////////////////////////////

   // Vector x
   SmartPtr<CompoundVector> comp_x = x_space_->MakeNewCompoundVector();
   if( init_x )
   {
      comp_x->GetCompNonConst(0)->Copy(*orig_ip_data_->curr()->x());
      comp_x->GetCompNonConst(1)->Set(1.0);
      comp_x->GetCompNonConst(2)->Set(1.0);
      comp_x->GetCompNonConst(3)->Set(1.0);
      comp_x->GetCompNonConst(4)->Set(1.0);
   }
   x = GetRawPtr(comp_x);

   // Vector y_c
   y_c = c_space_->MakeNew();
   if( init_y_c )
   {
      y_c->Set(0.0);  // ToDo
   }

   // Vector y_d
   y_d = d_space_->MakeNew();
   if( init_y_d )
   {
      y_d->Set(0.0);
   }

   // Vector z_L
   z_L = x_l_space_->MakeNew();
   if( init_z_L )
   {
      z_L->Set(1.0);
   }

   // Vector z_U
   z_U = x_u_space_->MakeNew();
   if( init_z_U )
   {
      z_U->Set(1.0);
   }

   // Vector v_L
   v_L = d_l_space_->MakeNew();

   // Vector v_U
   v_U = d_u_space_->MakeNew();

   // Initialize other data needed by the restoration nlp.  x_ref is
   // the point to reference to which we based the regularization
   // term
   SmartPtr<Vector> x_ref = orig_x_space->MakeNew();
   x_ref->Copy(*orig_ip_data_->curr()->x());
   x_ref_ = x_ref;

   SmartPtr<Vector> dr_x = orig_x_space->MakeNew();
   dr_x->Set(1.0);
   SmartPtr<Vector> tmp = dr_x->MakeNew();
   tmp->Copy(*x_ref_);
   dr_x->ElementWiseMax(*tmp);
   tmp->Scal(-1.);
   dr_x->ElementWiseMax(*tmp);
   dr_x->ElementWiseReciprocal();
   dr_x_ = dr_x;
   DBG_PRINT_VECTOR(2, "dr_x_", *dr_x_);

   // dr_x^2
   SmartPtr<Vector> dr2_x = dr_x->MakeNewCopy();
   dr2_x->ElementWiseMultiply(*dr_x_);
   dr2_x_ = dr2_x;

   SmartPtr<DiagMatrix> DR2_x = DR2_x_space->MakeNewDiagMatrix();
   DR2_x->SetDiag(*dr2_x_);
   DR2_x_ = DR2_x;

   return true;
}

Number RestoIpoptNLP::f(
   const Vector& /*x*/
)
{
   THROW_EXCEPTION(INTERNAL_ABORT, "ERROR: In RestoIpoptNLP f() is called without mu!");
   return 0.;
}

Number RestoIpoptNLP::f(
   const Vector& x,
   Number        mu
)
{
   DBG_START_METH("RestoIpoptNLP::f",
                  dbg_verbosity);
   Number ret = 0.0;
   // rho*(pcTe + ncTe + pdT*e + ndT*e) + eta/2*||Dr*(x-xr)||_2^2
   const CompoundVector* c_vec = static_cast<const CompoundVector*>(&x);
   DBG_ASSERT(c_vec);
   SmartPtr<const Vector> x_only = c_vec->GetComp(0);
   ret = x.Sum() - x_only->Sum();
   DBG_PRINT((1, "xdiff sum = %e\n", ret));
   ret = rho_ * ret;
   DBG_PRINT((1, "rho_ = %e\n", rho_));

   SmartPtr<Vector> x_diff = x_only->MakeNew();
   x_diff->Copy(*x_only);
   x_diff->Axpy(-1.0, *x_ref_);
   DBG_PRINT_VECTOR(2, "x_ref", *x_ref_);
   x_diff->ElementWiseMultiply(*dr_x_);
   Number ret2 = x_diff->Nrm2();
   DBG_PRINT((1, "Eta = %e\n", Eta(mu)));
   ret2 = Eta(mu) / 2.0 * ret2 * ret2;

   ret += ret2;

   // We evaluate also the objective function for the original
   // problem here.  This might be wasteful, but it will detect if
   // the original objective function cannot be evaluated at the
   // trial point in the restoration phase
   if( evaluate_orig_obj_at_resto_trial_ )
   {
      /* Number orig_f = */orig_ip_nlp_->f(*x_only);
   }

   return ret;
}

SmartPtr<const Vector> RestoIpoptNLP::grad_f(
   const Vector& x,
   Number        mu
)
{
   SmartPtr<Vector> retPtr = x.MakeNew();
   // Scale the p's and n's by rho (Scale all, take out the x part later)
   retPtr->Set(rho_);

   const CompoundVector* c_vec_in = static_cast<const CompoundVector*>(&x);
   SmartPtr<const Vector> x_only_in = c_vec_in->GetComp(0);

   CompoundVector* c_vec = static_cast<CompoundVector*>(GetRawPtr(retPtr));
   DBG_ASSERT(c_vec);
   SmartPtr<Vector> x_only = c_vec->GetCompNonConst(0);
   x_only->Copy(*x_only_in);
   x_only->Axpy(-1.0, *x_ref_);
   x_only->ElementWiseMultiply(*dr2_x_);
   x_only->Scal(Eta(mu));

   return ConstPtr(retPtr);
}

SmartPtr<const Vector> RestoIpoptNLP::c(
   const Vector& x
)
{
   const CompoundVector* c_vec = static_cast<const CompoundVector*>(&x);
   SmartPtr<const Vector> x_only = c_vec->GetComp(0);
   SmartPtr<const Vector> nc_only = c_vec->GetComp(1);
   SmartPtr<const Vector> pc_only = c_vec->GetComp(2);

   SmartPtr<const Vector> orig_c = orig_ip_nlp_->c(*x_only);
   SmartPtr<Vector> retPtr = c_space_->MakeNew();
   SmartPtr<CompoundVector> CretPtr = static_cast<CompoundVector*>(GetRawPtr(retPtr));
   DBG_ASSERT(dynamic_cast<CompoundVector*> (GetRawPtr(retPtr)));
   SmartPtr<Vector> CretPtr0 = CretPtr->GetCompNonConst(0);
   CretPtr0->Copy(*orig_c);
   CretPtr0->Axpy(1.0, *nc_only);
   CretPtr0->Axpy(-1.0, *pc_only);

   return GetRawPtr(retPtr);
}

SmartPtr<const Vector> RestoIpoptNLP::grad_f(
   const Vector& /*x*/
)
{
   THROW_EXCEPTION(INTERNAL_ABORT, "ERROR: In RestoIpoptNLP grad_f() is called without mu!");
}

SmartPtr<const Vector> RestoIpoptNLP::d(
   const Vector& x
)
{
   const CompoundVector* c_vec = static_cast<const CompoundVector*>(&x);
   SmartPtr<const Vector> x_only = c_vec->GetComp(0);
   SmartPtr<const Vector> nd_only = c_vec->GetComp(3);
   SmartPtr<const Vector> pd_only = c_vec->GetComp(4);

   SmartPtr<const Vector> orig_d = orig_ip_nlp_->d(*x_only);
   SmartPtr<Vector> retPtr = d_space_->MakeNew();
   SmartPtr<CompoundVector> CretPtr = static_cast<CompoundVector*>(GetRawPtr(retPtr));
   DBG_ASSERT(dynamic_cast<CompoundVector*> (GetRawPtr(retPtr)));
   SmartPtr<Vector> CretPtr0 = CretPtr->GetCompNonConst(0);
   CretPtr0->Copy(*orig_d);
   CretPtr0->Axpy(1., *nd_only);
   CretPtr0->Axpy(-1., *pd_only);

   return GetRawPtr(retPtr);
}

SmartPtr<const Matrix> RestoIpoptNLP::jac_c(
   const Vector& x
)
{

   // Here, we set the (0,0) block with the values from the
   // original jac_c and set the factor for the -I (jac w.r.t. p_c)

   // get out the x_only part
   const CompoundVector* c_vec = static_cast<const CompoundVector*>(&x);
   DBG_ASSERT(c_vec);
   SmartPtr<const Vector> x_only = c_vec->GetComp(0);

   // calculate the jacobian for the original problem
   SmartPtr<const Matrix> jac_c_only = orig_ip_nlp_->jac_c(*x_only);

   // Create the new compound matrix
   // The zero parts remain NULL, the identities are created from the matrix
   // space (since auto_allocate was set to true in SetCompSpace)
   SmartPtr<CompoundMatrix> retPtr = jac_c_space_->MakeNewCompoundMatrix();

   // set the (0,0) block to the original jacobian
   retPtr->SetComp(0, 0, *jac_c_only);

   // we currently do not have a default factor in the matrix spaces
   // so we need to set the factor on the identity (jacobian of the
   // restoration c w.r.t. p_c is -I)
   // This could easily be changed to include special processing
   // for identities in the CompoundMatrixSpace (and a factor)
   SmartPtr<Matrix> jac_c_pc_mat = retPtr->GetCompNonConst(0, 2);
   IdentityMatrix* jac_c_pc = static_cast<IdentityMatrix*>(GetRawPtr(jac_c_pc_mat));
   DBG_ASSERT(jac_c_pc);
   jac_c_pc->SetFactor(-1.0);

   return GetRawPtr(retPtr);
}

SmartPtr<const Matrix> RestoIpoptNLP::jac_d(
   const Vector& x
)
{
   DBG_START_METH("RestoIpoptNLP::jac_d", dbg_verbosity);

   // Here, we set the (0,0) block with the values from the
   // original jac_d and set the factor for the -I (jac w.r.t. p_d)

   // get out the x_only part
   const CompoundVector* c_vec = static_cast<const CompoundVector*>(&x);
   DBG_ASSERT(c_vec);
   SmartPtr<const Vector> x_only = c_vec->GetComp(0);

   // calculate the jacobian for the original problem
   SmartPtr<const Matrix> jac_d_only = orig_ip_nlp_->jac_d(*x_only);

   // Create the new compound matrix
   // The zero parts remain NULL, the identities are created from the matrix
   // space (since auto_allocate was set to true in SetCompSpace)
   SmartPtr<CompoundMatrix> retPtr = jac_d_space_->MakeNewCompoundMatrix();
   DBG_PRINT((1, "jac_d_space_ = %p\n", GetRawPtr(jac_d_space_)))

   // Set the block for the original Jacobian
   retPtr->SetComp(0, 0, *jac_d_only);

   // (0,1) and (0,2) blocks are zero (NULL)

   // set the factor for the identity matrix for the pd variables
   // (likr in jac_c)
   SmartPtr<Matrix> jac_d_pd_mat = retPtr->GetCompNonConst(0, 4);
   IdentityMatrix* jac_d_pd = static_cast<IdentityMatrix*>(GetRawPtr(jac_d_pd_mat));
   DBG_ASSERT(jac_d_pd);
   jac_d_pd->SetFactor(-1.0);

   return GetRawPtr(retPtr);
}

SmartPtr<const SymMatrix> RestoIpoptNLP::h(
   const Vector& /*x*/,
   Number        /*obj_factor*/,
   const Vector& /*yc*/,
   const Vector& /*yd*/
)
{
   assert(false && "ERROR: In RestoIpoptNLP h() is called without mu!");
   return NULL;
}

SmartPtr<const SymMatrix> RestoIpoptNLP::h(
   const Vector& x,
   Number        obj_factor,
   const Vector& yc,
   const Vector& yd,
   Number        mu
)
{
   // Here, we use a SumSymMatrix for the (0,0) block of the
   // Hessian. We need to set this to the hessian of the restoration
   // problem, which is the hessian of the objective from the restoration
   // problem + the constraint only part of the hessian from the original
   // problem
   // All other blocks are zero'ed (NULL)

   // get the x_only part
   const CompoundVector* c_vec = static_cast<const CompoundVector*>(&x);
   DBG_ASSERT(dynamic_cast<const CompoundVector*>(&x));
   SmartPtr<const Vector> x_only = c_vec->GetComp(0);

   // yc and yd should be trivial compound vectors
   const CompoundVector* Cyc = static_cast<const CompoundVector*>(&yc);
   DBG_ASSERT(dynamic_cast<const CompoundVector*>(&yc));
   DBG_ASSERT(Cyc->NComps() == 1);
   SmartPtr<const Vector> Cyc0 = Cyc->GetComp(0);
   const CompoundVector* Cyd = static_cast<const CompoundVector*>(&yd);
   DBG_ASSERT(dynamic_cast<const CompoundVector*>(&yd));
   DBG_ASSERT(Cyd->NComps() == 1);
   SmartPtr<const Vector> Cyd0 = Cyd->GetComp(0);

   // calculate the original hessian
   SmartPtr<const SymMatrix> h_con_orig = orig_ip_nlp_->h(*x_only, 0.0, *Cyc0, *Cyd0);

   // Create the new compound matrix
   // The SumSymMatrix is auto_allocated
   SmartPtr<CompoundSymMatrix> retPtr = h_space_->MakeNewCompoundSymMatrix();

   // Set the entries in the SumSymMatrix
   SmartPtr<Matrix> h_sum_mat = retPtr->GetCompNonConst(0, 0);
   SmartPtr<SumSymMatrix> h_sum = static_cast<SumSymMatrix*>(GetRawPtr(h_sum_mat));
   h_sum->SetTerm(0, 1.0, *h_con_orig);
   h_sum->SetTerm(1, obj_factor * Eta(mu), *DR2_x_);

   return GetRawPtr(retPtr);
}

SmartPtr<const SymMatrix> RestoIpoptNLP::uninitialized_h()
{
   SmartPtr<CompoundSymMatrix> retPtr;
   if( hessian_approximation_ == LIMITED_MEMORY )
   {
      retPtr = h_space_->MakeNewCompoundSymMatrix();
   }
   else
   {
      SmartPtr<const SymMatrix> h_con_orig = orig_ip_nlp_->uninitialized_h();
      retPtr = h_space_->MakeNewCompoundSymMatrix();
      SmartPtr<Matrix> h_sum_mat = retPtr->GetCompNonConst(0, 0);
      SmartPtr<SumSymMatrix> h_sum = static_cast<SumSymMatrix*>(GetRawPtr(h_sum_mat));
      h_sum->SetTerm(0, 1.0, *h_con_orig);
      h_sum->SetTerm(1, 1.0, *DR2_x_);
   }

   return GetRawPtr(retPtr);
}

void RestoIpoptNLP::GetSpaces(
   SmartPtr<const VectorSpace>&    x_space,
   SmartPtr<const VectorSpace>&    c_space,
   SmartPtr<const VectorSpace>&    d_space,
   SmartPtr<const VectorSpace>&    x_l_space,
   SmartPtr<const MatrixSpace>&    px_l_space,
   SmartPtr<const VectorSpace>&    x_u_space,
   SmartPtr<const MatrixSpace>&    px_u_space,
   SmartPtr<const VectorSpace>&    d_l_space,
   SmartPtr<const MatrixSpace>&    pd_l_space,
   SmartPtr<const VectorSpace>&    d_u_space,
   SmartPtr<const MatrixSpace>&    pd_u_space,
   SmartPtr<const MatrixSpace>&    Jac_c_space,
   SmartPtr<const MatrixSpace>&    Jac_d_space,
   SmartPtr<const SymMatrixSpace>& Hess_lagrangian_space
)
{
   x_space = GetRawPtr(x_space_);
   c_space = GetRawPtr(c_space_);
   d_space = GetRawPtr(d_space_);
   x_l_space = GetRawPtr(x_l_space_);
   px_l_space = GetRawPtr(px_l_space_);
   x_u_space = GetRawPtr(x_u_space_);
   px_u_space = GetRawPtr(px_u_space_);
   d_l_space = GetRawPtr(d_l_space_);
   pd_l_space = GetRawPtr(pd_l_space_);
   d_u_space = GetRawPtr(d_u_space_);
   pd_u_space = GetRawPtr(pd_u_space_);
   Jac_c_space = GetRawPtr(jac_c_space_);
   Jac_d_space = GetRawPtr(jac_d_space_);
   Hess_lagrangian_space = GetRawPtr(h_space_);
}

Number RestoIpoptNLP::Eta(
   Number mu
) const
{
   return eta_factor_ * std::pow(mu, eta_mu_exponent_);
}

void RestoIpoptNLP::AdjustVariableBounds(
   const Vector& new_x_L,
   const Vector& new_x_U,
   const Vector& new_d_L,
   const Vector& new_d_U
)
{

   const CompoundVector* comp_new_x_L = static_cast<const CompoundVector*>(&new_x_L);
   DBG_ASSERT(dynamic_cast<const CompoundVector*>(&new_x_L));
   SmartPtr<const Vector> new_orig_x_L = comp_new_x_L->GetComp(0);

   const CompoundVector* comp_new_x_U = static_cast<const CompoundVector*>(&new_x_U);
   DBG_ASSERT(dynamic_cast<const CompoundVector*>(&new_x_U));
   SmartPtr<const Vector> new_orig_x_U = comp_new_x_U->GetComp(0);

   const CompoundVector* comp_new_d_L = static_cast<const CompoundVector*>(&new_d_L);
   DBG_ASSERT(dynamic_cast<const CompoundVector*>(&new_d_L));
   SmartPtr<const Vector> new_orig_d_L = comp_new_d_L->GetComp(0);

   const CompoundVector* comp_new_d_U = static_cast<const CompoundVector*>(&new_d_U);
   DBG_ASSERT(dynamic_cast<const CompoundVector*>(&new_d_U));
   SmartPtr<const Vector> new_orig_d_U = comp_new_d_U->GetComp(0);

   // adapt bounds for the original NLP
   orig_ip_nlp_->AdjustVariableBounds(*new_orig_x_L, *new_orig_x_U, *new_orig_d_L, *new_orig_d_U);

   // adapt bounds for the p and n variables
   SmartPtr<const Vector> new_nc_L = comp_new_x_L->GetComp(1);
   SmartPtr<const Vector> new_pc_L = comp_new_x_L->GetComp(2);
   SmartPtr<const Vector> new_nd_L = comp_new_x_L->GetComp(3);
   SmartPtr<const Vector> new_pd_L = comp_new_x_L->GetComp(4);

   x_L_->GetCompNonConst(1)->Copy(*new_nc_L);
   x_L_->GetCompNonConst(2)->Copy(*new_pc_L);
   x_L_->GetCompNonConst(3)->Copy(*new_nd_L);
   x_L_->GetCompNonConst(4)->Copy(*new_pd_L);
}

bool RestoIpoptNLP::IntermediateCallBack(
   AlgorithmMode                       mode,
   Index                               iter,
   Number                              obj_value,
   Number                              inf_pr,
   Number                              inf_du,
   Number                              mu,
   Number                              d_norm,
   Number                              regularization_size,
   Number                              alpha_du,
   Number                              alpha_pr,
   Index                               ls_trials,
   SmartPtr<const IpoptData>           ip_data,
   SmartPtr<IpoptCalculatedQuantities> ip_cq
)
{
   return orig_ip_nlp_->IntermediateCallBack(mode, iter, obj_value, inf_pr, inf_du, mu, d_norm, regularization_size,
          alpha_du, alpha_pr, ls_trials, ip_data, ip_cq);
}

} // namespace Ipopt