Skip to content
Snippets Groups Projects
Commit d25caee5 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Revert "New methods" 

This reverts commit 5651e578.
parent d47862c2
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/ZMPRefTrajectoryGeneration/qp-problem.cpp
+ 46
− 108
View file @ d25caee5
......@@ -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);
}
......
src/ZMPRefTrajectoryGeneration/qp-problem.hh
+ 22
− 64
View file @ d25caee5
......@@ -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
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment