diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index 014682604415c1cf184f5d39d9df38f81476b124..843d342ebedc39f0fc3038ff258b589d5cda3a95 100755
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -341,10 +341,10 @@ GeneratorVelRef::build_constraints_cop(const linear_inequality_t & IneqCoP,
// +D*V
compute_term ( MM_, 1.0, IneqCoP.D.x, IntermedData_->State().V );
- // + Robot_->LeftFoot().Dynamics(COP).U + Robot_->RightFoot().Dynamics(COP).U );
+ // + Robot_->LeftFoot().Dynamics(COP).U + Robot_->RightFoot().Dynamics(COP).U );
Pb.add_term_to( QPProblem::MATRIX_DU, MM_, NbConstraints, 2*N_ );
compute_term ( MM_, 1.0, IneqCoP.D.y, IntermedData_->State().V );
- // + Robot_->LeftFoot().Dynamics(COP).U + Robot_->RightFoot().Dynamics(COP).U );
+ // + Robot_->LeftFoot().Dynamics(COP).U + Robot_->RightFoot().Dynamics(COP).U );
Pb.add_term_to( QPProblem::MATRIX_DU, MM_, NbConstraints, 2*N_+NbStepsPreviewed );
//constant part
@@ -361,7 +361,7 @@ GeneratorVelRef::build_constraints_cop(const linear_inequality_t & IneqCoP,
Pb.add_term_to( QPProblem::VECTOR_DS, MV_, NbConstraints );
compute_term ( MV_, -1.0, IneqCoP.D.x, Robot_->RightFoot().Dynamics(COP).S, Robot_->RightFoot().State().X );
Pb.add_term_to( QPProblem::VECTOR_DS, MV_, NbConstraints );
- */
+ */
compute_term ( MV_, -1.0, IneqCoP.D.y, Robot_->DynamicsCoPJerk().S, IntermedData_->State().CoM.y );
Pb.add_term_to( QPProblem::VECTOR_DS, MV_, NbConstraints );
/*
@@ -371,7 +371,7 @@ GeneratorVelRef::build_constraints_cop(const linear_inequality_t & IneqCoP,
Pb.add_term_to( QPProblem::VECTOR_DS, MV_, NbConstraints );
compute_term ( MV_, -1.0, IneqCoP.D.y, Robot_->RightFoot().Dynamics(COP).S, Robot_->RightFoot().State().Y );
Pb.add_term_to( QPProblem::VECTOR_DS, MV_, NbConstraints );
- */
+ */
// +D*Vc*FP
compute_term ( MV_, IntermedData_->State().SupportState.X, IneqCoP.D.x, IntermedData_->State().Vc );
@@ -449,7 +449,7 @@ GeneratorVelRef::build_constraints( QPProblem & Pb, RelativeFeetInequalities * R
unsigned int NbStepsPreviewed = PrwSupportStates_deq.back().StepNumber;
//Equality constraints
-// build_eq_constraints_feet( PrwSupportStates_deq, NbStepsPreviewed, Pb );
+ // build_eq_constraints_feet( PrwSupportStates_deq, NbStepsPreviewed, Pb );
//CoP constraints
linear_inequality_t & IneqCoP = IntermedData_->Inequalities(IntermedQPMat::INEQ_COP);
@@ -560,69 +560,69 @@ GeneratorVelRef::update_problem( QPProblem & Pb, const std::deque<support_state_
// compute initial solution wich respect all the constraints
void GeneratorVelRef::computeWarmStart( solution_t & Solution){
- int N=Robot_->NbSamplingsPreviewed();
- int M=Solution.SupportStates_deq[N].StepNumber;
+ int N=Robot_->NbSamplingsPreviewed();
+ int M=Solution.SupportStates_deq[N].StepNumber;
- Solution.initialSolution.resize(2*N+2*M);
+ Solution.initialSolution.resize(2*N+2*M);
- // Current support state
- FootType CurrentSupportFoot = Solution.SupportStates_deq[0].Foot;
- double currentSupportFoot_x = Solution.SupportStates_deq[0].X;
- double currentSupportFoot_y = Solution.SupportStates_deq[0].Y;
- double currentYaw = Solution.SupportStates_deq[0].Yaw;
+ // Current support state
+ FootType CurrentSupportFoot = Solution.SupportStates_deq[0].Foot;
+ double currentSupportFoot_x = Solution.SupportStates_deq[0].X;
+ double currentSupportFoot_y = Solution.SupportStates_deq[0].Y;
+ double currentYaw = Solution.SupportStates_deq[0].Yaw;
- // ZMP position vector
- boost_ublas::vector<double> zx(N);
- boost_ublas::vector<double> zy(N);
+ // ZMP position vector
+ boost_ublas::vector<double> zx(N);
+ boost_ublas::vector<double> zy(N);
- double feetSpacing=0.2;
- double sgn;
- int j=0;
+ double feetSpacing=0.2;
+ double sgn;
+ int j=0;
- for(int i=1;i<=N;i++){
- // Check if the support foot has changed
- if (CurrentSupportFoot != Solution.SupportStates_deq[i].Foot){
- CurrentSupportFoot=Solution.SupportStates_deq[i].Foot;
- if (Solution.SupportStates_deq[i].Foot==RIGHT){
- sgn=1;
- }else{
- sgn=-1;
- }
+ for(int i=1;i<=N;i++){
+ // Check if the support foot has changed
+ if (CurrentSupportFoot != Solution.SupportStates_deq[i].Foot){
+ CurrentSupportFoot=Solution.SupportStates_deq[i].Foot;
+ if (Solution.SupportStates_deq[i].Foot==RIGHT){
+ sgn=1;
+ }else{
+ sgn=-1;
+ }
- // Compute new feasible foot position
- currentSupportFoot_x+=sgn*feetSpacing*sin(currentYaw);
- currentSupportFoot_y-=sgn*feetSpacing*cos(currentYaw);
- currentYaw=Solution.SupportOrientations_deq[j];
+ // Compute new feasible foot position
+ currentSupportFoot_x+=sgn*feetSpacing*sin(currentYaw);
+ currentSupportFoot_y-=sgn*feetSpacing*cos(currentYaw);
+ currentYaw=Solution.SupportOrientations_deq[j];
- // Set the new position into initial solution vector
- Solution.initialSolution(2*N+j)=currentSupportFoot_x;
- Solution.initialSolution(2*N+M+j)=currentSupportFoot_y;
+ // Set the new position into initial solution vector
+ Solution.initialSolution(2*N+j)=currentSupportFoot_x;
+ Solution.initialSolution(2*N+M+j)=currentSupportFoot_y;
- ++j;
- }
+ ++j;
+ }
- // Set the ZMP at the center of the foot
- zx(i-1)=currentSupportFoot_x;
- zy(i-1)=currentSupportFoot_y;
- }
+ // Set the ZMP at the center of the foot
+ zx(i-1)=currentSupportFoot_x;
+ zy(i-1)=currentSupportFoot_y;
+ }
- boost_ublas::vector<double> X(N);
- boost_ublas::vector<double> Y(N);
+ boost_ublas::vector<double> X(N);
+ boost_ublas::vector<double> Y(N);
- // Compute initial jerk
- MV2_=prod(Robot_->DynamicsCoPJerk().S,IntermedData_->State().CoM.x);
- X=prod(Robot_->DynamicsCoPJerk().Um1,zx-MV2_);
+ // Compute initial jerk
+ MV2_=prod(Robot_->DynamicsCoPJerk().S,IntermedData_->State().CoM.x);
+ X=prod(Robot_->DynamicsCoPJerk().Um1,zx-MV2_);
- MV2_=prod(Robot_->DynamicsCoPJerk().S,IntermedData_->State().CoM.y);
- Y=prod(Robot_->DynamicsCoPJerk().Um1,zy-MV2_);
+ MV2_=prod(Robot_->DynamicsCoPJerk().S,IntermedData_->State().CoM.y);
+ Y=prod(Robot_->DynamicsCoPJerk().Um1,zy-MV2_);
- for(int i=0;i<N;i++){
- Solution.initialSolution(i)=X(i);
- Solution.initialSolution(N+i)=Y(i);
- }
+ for(int i=0;i<N;i++){
+ Solution.initialSolution(i)=X(i);
+ Solution.initialSolution(N+i)=Y(i);
+ }
}
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
index ad379050de1314aad091d934be48adc7eae31198..fbeeee95fc9e48bba4820431f0e1f64ca88f5d50 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
@@ -52,11 +52,10 @@ using namespace PatternGeneratorJRL;
QPProblem_s::QPProblem_s():
- m_(0),me_(0),mmax_(0), n_(0), nmax_(0), mnn_(0),
- iout_(0),ifail_(0), iprint_(0),
- lwar_(0), liwar_(0),
- eps_(0),
- NbVariables_(0), NbConstraints_(0),NbEqConstraints_(0)
+ m_(0),me_(0),mmax_(0), n_(0), nmax_(0), mnn_(0),
+ iout_(0),ifail_(0), iprint_(0),
+ lwar_(0), liwar_(0), eps_(0),
+ NbVariables_(0), NbConstraints_(0),NbEqConstraints_(0)
{
NbVariables_ = 0;
@@ -68,7 +67,7 @@ QPProblem_s::QPProblem_s():
n_ = NbVariables_;
nmax_ = n_;
mnn_ = m_+2*n_;
-
+
iout_ = 0;
iprint_ = 1;
lwar_ = 3*nmax_*nmax_/2+ 10*nmax_ + 2*mmax_ + 20000;
@@ -77,21 +76,14 @@ QPProblem_s::QPProblem_s():
last_solution_.resize(1,1);
last_solution_.empty();
-
- istate_ = 0x0;
- kx_ = 0x0;
-
- b_ =0x0;
-
- clamda_= 0x0;
+ istate_ = 0x0;
+ kx_ = 0x0;
+ b_ = 0x0;
+ clamda_ = 0x0;
resize_all();
-
-
-
-
}
@@ -137,19 +129,17 @@ QPProblem_s::resize_all()
war_.resize(2*(3*NbVariables_*NbVariables_/2+10*NbVariables_+2*(NbConstraints_+1)+20000), 1,true);
}
- if (istate_!=0x0){
- delete [] istate_;
- delete [] kx_;
- delete [] b_;
- delete [] clamda_;
- }
-
- istate_ = new int [(NbVariables_+NbConstraints_+2)*10];
- kx_ = new int [(NbVariables_+1)*10];
+ if (istate_!=0x0){
+ delete [] istate_;
+ delete [] kx_;
+ delete [] b_;
+ delete [] clamda_;
+ }
- b_ =new double [(NbVariables_+1)*10];
-
- clamda_= new double [(NbVariables_+NbConstraints_+2)*10];
+ istate_ = new int [(NbVariables_+NbConstraints_+2)*10];
+ kx_ = new int [(NbVariables_+1)*10];
+ b_ =new double [(NbVariables_+1)*10];
+ clamda_ = new double [(NbVariables_+NbConstraints_+2)*10];
}
@@ -159,26 +149,26 @@ QPProblem_s::clear( QPElement Type )
{
switch(Type)
- {
- case MATRIX_Q:
- Q_.fill(0.0);
- break;
- case MATRIX_DU:
- DU_.fill(0.0);
- break;
- case VECTOR_D:
- D_.fill(0.0);
- break;
- case VECTOR_DS:
- DS_.fill(0.0);
- break;
- case VECTOR_XL:
- XL_.fill(-1e8);
- break;
- case VECTOR_XU:
- XU_.fill(1e8);
- break;
- }
+ {
+ case MATRIX_Q:
+ Q_.fill(0.0);
+ break;
+ case MATRIX_DU:
+ DU_.fill(0.0);
+ break;
+ case VECTOR_D:
+ D_.fill(0.0);
+ break;
+ case VECTOR_DS:
+ DS_.fill(0.0);
+ break;
+ case VECTOR_XL:
+ XL_.fill(-1e8);
+ break;
+ case VECTOR_XU:
+ XU_.fill(1e8);
+ break;
+ }
}
@@ -253,115 +243,115 @@ QPProblem_s::solve( Solver Solver, solution_t & Result, const Tests & tests )
- if (tests==ITT || tests==ALL){
- int nb_itt_approx=0;
- for(int i = 0; i < m_; i++){
- if (Result.ConstrLagr_vec(i)!=0){
- nb_itt_approx++;
- }
- }
- std::cout << "nb itérations : " << nb_itt_approx << std::endl;
- }
+ if (tests==ITT || tests==ALL){
+ int nb_itt_approx=0;
+ for(int i = 0; i < m_; i++){
+ if (Result.ConstrLagr_vec(i)!=0){
+ nb_itt_approx++;
+ }
+ }
+ std::cout << "nb itérations : " << nb_itt_approx << std::endl;
+ }
-break;
- case LSSOL:
+ break;
+ case LSSOL:
#ifdef LSSOL_FOUND
- sendOption("Print Level = 0");
-
- sendOption("Problem Type = QP2");
-
- double *bl=new double[n_+m_];
- double *bu=new double[n_+m_];
- int size1=n_;
- for(int i=0;i<size1;++i){
- bl[i]=XL_.Array_[i];
- bu[i]=XU_.Array_[i];
- }
- int size2=size1+me_;
- for(int i=size1;i<size2;++i){
- bl[i]=-DS_.Array_[i-size1];
- bu[i]=bl[i];
- }
- int size3=size1+m_;
- for(int i=size2;i<size3;++i){
- bl[i]=-DS_.Array_[i-size1];
- bu[i]=10e10;
- }
-
- if (Result.useWarmStart){
- for(unsigned i=0;i<NbVariables_;++i){
- X_.Array_[i]=Result.initialSolution(i);
- }
- }
-
- if (tests==CTR || tests==ALL){
- // Check if initial solution respect all the constraints
- boost_ublas::matrix<double> DU(NbConstraints_, NbVariables_);
- boost_ublas::vector<double> XX(NbVariables_);
- boost_ublas::vector<double> DS(NbConstraints_);
- boost_ublas::vector<double> tmp;
- for(unsigned i=0;i<NbConstraints_;++i){
- for(unsigned j=0;j<NbVariables_;++j){
- DU(i,j)=DU_.Array_[i+DU_.NbRows_*j];
- }
- DS(i)=DS_.Array_[i];
- }
-
- for(unsigned i=0;i<NbVariables_;++i){
- XX(i)=X_.Array_[i];
- }
-
- tmp=prod(DU,XX);
- int nb_ctr=0;
- for(unsigned i=0;i<NbConstraints_;++i){
- if (tmp(i)+DS(i)<-1e-6){
- std::cout << "Unrespected constraint " << i << " : " << tmp(i) << " < " << -DS(i) << std::endl;
- ++nb_ctr;
- }
- }
- std::cout << std::endl << "Nb unrespected constraints : " << nb_ctr << std::endl;
- }
-
-
- lssol_(&n_, &n_,
- &m_, &mmax_, &n_,
- DU_dense_.Array_, bl, bu, D_.Array_,
- istate_, kx_, X_.Array_, Q_dense_.Array_, b_,
- &inform_, &iter_, &obj_, clamda_,
- iwar_.Array_, &liwar_, war_.Array_, &lwar_);
-
-
-
-
- last_solution_.resize(n_);
- for(int i = 0; i < n_; i++)
- {
- Result.Solution_vec(i) = X_.Array_[i];
- last_solution_(i)=Result.Solution_vec(i);
- Result.LBoundsLagr_vec(i) = 0;
- Result.UBoundsLagr_vec(i) = 0;
- }
- Result.Fail=0;
- Result.Print = 0;
- for(int i = 0; i < m_; i++)
- {
- Result.ConstrLagr_vec(i) = 0;
- }
-
-
-
- if (tests==ITT || tests==ALL){
- std::cout << "nb itérations : " << iter_ << std::endl;
- }
+ sendOption("Print Level = 0");
+
+ sendOption("Problem Type = QP2");
+
+ double *bl=new double[n_+m_];
+ double *bu=new double[n_+m_];
+ int size1=n_;
+ for(int i=0;i<size1;++i){
+ bl[i]=XL_.Array_[i];
+ bu[i]=XU_.Array_[i];
+ }
+ int size2=size1+me_;
+ for(int i=size1;i<size2;++i){
+ bl[i]=-DS_.Array_[i-size1];
+ bu[i]=bl[i];
+ }
+ int size3=size1+m_;
+ for(int i=size2;i<size3;++i){
+ bl[i]=-DS_.Array_[i-size1];
+ bu[i]=10e10;
+ }
+
+ if (Result.useWarmStart){
+ for(unsigned i=0;i<NbVariables_;++i){
+ X_.Array_[i]=Result.initialSolution(i);
+ }
+ }
+
+ if (tests==CTR || tests==ALL){
+ // Check if initial solution respect all the constraints
+ boost_ublas::matrix<double> DU(NbConstraints_, NbVariables_);
+ boost_ublas::vector<double> XX(NbVariables_);
+ boost_ublas::vector<double> DS(NbConstraints_);
+ boost_ublas::vector<double> tmp;
+ for(unsigned i=0;i<NbConstraints_;++i){
+ for(unsigned j=0;j<NbVariables_;++j){
+ DU(i,j)=DU_.Array_[i+DU_.NbRows_*j];
+ }
+ DS(i)=DS_.Array_[i];
+ }
+
+ for(unsigned i=0;i<NbVariables_;++i){
+ XX(i)=X_.Array_[i];
+ }
+
+ tmp=prod(DU,XX);
+ int nb_ctr=0;
+ for(unsigned i=0;i<NbConstraints_;++i){
+ if (tmp(i)+DS(i)<-1e-6){
+ std::cout << "Unrespected constraint " << i << " : " << tmp(i) << " < " << -DS(i) << std::endl;
+ ++nb_ctr;
+ }
+ }
+ std::cout << std::endl << "Nb unrespected constraints : " << nb_ctr << std::endl;
+ }
+
+
+ lssol_(&n_, &n_,
+ &m_, &mmax_, &n_,
+ DU_dense_.Array_, bl, bu, D_.Array_,
+ istate_, kx_, X_.Array_, Q_dense_.Array_, b_,
+ &inform_, &iter_, &obj_, clamda_,
+ iwar_.Array_, &liwar_, war_.Array_, &lwar_);
+
+
+
+
+ last_solution_.resize(n_);
+ for(int i = 0; i < n_; i++)
+ {
+ Result.Solution_vec(i) = X_.Array_[i];
+ last_solution_(i)=Result.Solution_vec(i);
+ Result.LBoundsLagr_vec(i) = 0;
+ Result.UBoundsLagr_vec(i) = 0;
+ }
+ Result.Fail=0;
+ Result.Print = 0;
+ for(int i = 0; i < m_; i++)
+ {
+ Result.ConstrLagr_vec(i) = 0;
+ }
+
+
+
+ if (tests==ITT || tests==ALL){
+ std::cout << "nb iterations : " << iter_ << std::endl;
+ }
#else
- std::cerr << " LSSOL_FOUND not available" << std::endl;
+ std::cerr << " LSSOL_FOUND not available" << std::endl;
#endif //LSSOL_FOUND
- break;
+ break;
- }
+ }
}
@@ -374,28 +364,28 @@ QPProblem_s::add_term_to( QPElement Type, const MAL_MATRIX (&Mat, double),
array_s<double> * Array_p = 0;
switch(Type)
- {
- case MATRIX_Q:
- Array_p = &Q_;
- NbVariables_ = (Col+Mat.size2()>NbVariables_) ? Col+Mat.size2() : NbVariables_;
- break;
-
- case MATRIX_DU:
- Array_p = &DU_;
- NbConstraints_ = (Row+Mat.size1()>NbConstraints_) ? Row+Mat.size1() : NbConstraints_;
- NbVariables_ = (Col+Mat.size2()>NbVariables_) ? Col+Mat.size2() : NbVariables_;
- Row++;//The first rows of DU,DS are empty
- break;
-
- case VECTOR_D:
- break;
- case VECTOR_XL:
- break;
- case VECTOR_XU:
- break;
- case VECTOR_DS:
- break;
- }
+ {
+ case MATRIX_Q:
+ Array_p = &Q_;
+ NbVariables_ = (Col+Mat.size2()>NbVariables_) ? Col+Mat.size2() : NbVariables_;
+ break;
+
+ case MATRIX_DU:
+ Array_p = &DU_;
+ NbConstraints_ = (Row+Mat.size1()>NbConstraints_) ? Row+Mat.size1() : NbConstraints_;
+ NbVariables_ = (Col+Mat.size2()>NbVariables_) ? Col+Mat.size2() : NbVariables_;
+ Row++;//The first rows of DU,DS are empty
+ break;
+
+ case VECTOR_D:
+ break;
+ case VECTOR_XL:
+ break;
+ case VECTOR_XU:
+ break;
+ case VECTOR_DS:
+ break;
+ }
if (NbVariables_ > Array_p->NbCols_ )
@@ -406,25 +396,25 @@ QPProblem_s::add_term_to( QPElement Type, const MAL_MATRIX (&Mat, double),
(NbConstraints_>0))
{
if (NbVariables_>0)
- {
- DU_.resize(2*NbConstraints_, 2*NbVariables_,true);
- }
+ {
+ DU_.resize(2*NbConstraints_, 2*NbVariables_,true);
+ }
}
-
+
if ((NbConstraints_ > DS_.NbRows_ ) &&
(NbConstraints_>0))
{
DS_.resize(2*NbConstraints_,1,true);
}
-
+
unsigned int USize = 2*(NbConstraints_+2*NbVariables_);
if (USize>U_.NbRows_)
{
U_.resize(USize, 1,true);
}
-
+
unsigned int warsize = 2*(3*NbVariables_*NbVariables_/2+10*NbVariables_+2*(NbConstraints_+1)+20000);
-
+
if (warsize> war_.NbRows_)
{
war_.resize(warsize, 1,true);
@@ -448,33 +438,33 @@ QPProblem_s::add_term_to( QPElement Type, const MAL_VECTOR (&Vec, double),
array_s<double> * Array_p = 0;
switch(Type)
- {
- case VECTOR_D:
- Array_p = &D_;
- NbVariables_ = (Row+Vec.size()>NbVariables_) ? Row+Vec.size() : NbVariables_;
- break;
-
- case VECTOR_XL:
- Array_p = &XL_;
- NbVariables_ = (Row+Vec.size()>NbVariables_) ? Row+Vec.size() : NbVariables_;
- break;
-
- case VECTOR_XU:
- Array_p = &XU_;
- NbVariables_ = (Row+Vec.size()>NbVariables_) ? Row+Vec.size() : NbVariables_;
- break;
-
- case VECTOR_DS:
- Array_p = &DS_;
- NbConstraints_ = (Row+Vec.size()>NbConstraints_) ? Row+Vec.size() : NbConstraints_;
- Row++;//The first rows of DU,DS are empty
- break;
-
- case MATRIX_DU:
- break;
- case MATRIX_Q:
- break;
- }
+ {
+ case VECTOR_D:
+ Array_p = &D_;
+ NbVariables_ = (Row+Vec.size()>NbVariables_) ? Row+Vec.size() : NbVariables_;
+ break;
+
+ case VECTOR_XL:
+ Array_p = &XL_;
+ NbVariables_ = (Row+Vec.size()>NbVariables_) ? Row+Vec.size() : NbVariables_;
+ break;
+
+ case VECTOR_XU:
+ Array_p = &XU_;
+ NbVariables_ = (Row+Vec.size()>NbVariables_) ? Row+Vec.size() : NbVariables_;
+ break;
+
+ case VECTOR_DS:
+ Array_p = &DS_;
+ NbConstraints_ = (Row+Vec.size()>NbConstraints_) ? Row+Vec.size() : NbConstraints_;
+ Row++;//The first rows of DU,DS are empty
+ break;
+
+ case MATRIX_DU:
+ break;
+ case MATRIX_Q:
+ break;
+ }
if((NbVariables_ > D_.NbRows_ ) && (NbVariables_>0))
{
@@ -516,55 +506,55 @@ QPProblem_s::dump( QPElement Type, std::ostream & aos)
double * Array=0;
std::string Name;
switch(Type)
- {
- case MATRIX_Q:
- NbRows = NbCols = Q_dense_.NbCols_ ;
- Array = Q_dense_.Array_;
- Name = "Q";
- break;
-
- case MATRIX_DU:
- NbRows = DU_dense_.NbRows_;
- NbCols = DU_dense_.NbCols_;
- Array = DU_dense_.Array_;
- Name = "DU";
- break;
-
- case VECTOR_D:
- NbRows = NbVariables_;
- NbCols = 1 ;
- Array = D_.Array_;
- Name = "D";
- break;
-
- case VECTOR_XL:
- NbRows = NbVariables_;
- NbCols = 1;
- Array = XL_.Array_;
- Name = "XL";
- break;
-
- case VECTOR_XU:
- NbRows = NbVariables_;
- NbCols=1;
- Array = XU_.Array_;
- Name = "XU";
- break;
-
- case VECTOR_DS:
- NbRows = DS_.NbRows_;
- NbCols= 1;
- Array = DS_.Array_;
- Name = "DS";
- break;
- }
+ {
+ case MATRIX_Q:
+ NbRows = NbCols = Q_dense_.NbCols_ ;
+ Array = Q_dense_.Array_;
+ Name = "Q";
+ break;
+
+ case MATRIX_DU:
+ NbRows = DU_dense_.NbRows_;
+ NbCols = DU_dense_.NbCols_;
+ Array = DU_dense_.Array_;
+ Name = "DU";
+ break;
+
+ case VECTOR_D:
+ NbRows = NbVariables_;
+ NbCols = 1 ;
+ Array = D_.Array_;
+ Name = "D";
+ break;
+
+ case VECTOR_XL:
+ NbRows = NbVariables_;
+ NbCols = 1;
+ Array = XL_.Array_;
+ Name = "XL";
+ break;
+
+ case VECTOR_XU:
+ NbRows = NbVariables_;
+ NbCols=1;
+ Array = XU_.Array_;
+ Name = "XU";
+ break;
+
+ case VECTOR_DS:
+ NbRows = DS_.NbRows_;
+ NbCols= 1;
+ Array = DS_.Array_;
+ Name = "DS";
+ break;
+ }
aos << Name <<"["<<NbRows<< ","<< NbCols << "]" << std::endl;
-
+
for(unsigned int i=0;i<NbRows;i++)
{
for(unsigned int j=0;j<NbCols;j++)
- aos << Array[i+j*NbRows] << " ";
+ aos << Array[i+j*NbRows] << " ";
aos << std::endl;
}
aos << std::endl;
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.hh b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
index 71c7de24d9b88a5061b40c04c5a60a2772e0d974..931e341a2afd5e62d0b1ae8b41a17c8fa41ffdfb 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.hh
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
@@ -68,11 +68,12 @@ namespace PatternGeneratorJRL
enum Tests
{
- NONE,
- ALL,
- ITT,
- CTR
+ NONE,
+ ALL,
+ ITT,
+ CTR
};
+
//
//Public methods
//
@@ -157,7 +158,7 @@ namespace PatternGeneratorJRL
// Private methods
//
private:
-
+
/// \brief Release memory.
void release_memory();
@@ -210,22 +211,22 @@ namespace PatternGeneratorJRL
{
try {
- type * NewArray = 0;
- if ((FinalArray.SizeMem_<NbRows*NbCols) ||
- (FinalArray.Array_==0))
- {
- FinalArray.Array_ = new type[NbRows*NbCols];
- FinalArray.SizeMem_ = NbRows*NbCols;
- }
- NewArray = FinalArray.Array_;
-
- fill(NewArray, NbRows*NbCols, (type)0);
- for(unsigned int i = 0; i < NbRows; i++)
- for(unsigned int j = 0; j < NbCols; j++)
- NewArray[i+NbRows*j] = Array_[i+NbRows_*j];
-
- FinalArray.NbRows_ = NbRows;
- FinalArray.NbCols_ = NbCols;
+ type * NewArray = 0;
+ if ((FinalArray.SizeMem_<NbRows*NbCols) ||
+ (FinalArray.Array_==0))
+ {
+ FinalArray.Array_ = new type[NbRows*NbCols];
+ FinalArray.SizeMem_ = NbRows*NbCols;
+ }
+ NewArray = FinalArray.Array_;
+
+ fill(NewArray, NbRows*NbCols, (type)0);
+ for(unsigned int i = 0; i < NbRows; i++)
+ for(unsigned int j = 0; j < NbCols; j++)
+ NewArray[i+NbRows*j] = Array_[i+NbRows_*j];
+
+ FinalArray.NbRows_ = NbRows;
+ FinalArray.NbCols_ = NbCols;
}
catch (std::bad_alloc& ba)
{std::cerr << "bad_alloc caught: " << ba.what() << std::endl; }
@@ -244,31 +245,31 @@ namespace PatternGeneratorJRL
{
try {
- bool Reallocate = false;
- type * NewArray = 0;
- if (NbRows*NbCols>SizeMem_)
- {
- NewArray = new type[NbRows*NbCols];
- SizeMem_ = NbRows*NbCols;
- Reallocate = true;
- }
- else NewArray = Array_;
-
- fill(NewArray, NbRows*NbCols, (type)0);
- if ((Preserve) &&
- (Array_!=0) ) {
- for(unsigned int i = 0; i < NbRows_; i++)
- for(unsigned int j = 0; j < NbCols_; j++)
- NewArray[i+NbRows*j] = Array_[i+NbRows_*j]; }
-
- if ((Array_!=0) && Reallocate)
- {
- delete [] Array_;
- }
- Array_ = NewArray;
-
- NbRows_ = NbRows;
- NbCols_ = NbCols;
+ bool Reallocate = false;
+ type * NewArray = 0;
+ if (NbRows*NbCols>SizeMem_)
+ {
+ NewArray = new type[NbRows*NbCols];
+ SizeMem_ = NbRows*NbCols;
+ Reallocate = true;
+ }
+ else NewArray = Array_;
+
+ fill(NewArray, NbRows*NbCols, (type)0);
+ if ((Preserve) &&
+ (Array_!=0) ) {
+ for(unsigned int i = 0; i < NbRows_; i++)
+ for(unsigned int j = 0; j < NbCols_; j++)
+ NewArray[i+NbRows*j] = Array_[i+NbRows_*j]; }
+
+ if ((Array_!=0) && Reallocate)
+ {
+ delete [] Array_;
+ }
+ Array_ = NewArray;
+
+ NbRows_ = NbRows;
+ NbCols_ = NbCols;
}
catch (std::bad_alloc& ba)
{std::cerr << "bad_alloc caught: " << ba.what() << std::endl; }