From d25caee5385537491ab576b09056f645c9dd3ef7 Mon Sep 17 00:00:00 2001
From: olivier-stasse <olivier.stasse@aist.go.jp>
Date: Fri, 11 Mar 2011 05:07:24 +0900
Subject: [PATCH] Revert "New methods"
This reverts commit 5651e57819baee55b77be6b9827e29c29ea8c000.
---
src/ZMPRefTrajectoryGeneration/qp-problem.cpp | 154 ++++++------------
src/ZMPRefTrajectoryGeneration/qp-problem.hh | 86 +++-------
2 files changed, 68 insertions(+), 172 deletions(-)
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
index 8d0a738d..10a7a4c4 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
@@ -120,84 +120,28 @@ QPProblem_s::resizeAll(const int & NbVariables, const int & NbConstraints)
}
-void QPProblem_s::clear(const int type)
+template <class type>
+int
+QPProblem_s::resize(type *& array, const int & old_size, const int & new_size)
{
- double * array;
- int array_size;
- switch(type)
+ try
{
- 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;
+ 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;
}
-
- 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)
+ catch (std::bad_alloc& ba)
{
- 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;
+ std::cerr << "bad_alloc caught: " << ba.what() << std::endl;
}
- 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;
+ return 0;
}
@@ -238,6 +182,14 @@ 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)
{
@@ -253,57 +205,48 @@ QPProblem_s::solve(const int solver)
void
-QPProblem_s::addTerm(const MAL_MATRIX (&Mat, double), const int type,
+QPProblem_s::addTerm(const MAL_MATRIX (&Mat, double), const int target,
const int row, const int col)
{
- double * array;
- int
- max_rows, max_cols,
- n_rows,n_cols;
+ double * aArray;
+ int max_rows, max_cols;
- switch(type)
+ switch(target)
{
case MATRIX_Q:
- array = Q;
- max_rows = n_rows = n;
+ aArray = Q;
+ max_rows = n;
max_cols = n;
break;
case MATRIX_DU:
- array = DU;
+ aArray = DU;
max_rows = m;
max_cols = n;
- n_rows = mmax;
- n_cols = n;
break;
}
- if(row + Mat.size1() > max_rows || col + Mat.size2() > max_cols)
+ if(row >= max_rows || col >= max_cols)
{
- //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;
+ //throw sth.
}
for(unsigned int i = 0;i < MAL_MATRIX_NB_ROWS(Mat); i++)
for(unsigned int j = 0;j < MAL_MATRIX_NB_COLS(Mat); j++)
- array[row+i+(col+j)*n_rows] += Mat(i,j);
+ aArray[row+i+(col+j)*n] += Mat(i,j);
}
-void QPProblem_s::addTerm(const MAL_VECTOR (&Vec, double), const int type,
+void QPProblem_s::addTerm(const MAL_VECTOR (&Vec, double), const int target,
const int row)
{
double * aArray;
int max_rows;
- switch(type)
+ switch(target)
{
case VECTOR_D:
aArray = D;
@@ -322,22 +265,18 @@ void QPProblem_s::addTerm(const MAL_VECTOR (&Vec, double), const int type,
case VECTOR_DS:
aArray = DS;
- max_rows = mmax;
+ max_rows = n;
break;
}
- if(row + Vec.size() > max_rows)
+ if(row >= max_rows)
{
- //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;
+ //throw sth.
}
- for(unsigned int i = 0;i < Vec.size(); i++)
+ for(unsigned int i = 0;i < MAL_VECTOR_SIZE(Vec); i++)
aArray[row+i] += Vec(i);
-
}
@@ -359,13 +298,13 @@ QPProblem_s::dumpSolverParameters(std::ostream & aos)
void
-QPProblem_s::dump(const int type, std::ostream & aos)
+QPProblem_s::dump(const int array, std::ostream & aos)
{
int lnbrows=0, lnbcols=0;
double * aArray=0;
std::string Name;
- switch(type)
+ switch(array)
{
case MATRIX_Q:
lnbrows = lnbcols = n ;
@@ -422,11 +361,11 @@ QPProblem_s::dump(const int type, std::ostream & aos)
void
-QPProblem_s::dump(const int type, const char * filename)
+QPProblem_s::dump(const int array, const char * filename)
{
std::ofstream aof;
aof.open(filename,std::ofstream::out);
- dump(type,aof);
+ dump(array,aof);
aof.close();
}
@@ -435,13 +374,12 @@ void
QPProblem_s::dumpProblem(std::ostream &aos)
{
dump(MATRIX_Q,aos);
- dump(VECTOR_D,aos);
-
dump(MATRIX_DU,aos);
- dump(VECTOR_DS,aos);
-
+
+ dump(VECTOR_D,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 2819568d..40dbc29c 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.hh
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
@@ -33,8 +33,8 @@
namespace PatternGeneratorJRL
{
- /*! \brief Final optimization problem.
- This object stores and solves a quadratic problem with linear constraints.
+ /*! \brief Final optimization problem to handle velocity reference.
+ This object store a standardized optimization quadratic problem.
*/
struct QPProblem_s
{
@@ -44,20 +44,6 @@ 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();
@@ -82,26 +68,7 @@ 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)
- {
- 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;}
-
+ template <class type> int resize(type * &array, const int & old_size, const int & new_size);
/// \brief Add a matrix to the final optimization problem in array form
///
@@ -109,14 +76,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 type,
+ void addTerm(const MAL_MATRIX (&Mat, double), const int target,
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 type
+ /// \param target Target vector
/// \param row First row inside the target
- void addTerm(const MAL_VECTOR (&Vec, double), const int type,
+ void addTerm(const MAL_VECTOR (&Vec, double), const int target,
const int row);
/// \brief Print of disk the parameters that are passed to the solver
@@ -127,37 +94,31 @@ namespace PatternGeneratorJRL
void dumpProblem(std::ostream &);
/// \brief Dump on disk an array.
- ///
- /// \param type
+ /// \param array
/// \param filename
- void dump(const int type, const char *filename);
- void dump(const int type, std::ostream &);
+ void dump(const int array, const char *filename);
+ void dump(const int array, std::ostream &);
/// \brief Initialize array
- ///
/// \param array
/// \param size
- 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);
-
- /// \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);
+ template <class type> void initialize(type * array, const int & size, type value);
/// \brief Solve the problem
void solve(const int solver);
- //
- //Protected methods
- //
+
+ 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;
+
+
protected:
/// The method doing the real job of releasing the memory.
@@ -167,9 +128,6 @@ 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
--
GitLab