// Copyright (C) 2005, 2006 International Business Machines and others.
// All Rights Reserved.
// This code is published under the Eclipse Public License.
//
// $Id: hs071_nlp.cpp 1864 2010-12-22 19:21:02Z andreasw $
//
// Authors: Carl Laird, Andreas Waechter IBM 2005-08-16
#include "oc_p2p.hpp"
// for printf
#ifdef HAVE_CSTDIO
# include <cstdio>
#else
# ifdef HAVE_STDIO_H
# include <stdio.h>
# else
# error "don't have header file for stdio"
# endif
#endif
using namespace Ipopt;
const double OCP_P2P::INF;
const double OCP_P2P::a1, OCP_P2P::a2, OCP_P2P::a3;
const double OCP_P2P::b1, OCP_P2P::b2, OCP_P2P::b3;
const double OCP_P2P::kp;
// constructor
OCP_P2P::OCP_P2P()
{
numPoints = 0;//15;
xmin = new double[6];
xmax = new double[6];
umin = new double[2];
umax = new double[2];
x0 = new double[6];
xf = new double[6];
xmin[0] = -0.1;
xmin[1] = -2e19;
xmin[2] = -0.45;
xmin[3] = -2e19;
xmin[4] = -1.0;
xmin[5] = -2e19;
xmax[0] = 10.0;
xmax[1] = 2e19;
xmax[2] = 0.2;
xmax[3] = 2e19;
xmax[4] = 1.0;
xmax[5] = 2e19;
umin[0] = umin[1] = 1.0;
umax[0] = umax[1] = 10.0;
tmin = 0.5;
tmax = 50.0;
x0[0] = 0.0;
x0[1] = x0[2] = x0[3] = x0[4] = x0[5] = 0.0;
//x0[2] = -0.4;
xf[0] = 3.0;
xf[1] = xf[2] = xf[3] = xf[4] = xf[5] = 0.0;
x0[2] = xf[2] = -0.4;
x1b1 = 0.5;
x1b2 = 2.50;
x3bl = -0.5;
x3bu = 0.0;
tfg = 0.5;
tapeO = 1;
tapeG = 2;
tapeL = 3;
nP = 3;
nS = 4;
nB = 2*8;
// Das Problem hat P + P*B Variablen: tf[1..P] sowie a[1..P][1..B]
sizeN = nP + nP*nB;
// Nebenbedingungen:
// Gleichheitsbedingen: 6 AB, 6 EB, (P-1)*2*S Stetigkeitsbedingen
sizeMEq = 12 + 2*(nP-1)*nS; // 28
// Ungleichheitsbedingungen: 4 Ueberflugbed, 4*P*numPoints Eingangsbeschraenk.
sizeMIEq = 4 + 4*nP*numPoints;
sizeM = sizeMEq + sizeMIEq;
rindJacG = NULL;
cindJacG = NULL;
valsJacG = NULL;
rindHess = NULL;
cindHess = NULL;
valsHess = NULL;
xeTot = new double [sizeN + sizeM + 1];
}
//constructor with mesh
OCP_P2P::OCP_P2P(int points)
{
OCP_P2P();
this->numPoints = points;
}
//destructor
OCP_P2P::~OCP_P2P()
{
free(iRowJacG);
free(iColJacG);
free(valsJacG);
free(rindHess);
free(cindHess);
free(valsHess);
delete [] iRowHess;
delete [] iColHess;
delete [] hessElements;
delete [] xeTot;
delete [] xf;
delete [] x0;
delete [] umax;
delete [] umin;
delete [] xmax;
delete [] xmin;
}
// Liefert die Problemspezifikation
bool OCP_P2P::get_nlp_info(Index& n, Index& m, Index& nnz_jac_g,
Index& nnz_h_lag, IndexStyleEnum& index_style)
{
n = sizeN;
// n = 4;
m = sizeM;
// m = 2;
prepare(sizeN, sizeM);
// in this example the jacobian is dense and contains 8 nonzeros
nnz_jac_g = nnzJacG;
// the hessian is also dense and has 16 total nonzeros, but we
// only need the lower left corner (since it is symmetric)
nnz_h_lag = nnzHess;
// use the C style indexing (0-based)
index_style = TNLP::C_STYLE;
return true;
}
// returns the variable bounds
bool OCP_P2P::get_bounds_info(Index n, Number* x_l, Number* x_u,
Index m, Number* g_l, Number* g_u)
{
// here, the n and m we gave IPOPT in get_nlp_info are passed back to us.
// If desired, we could assert to make sure they are what we think they are.
assert(n == sizeN);
assert(m == sizeM);
// Die Zeit auf ein sinnvolles Maß beschränken
for(Index i=0; i<nP; i++){
x_l[i] = tmin;
x_u[i] = tmax;
}
// Die Parameter der Polynome snd frei wählbar
for(Index i=nP; i<nP+nP*nB; i++){
x_l[i] = -INF;
x_u[i] = INF;
}
for(int i=0; i<sizeMEq; i++){
g_l[i] = g_u[i] = 0;
}
for(int i=0; i<sizeMIEq; i++){
g_l[sizeMEq + i] = 0.0;
g_u[sizeMEq + i] = INF;
}
return true;
}
// returns the initial point for the problem
bool OCP_P2P::get_starting_point(Index n, bool init_x, Number* x,
bool init_z, Number* z_L, Number* z_U,
Index m, bool init_lambda,
Number* lambda)
{
// Here, we assume we only have starting values for x, if you code
// your own NLP, you can provide starting values for the dual variables
// if you wish
assert(init_x == true);
assert(init_z == false);
assert(init_lambda == false);
// Erste Schaetzung fuer Transistionszeiten
for(Index i=0; i<nP; i++)
x[i] = tfg;
// Polynomparameter mit 0 initialisieren
for(Index i=nP; i<nP+nP*nB; i++){
x[i] = 0.0;
}
return true;
}
// returns the value of the objective function
bool OCP_P2P::eval_f(Index n, const Number* x, bool new_x, Number& obj_v)
{
assert(n == sizeN);
this -> getObjective(n, x, new_x, obj_v);
return true;
}
// return the gradient of the objective function grad_{x} f(x)
bool OCP_P2P::eval_grad_f(Index n, const Number* x, bool new_x, Number* grad_f)
{
assert(n == sizeN);
int ret = gradient(tapeO, n, x, grad_f);
if(ret < 0){
printf("Problem bei ADOLC in eval_grad_f\n");
generateOTape(n, x);
//return false;
}
return true;
}
// return the value of the constraints: g(x)
bool OCP_P2P::eval_g(Index n, const Number* x, bool new_x, Index m, Number* g)
{
assert(n == sizeN);
assert(m == sizeM);
getConstraints(x, new_x, g);
return true;
}
// return the structure or values of the jacobian
bool OCP_P2P::eval_jac_g(Index n, const Number* x, bool new_x,
Index m, Index nele_jac, Index* iRow, Index *jCol,
Number* values)
{
assert(n == sizeN);
assert(m == sizeM);
if (values == NULL) {
// return the structure of the jacobian
assert(nnzJacG == nele_jac);
for(int i=0; i<nnzJacG; i++){
iRow[i] = iRowJacG[i];
jCol[i] = iColJacG[i];
}
}
else {
// return the values of the jacobian of the constraints
int nnz = nnzJacG;
int options[4];
options[0] = 0;
options[1] = 0;
options[2] = 0; //TODO
options[3] = 0;
int ret = sparse_jac(tapeG, m, n, 1, x, &nnz, &rindJacG, &cindJacG, &valsJacG, options);
if(ret < 0){
printf("Problem bei ADOLC in jac_g: %i\n", ret);
generateTapes(n, x, m);
//return false;
}
// else
// printf("jac_g liefert %i\n", ret);
assert(nnz == nele_jac);
assert(nnz == nnzJacG);
for(int i=0; i<nnz; i++){
values[i] = valsJacG[i];
}
}
return true;
}
//return the structure or values of the hessian
bool OCP_P2P::eval_h(Index n, const Number* x, bool new_x,
Number obj_factor, Index m, const Number* lambda,
bool new_lambda, Index nele_hess, Index* iRow,
Index* jCol, Number* values)
{
if (values == NULL) {
// return the structure. This is a symmetric matrix, fill the lower left
// triangle only.
assert(nnzHess == nele_hess);
for(int i=0; i<nnzHess; i++){
// Just copy the values of the pattern
iRow[i] = iRowHess[i];
jCol[i] = iColHess[i];
}
}
else {
// return the values. This is a symmetric matrix, fill the lower left
// triangle only
int nnz = nnzHessTotal;
int options[2];
options[0] = 0;
options[1] = 1;
// create enlarged vector of variables : xe' = [x, lambda, sigma]
double *xe = xeTot;
for(int i=0; i<n; i++)
xe[i] = x[i];
for(int i=0; i<m; i++)
xe[n+i] = lambda[i];
xe[n+m] = obj_factor;
int ret = sparse_hess(tapeL, n+m+1, 1, xe, &nnz, &rindHess, &cindHess, &valsHess, options);
if(ret < 0){
printf("Problem bei ADOLC in eval_h\n");
generateTapes(n,x,m);
}
int cnt = 0;
for(int i=0; i<nnz; i++){
if(cindHess[i] < n && cindHess[i] >= rindHess[i]){
// lower left triangular matrix of (reduced) hessian
values[cnt] = valsHess[i];
cnt ++;
}
}
}
return true;
}
void OCP_P2P::finalize_solution(SolverReturn status,
Index n, const Number* x, const Number* z_L, const Number* z_U,
Index m, const Number* g, const Number* lambda,
Number obj_value,
const IpoptData* ip_data,
IpoptCalculatedQuantities* ip_cq)
{
printf("Lösung der Optimierung:\n\n");
printf("tf ");
for(Index i=0; i<nP; i++){
printf("%10.4f ", x[i]);
}
printf("\n");
for(Index z=0; z<nB; z++){
printf("a%2i ",z);
for(Index i=0; i<nP; i++){
printf("%10.4f ", x[nP+i*nB+z]);
}
printf("\n");
}
}
void OCP_P2P::prepare(Index const n, Index const m)
{
double x[n], zl[m], zu[m], lambda[m];
get_starting_point(n, true, x, false, zl, zu, m, false, lambda);
generateTapes(n,x,m);
generatePatterns(n,x,m);
}
void OCP_P2P::generatePatterns(Index const n, Number* const x, Index const m)
{
generateJacGPattern(n,x,m);
generateHesPattern(n,x,m);
}
void OCP_P2P::generateJacGPattern(Index const n, Number* const x, Index const m)
{
// Pattern der Jacobbimatrix der NB erstellen
// Variablen dekl.
int options[4], nnz;
// Pattern bestimmen
options[0] = 0;
options[1] = 0;
options[2] = 0;
options[3] = 0;
sparse_jac(tapeG, m, n, 0, x, &nnz, &rindJacG, &cindJacG, &valsJacG, options);
// # NZ Elemente in Jac G bestimmen und Speicher reservieren
nnzJacG = nnz;
iRowJacG = rindJacG;
iColJacG = cindJacG;
// Aufräumen
}
void OCP_P2P::generateHesPattern(Index const n, Number* const x, Index const m)
{
// Create pattern of hessian
//Var dekl.
// double xe[n+m+1];
double *xe = xeTot;
int nnz;
// Generate enlarged vector of variables
int options[] = {0,1};
for(int i=0; i<n; i++){
xe[i] = x[i];
}
for(int i=n; i<n+m+1; i++)
xe[i] = 1.0; // set lambda=sigma=1 as we are only generating the pattern
sparse_hess(tapeL, n+m+1, 0, xe, &nnzHessTotal, &rindHess, &cindHess, &valsHess, options);
// calculate # nonzeros and prepare memory
// Only the upper left n*n bock of hessian is interesting to us/ipopt
nnzHess = 0; // Init counter
unsigned int *tmpElements = new unsigned int[nnzHessTotal];
for(int i=0; i<nnzHessTotal; i++){
if(cindHess[i] < n && cindHess[i] >= rindHess[i]){
// left triangular matrix of n*n block matrix
tmpElements[nnzHess] = i; // save the current index in tmpElements
nnzHess ++; // increase counter
}
}
// allocate enough memory
iRowHess = new unsigned int [nnzHess];
iColHess = new unsigned int [nnzHess];
hessElements = new unsigned int[nnzHess];
// reorder and save
for(int i=0; i<nnzHess; i++){ // for all interesting nonzeros
hessElements[i] = tmpElements[i]; // save number in the series of nonzeros returned by sparse_hess
iRowHess[i] = rindHess[tmpElements[i]]; // save rows and cols into class members for use in eval_h
iColHess[i] = cindHess[tmpElements[i]];
}
// tidy up
delete [] tmpElements;
}
void OCP_P2P::generateTapes(const Index n, const Number* x, const Index m)
{
adouble *g = new adouble[m];
generateOTape(n,x);
generateGTape(n,x,m,g);
generateLTape(n,x,m,g);
delete [] g;
}
void OCP_P2P::generateOTape(Index const n, const Number* const x)
{
// Definition des G-Tapes
trace_on(tapeO);
// Aktive unabh. Variablen dekl.
adouble *ax = new adouble[n];
adouble g;
for(Index i=0; i<n; i++){
ax[i] <<= x[i];
}
// Aktive abh. Vaiablen dekl und NB berechnen
this -> getObjective(n, ax, false, g);
// Aktive abh. Var. auslesen
double dummy;
g >>= dummy;
delete [] ax;
trace_off();
}
void OCP_P2P::generateGTape(Index const n, const Number* const x, Index const m, adouble *ag)
{
// Definition des G-Tapes
trace_on(tapeG);
// Aktive unabh. Variablen dekl.
adouble *ax = new adouble [n];
for(Index i=0; i<n; i++){
ax[i] <<= x[i];
}
// Aktive abh. Vaiablen dekl und NB berechnen
this->getConstraints(ax, false, ag);
// Aktive abh. Var. auslesen
double dummy;
for(Index i=0; i<m; i++){
ag[i] >>= dummy;
}
delete [] ax;
trace_off();
}
void OCP_P2P::generateLTape(Index const n, const Number* const x, Index const m, adouble *g)
{
trace_on(tapeL);
// Unabh. Var. dekl. und initiieren
adouble *ax = new adouble[n], *alambda = new adouble[m], asigma ;
for(int i=0; i<n ;i++)
ax[i] <<= x[i];
for(int i=0; i<m; i++)
alambda[i] <<= 1.0;
asigma <<= 1.0;
// NB berechnen
getConstraints(ax,false,g);
// Lagrange-Fct. Aufstellen
adouble l, obj;
getObjective(n, ax, false, obj);
l = asigma * obj;
for(int i=0; i<m; i++){
l += alambda[i] * g[i];
}
//Abh. Var. extrahieren
double dummy;
l >>= dummy;
// Aufräumen
delete [] ax;
delete [] alambda;
trace_off();
}
template<class T> void OCP_P2P::getConstraints(T const *xe, bool const new_e, T *g)
{
getEqConstraints(xe, new_e, g);
getIeqConstraints(xe, new_e, g+sizeMEq);
}
template<class T> void OCP_P2P::getEqConstraints(T const *xe, bool const, T *g)
{
Index idx = 0;
T * x = new T [6], *u1 = new T, *u2 = new T;
getParam(xe+nP, T(0.0), x, u1,u2);
// AB kodieren (6NB)
for(Index i=0; i<6; i++){
// AB
g[i] = x0[i] - x[i];
}
idx += 6;
getParam(xe+nP+(nP-1)*nB, xe[2], x, u1,u2);
// EB kodieren (6NB)
for(Index i=0; i<6; i++){
// AB
g[idx + i] = xf[i] - x[i];
}
idx += 6;
for(Index j=0; j<nP-1; j++){ // Teilabschnitt
for(Index k=0; k<nS; k++){ // Ableitung an Stelle
T const *par = xe + nP + nB*j;
g[idx + 2*nS*j + 2*k] = getPoly(par, xe[j], k) - getPoly(par+nB, T(0.0), k);
g[idx + 2*nS*j + 2*k + 1] = getPoly(par + nB/2, xe[j], k) - getPoly(par+nB*3/2, T(0.0), k);
}
}
idx += 2*(nP-1)*nS; //(16NB)
delete u2;
delete u1;
delete [] x;
}
template<class T> void OCP_P2P::getIeqConstraints(T const *xe, bool const, T *g)
{
Index idx = 0;
T *dt = new T [nP];
T *x = new T [6];
T *u1 = new T, *u2 = new T;
for(Index i=0; i<nP; i++){
dt[i] = xe[i] / (numPoints-1);
}
// TODO NB Ueberflug-Bedingungen allgemein aufstellen
getParam(xe + nP + nB, T(0), x, u1, u2);
g[idx] = x[0] - x1b1;
g[idx + 1] = x[2] - x3bu;
getParam(xe + nP + (nP-1)*nB, T(0), x, u1, u2);
g[idx + 2] = x1b2 - x[0];
g[idx + 3] = x[2] - x3bu;
idx += 4;
// Grenzen der Eingaenge (12*numPoints NB)
for(Index i=0; i<numPoints; i++){
for(Index j=0; j<nP; j++){
getParam(xe + nP + j*nB, T(dt[j] * i), x, u1, u2); // 1. Teilstueck
g[idx] = *u1 - umin[0];
g[idx + 1] = *u2 - umin[1];
g[idx + 2] = umax[0] - *u1;
g[idx + 3] = umax[1] - *u2;
idx += 4;
}
}
delete u2;
delete u1;
delete [] x;
delete [] dt;
}
template<class T> void OCP_P2P::getObjective(const Index n, T const *x_e, bool const new_x, T &obj)
{
obj = 0;
for(Index i=0; i<nP; i++)
obj += x_e[i];
}
template<class T> T OCP_P2P::getPoly(T const *par, T const t, int const der)
{
T val = 0.0;
for(Index i=der; i<nB/2; i++){ // TODO 7 oder 8?
T part;
if(i == der)
part = par[i];
else
part = par[i] * pow(t,(i-der));
for(Index j=i; j>i-der; j--)
part *= j;
val += part;
}
return val;
}
template<class T> void OCP_P2P::getParam(T const *params, T const t, T *x, T *u1, T *u2)
{
T const *par_y1 = params;
T const *par_y2 = params + nB/2;
T const x1 = getPoly(par_y1,t,0), x2 = getPoly(par_y1,t,1);
T const x3 = getPoly(par_y2,t,0), x4 = getPoly(par_y2,t,1);
T const x5 = atan((b2*getPoly(par_y1,t,2))/(b1*cos(x3)*(getPoly(par_y2,t,2)-a1*sin(x3)-a2*cos(x3))));
*u1 = -(getPoly(par_y2,t,2)-getPoly(par_y1,t,2)-a1*sin(x3)-a2*cos(x3))/(b1*cos(x3)*sin(x5)-b2*cos(x5));
T const d1u1 = (b1*cos(x3)*cos(x5)*(getPoly(par_y2,t,3))+b2*sin(x5)* (getPoly(par_y1,t,3))+b1*b2*(*u1)*sin(x3)*x4*pow(sin(x5),2)+(a2*b1*cos(x3)*sin(x3)-a1*b1*pow(cos(x3),2))* x4*cos(x5))/(b1*b2*cos(x3));
T const x6 = -(getPoly(par_y2,t,3)+getPoly(par_y1,t,3)+(b1**u1*sin(x3)*x4-b1*(d1u1)*cos(x3))*sin(x5)-b2*(d1u1)*cos(x5)+(a2*sin(x3)-a1*cos(x3))*x4)/(b2**u1*sin(x5)-b1**u1*cos(x3)*cos(x5));
*u2 = -(b1*cos(x3)*sin(x5)*(getPoly(par_y2,t,4))-b2*cos(x5)*(getPoly(par_y1,t,4))+(2*b1*b2*(d1u1)*cos(x3)-2*b1*b2**u1*sin(x3)*x4*pow(cos(x5),2))*x6+(-b1*b2*b2*pow(*u1,2)*sin(x3)*pow(cos(x5),2)+(-b1*b2**u1*cos(x3)*pow(x4,2)-2*b1*b2*(d1u1)*sin(x3)*x4-a1*b1*b2**u1)*cos(x5)+(a1*b1*cos(x3)*sin(x3)+a2*b1*pow(cos(x3),2))*pow(x4,2)+(a2*a2-a1*a1)*b1*pow(cos(x3),2)*sin(x3)-2*a1*a2*b1*pow(cos(x3),3)+a3*b1*b2**u1*pow(cos(x3),2)+a1*a2*b1*cos(x3))*sin(x5))/(b1*b2*b3**u1*cos(x3));
x[0] = x1; x[1] = x2; x[2] = x3; x[3] = x4; x[4] = x5; x[5] = x6;
}