diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
index 10a7a4c47244df2e7b7d9d1fb6bcdc3a19522a30..8d0a738d4e523b02c7fbb418b4b54f54324f7122 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
@@ -120,28 +120,84 @@ QPProblem_s::resizeAll(const int & NbVariables, const int & NbConstraints)
}
-template <class type>
-int
-QPProblem_s::resize(type *& array, const int & old_size, const int & new_size)
+void QPProblem_s::clear(const int type)
{
- try
+ double * array;
+ int array_size;
+ switch(type)
{
- type * NewArray = new type[new_size];
- initialize(NewArray, new_size, (type)0);
- for(int i = 0; i < old_size; i++)
- NewArray[i] = array[i];
-
- if (array!=0)
- delete [] array;
- array = NewArray;
+ case MATRIX_Q:
+ array = Q;
+ array_size = n*n;
+ break;
+ case MATRIX_DU:
+ array = DU;
+ array_size = mmax*n;
+ break;
+ case VECTOR_D:
+ array = D;
+ array_size = n;
+ break;
+ case VECTOR_DS:
+ array = DS;
+ array_size = mmax;
+ break;
+ case VECTOR_XL:
+ array = XL;
+ array_size = n;
+ break;
+ case VECTOR_XU:
+ array = XU;
+ array_size = n;
+ break;
}
- catch (std::bad_alloc& ba)
+
+ std::fill_n(array,array_size,0);
+
+}
+
+
+void QPProblem_s::clear(const int type,
+ const int & row, const int & col,
+ const int & nb_rows, const int & nb_cols)
+{
+
+ double * array;
+ int
+ max_rows, max_cols,
+ n_rows,n_cols;
+
+ switch(type)
{
- std::cerr << "bad_alloc caught: " << ba.what() << std::endl;
+ case MATRIX_Q:
+ array = Q;
+ max_rows = n_rows = n;
+ max_cols = n;
+ break;
+
+ case MATRIX_DU:
+ array = DU;
+ max_rows = m;
+ max_cols = n;
+ n_rows = mmax;
+ n_cols = n;
+ break;
+ }
+
+ if(row + nb_rows > max_rows || col + nb_cols > max_cols)
+ {
+ //throw sth. like:
+ std::cout<<"Matrix "<<type<<" bounds violated in clear(): "<<std::endl<<
+ "max_rows: "<<max_rows<<std::endl<<
+ "max_cols: "<<max_cols<<std::endl<<
+ "req. cols: "<<row + nb_rows<<
+ "req. rows: "<<col + nb_cols<<std::endl;
}
- return 0;
+ for(unsigned int i = row;i < row+nb_rows; i++)
+ for(unsigned int j = col;j < nb_cols; j++)
+ array[row+i+(col+j)*n_rows] = 0.0;
}
@@ -182,14 +238,6 @@ QPProblem_s::setDimensions(const int & NbVariables,
}
-template <class type>
-void
-QPProblem_s::initialize(type * array, const int & size, type value)
-{
- std::fill_n(array,size,value);
-}
-
-
void
QPProblem_s::solve(const int solver)
{
@@ -205,48 +253,57 @@ QPProblem_s::solve(const int solver)
void
-QPProblem_s::addTerm(const MAL_MATRIX (&Mat, double), const int target,
+QPProblem_s::addTerm(const MAL_MATRIX (&Mat, double), const int type,
const int row, const int col)
{
- double * aArray;
- int max_rows, max_cols;
+ double * array;
+ int
+ max_rows, max_cols,
+ n_rows,n_cols;
- switch(target)
+ switch(type)
{
case MATRIX_Q:
- aArray = Q;
- max_rows = n;
+ array = Q;
+ max_rows = n_rows = n;
max_cols = n;
break;
case MATRIX_DU:
- aArray = DU;
+ array = DU;
max_rows = m;
max_cols = n;
+ n_rows = mmax;
+ n_cols = n;
break;
}
- if(row >= max_rows || col >= max_cols)
+ if(row + Mat.size1() > max_rows || col + Mat.size2() > max_cols)
{
- //throw sth.
+ //throw sth. like:
+ std::cout<<"Matrix "<<type<<" bounds violated in addTerm: "<<std::endl<<
+ " max_rows: "<<max_rows<<std::endl<<
+ " max_cols: "<<max_cols<<std::endl<<
+ " req. cols: "<<row + Mat.size1()<<std::endl<<
+ " req. rows: "<<col + Mat.size2()<<std::endl;
}
for(unsigned int i = 0;i < MAL_MATRIX_NB_ROWS(Mat); i++)
for(unsigned int j = 0;j < MAL_MATRIX_NB_COLS(Mat); j++)
- aArray[row+i+(col+j)*n] += Mat(i,j);
+ array[row+i+(col+j)*n_rows] += Mat(i,j);
}
-void QPProblem_s::addTerm(const MAL_VECTOR (&Vec, double), const int target,
+void QPProblem_s::addTerm(const MAL_VECTOR (&Vec, double), const int type,
const int row)
{
double * aArray;
int max_rows;
- switch(target)
+ switch(type)
{
case VECTOR_D:
aArray = D;
@@ -265,18 +322,22 @@ void QPProblem_s::addTerm(const MAL_VECTOR (&Vec, double), const int target,
case VECTOR_DS:
aArray = DS;
- max_rows = n;
+ max_rows = mmax;
break;
}
- if(row >= max_rows)
+ if(row + Vec.size() > max_rows)
{
- //throw sth.
+ //throw sth. like:
+ std::cout<<"Vector "<<type<<" bounds violated in addTerm: "<<std::endl<<
+ "max_rows: "<<max_rows<<std::endl<<
+ "required: "<<row + Vec.size()<<std::endl;
}
- for(unsigned int i = 0;i < MAL_VECTOR_SIZE(Vec); i++)
+ for(unsigned int i = 0;i < Vec.size(); i++)
aArray[row+i] += Vec(i);
+
}
@@ -298,13 +359,13 @@ QPProblem_s::dumpSolverParameters(std::ostream & aos)
void
-QPProblem_s::dump(const int array, std::ostream & aos)
+QPProblem_s::dump(const int type, std::ostream & aos)
{
int lnbrows=0, lnbcols=0;
double * aArray=0;
std::string Name;
- switch(array)
+ switch(type)
{
case MATRIX_Q:
lnbrows = lnbcols = n ;
@@ -361,11 +422,11 @@ QPProblem_s::dump(const int array, std::ostream & aos)
void
-QPProblem_s::dump(const int array, const char * filename)
+QPProblem_s::dump(const int type, const char * filename)
{
std::ofstream aof;
aof.open(filename,std::ofstream::out);
- dump(array,aof);
+ dump(type,aof);
aof.close();
}
@@ -374,12 +435,13 @@ void
QPProblem_s::dumpProblem(std::ostream &aos)
{
dump(MATRIX_Q,aos);
- dump(MATRIX_DU,aos);
-
dump(VECTOR_D,aos);
+
+ dump(MATRIX_DU,aos);
+ dump(VECTOR_DS,aos);
+
dump(VECTOR_XL,aos);
dump(VECTOR_XU,aos);
- dump(VECTOR_DS,aos);
dumpSolverParameters(aos);
}
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.hh b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
index 40dbc29cea86c41fb5aa0283dcf2f6d5a921f32c..2819568d898f23a6d52df1ba2071ce7f86fa9134 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.hh
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
@@ -33,8 +33,8 @@
namespace PatternGeneratorJRL
{
- /*! \brief Final optimization problem to handle velocity reference.
- This object store a standardized optimization quadratic problem.
+ /*! \brief Final optimization problem.
+ This object stores and solves a quadratic problem with linear constraints.
*/
struct QPProblem_s
{
@@ -44,6 +44,20 @@ namespace PatternGeneratorJRL
int iout, ifail, iprint, lwar, liwar;
double Eps;
+ 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_XL=4;
+ const static int VECTOR_XU=5;
+
+ const static int QLD=10;
+ const static int PLDP=11;
+
+ //
+ //Public methods
+ //
+
/// \brief Initialize by default an empty problem.
QPProblem_s();
@@ -68,7 +82,26 @@ namespace PatternGeneratorJRL
/// \name array
/// \name old_size
/// \name new_size
- template <class type> int resize(type * &array, const int & old_size, const int & new_size);
+ template <class type>
+ int resize(type * &array, const int & old_size, const int & new_size)
+ {
+ try
+ {
+ type * NewArray = new type[new_size];
+ initialize(NewArray, new_size, (type)0);
+ for(int i = 0; i < old_size; i++)
+ {
+ NewArray[i] = array[i]; std::cout<<NewArray[i]<<" xx ";
+ }
+ std::cout<<std::endl;
+ if (array!=0)
+ delete [] array;
+ array = NewArray;
+ }
+ catch (std::bad_alloc& ba)
+ {std::cerr << "bad_alloc caught: " << ba.what() << std::endl; }
+ return 0;}
+
/// \brief Add a matrix to the final optimization problem in array form
///
@@ -76,14 +109,14 @@ namespace PatternGeneratorJRL
/// \param target Target matrix
/// \param row First row inside the target
/// \param col First column inside the target
- void addTerm(const MAL_MATRIX (&Mat, double), const int target,
+ void addTerm(const MAL_MATRIX (&Mat, double), const int type,
const int row, const int col);
/// \brief Add a vector to the final optimization problem in array form
///
/// \param Mat Added vector
- /// \param target Target vector
+ /// \param target Target vector type
/// \param row First row inside the target
- void addTerm(const MAL_VECTOR (&Vec, double), const int target,
+ void addTerm(const MAL_VECTOR (&Vec, double), const int type,
const int row);
/// \brief Print of disk the parameters that are passed to the solver
@@ -94,31 +127,37 @@ namespace PatternGeneratorJRL
void dumpProblem(std::ostream &);
/// \brief Dump on disk an array.
- /// \param array
+ ///
+ /// \param type
/// \param filename
- void dump(const int array, const char *filename);
- void dump(const int array, std::ostream &);
+ void dump(const int type, const char *filename);
+ void dump(const int type, std::ostream &);
/// \brief Initialize array
+ ///
/// \param array
/// \param size
- template <class type> void initialize(type * array, const int & size, type value);
-
- /// \brief Solve the problem
- void solve(const int solver);
+ template <class type> void initialize(type * array, const int & size, type value)
+ { std::fill_n(array,size,value); }
+ /// \brief Initialize whole array
+ ///
+ /// \param type
+ void clear(const int type);
- 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_XL=4;
- const static int VECTOR_XU=5;
-
- const static int QLD=10;
- const static int PLDP=11;
+ /// \brief Initialize block of matrix-array
+ ///
+ /// \param type
+ void clear(const int type,
+ const int & row, const int & col,
+ const int & nb_rows, const int & nb_cols);
+ /// \brief Solve the problem
+ void solve(const int solver);
+ //
+ //Protected methods
+ //
protected:
/// The method doing the real job of releasing the memory.
@@ -128,6 +167,9 @@ namespace PatternGeneratorJRL
/// Called when setting the dimensions of the problem.
void resizeAll(const int & NbVariables, const int & NbConstraints);
+ //
+ //Private members
+ //
private:
/// \brief Number of optimization parameters