diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index d4e487c2ea89045c53d7db8cac137034348922a3..8b17644d17fef89340df7775bde91a3252684025 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -310,19 +310,19 @@ GeneratorVelRef::computeTerm(MAL_VECTOR (&weightMV, double),
void
-GeneratorVelRef::addTerm(MAL_MATRIX (&Mat, double), double * target, int row, int col, int nrows, int ncols)
+GeneratorVelRef::addTerm(MAL_MATRIX (&Mat, double), double * QPMat, int row, int col, int nrows, int ncols)
{
- for(int i = 0;i < nrows; i++)
- for(int j = 0;j < ncols; j++)
- target[row+i+(col+j)*m_NbVariables] = Mat(i,j);
+ for(int i = row;i < row+nrows; i++)
+ for(int j = col;j < col+ncols; j++)
+ QPMat[i+j*m_NbVariables] = Mat(i,j);
}
void
-GeneratorVelRef::addTerm(MAL_VECTOR (&Vec, double), double * target, int index, int nelem)
+GeneratorVelRef::addTerm(MAL_VECTOR (&Vec, double), double * QPVec, int index, int nelem)
{
for(int i = index;i < nelem; i++)
- target[i] = Vec(i);
+ QPVec[i] = Vec(i);
}
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
index d7e5b0ffe540d514097dba48d8c44c9c953fe5a3..e75130e9d4d09ddd875145e774e5e18c2a888616 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
@@ -173,10 +173,10 @@ namespace PatternGeneratorJRL
const MAL_MATRIX (&M2, double), const MAL_VECTOR (&V2, double));
/// \brief Add the computed matrix to the final optimization problem in array form
- void addTerm(MAL_MATRIX (&Mat, double), double * target, int row, int col, int nrows, int ncols);
+ void addTerm(MAL_MATRIX (&Mat, double), double * QPMat, int row, int col, int nrows, int ncols);
/// \brief Add the computed vector to the final optimization problem in array form
- void addTerm(MAL_VECTOR (&Vec, double), double * target, int index, int nelem);
+ void addTerm(MAL_VECTOR (&Vec, double), double * QPVec, int index, int nelem);
/// \brief Build the constant part of the constraint matrices.
int buildConstantPartOfConstraintMatrices();
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
index 70d121005344229c410fd641be252dc06577bd36..d42b5af9c782b3ef6e8b9c00ffacedbed8309564 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
@@ -48,9 +48,7 @@ QPProblem_s::QPProblem_s():
U(0),war(0), iwar(0),
iout(0),ifail(0), iprint(0),
lwar(0), liwar(0),
- Eps(0),
- m_NbVariables(0), m_NbConstraints(0),
- m_ReallocMarginVar(0), m_ReallocMarginConstr(0)
+ Eps(0)
{
}
@@ -75,8 +73,8 @@ void QPProblem_s::ReleaseMemory()
if (XL!=0)
delete [] XL;
-
- if (XU!=0)
+
+ if (XU!=0)
delete [] XU;
if (X!=0)
@@ -87,7 +85,7 @@ void QPProblem_s::ReleaseMemory()
if (iwar!=0)
delete [] iwar;
-
+
if (war!=0)
delete [] war;
@@ -95,48 +93,37 @@ void QPProblem_s::ReleaseMemory()
delete [] U;
}
-
-void
-QPProblem_s::resizeAll(const int & NbVariables, const int & NbConstraints)
+void QPProblem_s::AllocateMemory()
{
+ war= new double[lwar];
+ iwar = new int[liwar]; // The Cholesky decomposition is done internally.
- resize(war,2*lwar,3*NbVariables*NbVariables/2+ 10*NbVariables + 2*(NbConstraints+1) + 20000);
- resize(iwar,2*liwar,2*NbVariables); // The Cholesky decomposition is done internally.
+ U = new double[mnn];
- resize(U,2*mnn,2*(NbConstraints+2*NbVariables));
+ DS = new double[(8*m_QP_N+1)*2*(m_QP_N+m_stepNumber)];
+ DU = new double[(8*m_QP_N+1)*2*(m_QP_N+m_stepNumber)];
- resize(DS,2*m,2*NbConstraints);
- resize(DU,2*m*n,2*NbVariables*NbConstraints);
- initialize(DS,2*NbConstraints);
- //initialize(DU,2*NbVariables*NbConstraints);
+ Q=new double[4*(m_QP_N+m_stepNumber)*(m_QP_N+m_stepNumber)]; //Quadratic part of the objective function
+ D=new double[2*(m_QP_N+m_stepNumber)]; // Linear part of the objective function
- resize(Q,2*n*n,2*NbVariables*NbVariables); //Quadratic part of the objective function
- resize(D,2*n,2*NbVariables); // Linear part of the objective function
- //initialize(Q,2*NbVariables*NbVariables);
- //initialize(D,2*NbVariables);
+ XL=new double[2*(m_QP_N+m_stepNumber)]; // Lower bound on the solution.
+ XU=new double[2*(m_QP_N+m_stepNumber)]; // Upper bound on the solution.
- resize(XL,2*n,2*NbVariables); // Lower bound on the solution.
- resize(XU,2*n,2*NbVariables); // Upper bound on the solution.
-
- resize(X,2*n,2*NbVariables); // Solution of the problem.
- resize(NewX,2*n,2*NbVariables); // Solution of the problem.
+ X=new double[2*(m_QP_N+m_stepNumber)]; // Solution of the problem.
+ NewX=new double[2*(m_QP_N+m_stepNumber)]; // Solution of the problem.
}
int
-QPProblem_s::resize(double *& array, const int & old_size, const int & new_size)
+QPProblem_s::resize(double * array, int size)
{
+ if (array!=0)
+ delete [] array;
try
{
- double * NewArray = new double[new_size];
- for(int i = 0; i < old_size; i++)
- NewArray[i] = array[i];
-
- if (array!=0)
- delete [] array;
- array = NewArray;
+ array = new double[size];
}
catch (std::bad_alloc& ba)
{
@@ -144,23 +131,18 @@ QPProblem_s::resize(double *& array, const int & old_size, const int & new_size)
}
return 0;
-
}
int
-QPProblem_s::resize(int *& array, const int & old_size, const int & new_size)
+QPProblem_s::resize(int * array, int size)
{
+ if (array!=0)
+ delete [] array;
try
{
- int * NewArray = new int[new_size];
- for(int i = 0; i < old_size; i++)
- NewArray[i] = array[i];
-
- if (array!=0)
- delete [] array;
- array = NewArray;
+ array = new int[size];
}
catch (std::bad_alloc& ba)
{
@@ -168,57 +150,59 @@ QPProblem_s::resize(int *& array, const int & old_size, const int & new_size)
}
return 0;
-
}
void
-QPProblem_s::setDimensions(const int & NbVariables,
- const int & NbConstraints,
- const int & NbEqConstraints)
+QPProblem_s::setNbVariables(int NbVariables)
{
+ m_NbVariables = NbVariables;
+}
+
+void QPProblem_s::setDimensions(int NbOfConstraints,
+ int NbOfEqConstraints,
+ int QP_N,
+ int StepNumber)
+{
+ bool reallocationNeeded = true;
+
// If all the dimensions are less than
// the current ones no need to reallocate.
- if (NbVariables > m_ReallocMarginVar)
- {
- m_ReallocMarginVar = 2*NbVariables;
- resizeAll(NbVariables, NbConstraints);
- }
- if (NbConstraints > m_ReallocMarginConstr)
+ // TODO: Should not be necessary to reallocate that often
+ if ((NbOfConstraints <= m) &&
+ (StepNumber <= m_stepNumber) &&
+ (QP_N <= m_QP_N))
+ reallocationNeeded = false;
+ m_stepNumber = StepNumber;
+ m_QP_N = QP_N;
+ m=NbOfConstraints;
+ me=NbOfEqConstraints;
+ mmax=m+1;
+ n=2*(m_QP_N+StepNumber);
+ nmax=n;
+ mnn=m+2*n;
+
+ iout=0;
+ iprint=1;
+ lwar=3*nmax*nmax/2+ 10*nmax + 2*mmax + 20000;
+ liwar=n;
+ Eps=1e-8;
+
+ if(reallocationNeeded)
{
- m_ReallocMarginConstr = 2*NbConstraints;
- resize(DS,2*m,2*NbVariables*NbConstraints);
- initialize(DS,2*NbVariables*NbConstraints);
- resize(DU,2*m*n,2*NbVariables*NbConstraints);
- initialize(DU,2*NbVariables*NbConstraints);
+ ReleaseMemory();
+ AllocateMemory();
}
-
- m = m_NbConstraints = NbConstraints;
- me = NbEqConstraints;
- mmax = m+1;
- n = m_NbVariables = NbVariables;
- nmax = n;
- mnn = m+2*n;
-
- iout = 0;
- iprint = 1;
- lwar = 3*nmax*nmax/2+ 10*nmax + 2*mmax + 20000;
- liwar = n;
- Eps = 1e-8;
-
}
-
-void
-QPProblem_s::initialize(double * array, const int & size)
+void QPProblem_s::initializeProblem()
{
- memset(array,0,size*sizeof(double));
-}
+ memset(DU,0,(8*m_QP_N+1)*2*(m_QP_N+m_stepNumber)*sizeof(double));
+}
-void
-QPProblem_s::solve(const int solver)
+void QPProblem_s::solve(int solver)
{
switch(solver)
{
@@ -230,49 +214,27 @@ QPProblem_s::solve(const int solver)
}
}
-
-void
-QPProblem_s::printSolverParameters(std::ostream & aos)
-{
- aos << "m: " << m << std::endl
- << "me: " << me << std::endl
- << "mmax: " << mmax << std::endl
- << "n: " << n << std::endl
- << "nmax: " << nmax << std::endl
- << "mnn: " << mnn << std::endl
- << "iout: " << iout << std::endl
- << "iprint: " << iprint << std::endl
- << "lwar: " << lwar << std::endl
- << "liwar: " << liwar << std::endl
- << "Eps: " << Eps << std::endl;
-}
-
-
-void
-QPProblem_s::dumpMatrix(std::ostream & aos,
- const int type)
+void QPProblem_s::dumpMatrix(std::ostream & aos,
+ int type)
{
int lnbrows=0, lnbcols=0;
double *aMatrix=0;
- std::string Name;
switch(type)
{
case MATRIX_Q:
- lnbrows = lnbcols = m_NbVariables ;
+ lnbrows = lnbcols = (m_QP_N+m_stepNumber)*2 ;
aMatrix = Q;
- Name = "Q";
break;
case MATRIX_DU:
- lnbrows = m;
- lnbcols = m_NbVariables;
+ lnbrows = m; // NbOfConstraints.
+ lnbcols = (m_QP_N+m_stepNumber)*2;
aMatrix = DU;
- Name = "DU";
break;
}
- aos << Name <<"["<<lnbrows<< ","<< lnbcols << "]" << std::endl;
+ aos << "["<<lnbcols << ","<< lnbrows << "]" << std::endl;
for(int i=0;i<lnbrows;i++)
{
@@ -280,58 +242,47 @@ QPProblem_s::dumpMatrix(std::ostream & aos,
aos << aMatrix[j*lnbrows+i] << " ";
aos << std::endl;
}
- aos << std::endl;
}
-
-void
-QPProblem_s::dumpVector(std::ostream & aos,
- const int type)
+void QPProblem_s::dumpVector(std::ostream & aos,
+ int type)
{
int lsize=0;
double *aVector=0;
- std::string Name;
switch(type)
{
case VECTOR_D:
- lsize=m_NbVariables ;
+ lsize=2*(m_QP_N+m_stepNumber) ;
aVector = D;
- Name = "D";
break;
case VECTOR_XL:
- lsize=m_NbVariables ;
+ lsize=2*(m_QP_N+m_stepNumber) ;
aVector = XL;
- Name = "XL";
break;
case VECTOR_XU:
- lsize=m_NbVariables;
+ lsize=2*(m_QP_N+m_stepNumber) ;
aVector = XU;
- Name = "XU";
break;
case VECTOR_DS:
lsize= m;
aVector = DS;
- Name = "DS";
break;
}
- aos << Name <<"["<< lsize << "]" << std::endl;
for(int i=0;i<lsize;i++)
{
aos << aVector[i] << " ";
}
- aos << std::endl << std::endl;
+ aos << std::endl;
}
-
-void
-QPProblem_s::dumpVector(const char * filename,
- const int type)
+void QPProblem_s::dumpVector(const char * filename,
+ int type)
{
std::ofstream aof;
aof.open(filename,std::ofstream::out);
@@ -340,9 +291,8 @@ QPProblem_s::dumpVector(const char * filename,
}
-void
-QPProblem_s::dumpMatrix(const char * filename,
- const int type)
+void QPProblem_s::dumpMatrix(const char * filename,
+ int type)
{
std::ofstream aof;
aof.open(filename,std::ofstream::out);
@@ -350,23 +300,19 @@ QPProblem_s::dumpMatrix(const char * filename,
aof.close();
}
-
-void
-QPProblem_s::dumpProblem(std::ostream &aos)
+void QPProblem_s::dumpProblem(std::ostream &aos)
{
dumpMatrix(aos,MATRIX_Q);
dumpMatrix(aos,MATRIX_DU);
dumpVector(aos,VECTOR_D);
+ dumpVector(aos,VECTOR_DL);
dumpVector(aos,VECTOR_XL);
dumpVector(aos,VECTOR_XU);
dumpVector(aos,VECTOR_DS);
- printSolverParameters(aos);
-}
-
-void
-QPProblem_s::dumpProblem(const char * filename)
+}
+void QPProblem_s::dumpProblem(const char * filename)
{
std::ofstream aof;
aof.open(filename,std::ofstream::out);
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.hh b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
index 8b452d70fb311cce2f7da282169d6838cba5a39d..db1936f7362868583c0dcc2471273fd26e2464a2 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.hh
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
@@ -50,48 +50,48 @@ namespace PatternGeneratorJRL
/// \brief Release the memory at the end only.
~QPProblem_s();
- /// \brief Set the number of optimization parameters.
- inline void setNbVariables(const int & NbVariables)
- { m_NbVariables = NbVariables;};
+ /// \brief Set the number of optimization variables
+ void setNbVariables(int NbVariables);
/// \brief Set the dimensions of the problem.
/// This method has an internal logic to
/// allocate the memory. It is done only
/// when the problem gets bigger. When it shrinks
/// down the memory is kept to avoid overhead.
- void setDimensions(const int & NbVariables,
- const int & NbConstraints,
- const int & NbEqConstraints);
+ void setDimensions(int NbOfConstraints,
+ int NbOfEqConstraints,
+ int StepNumber,
+ int QP_N);
/// \brief Reallocate array
- int resize(double * &array, const int & old_size, const int & new_size);
- int resize(int * &array, const int & old_size, const int & new_size);
-
- void printSolverParameters(std::ostream & aos);
+ int resize(double * array, int size);
+ int resize(int * array, int size);
/// \brief Dump on disk a problem.
void dumpProblem(const char *filename);
void dumpProblem(std::ostream &);
/// \brief Dump on disk a matrix defined by type.
- void dumpMatrix(const char *filename, const int type);
- void dumpMatrix(std::ostream &, const int type);
+ void dumpMatrix(const char *filename,int type);
+ void dumpMatrix(std::ostream &, int type);
/// \brief Dump on disk a vector defined by type.
- void dumpVector(const char *filename, const int type);
- void dumpVector(std::ostream &, const int type);
+ void dumpVector(const char *filename,int type);
+ void dumpVector(std::ostream &,int type);
- void initialize(double * array, const int & size);
+ /// \brief Initialize the problem
+ void initializeProblem();
/// \brief Solve the problem
- void solve(const int solver);
+ void solve(int solver);
const static int MATRIX_Q=0;
const static int MATRIX_DU=1;
const static int VECTOR_D=2;
- const static int VECTOR_DS=3;
+ const static int VECTOR_DL=3;
const static int VECTOR_XL=4;
const static int VECTOR_XU=5;
+ const static int VECTOR_DS=6;
const static int QLD=7;
const static int PLDP=8;
@@ -104,18 +104,17 @@ namespace PatternGeneratorJRL
/// The method allocating the memory.
/// Called when setting the dimensions of the problem.
- void resizeAll(const int & NbVariables, const int & NbConstraints);
+ void AllocateMemory();
private:
-
- /// \brief Number of optimization parameters
+ /// Previous set of step number considered.
int m_NbVariables;
- /// \brief Number of optimization parameters
- int m_NbConstraints;
+ /// Previous set of step number considered.
+ int m_stepNumber;
- /// \brief Reallocation margins
- int m_ReallocMarginVar, m_ReallocMarginConstr;
+ /// Previous size of the preview.
+ int m_QP_N;
};
typedef struct QPProblem_s QPProblem;