diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 53280b1acc657e19704bac44f4c4fd5adda57c3e..443011584e25de662618ede14e64f791ffa76ee3 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -23,7 +23,7 @@
* Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
-
+
/*! This object composes all the objects necessary for the generation of walking
* trajectories as proposed by Herdt Advanced Robotics 2010.
@@ -408,7 +408,8 @@ ZMPVelocityReferencedQP::OnLine(double Time,
// SOLVE PROBLEM:
// --------------
solution_t Result;
- Problem_.solve( QPProblem_s::QLD, Result );
+
+ Problem_.solve( QPProblem_s::LSSOL, Result, QPProblem_s::ALL );
Problem_.reset();
@@ -450,6 +451,25 @@ ZMPVelocityReferencedQP::OnLine(double Time,
0.000001 * (end.tv_usec - start.tv_usec);
TotalAmountOfCPUTime += CurrentCPUTime;
+ static int k=0;
+ static double tab[100000];
+ static int nb=0;
+ tab[nb]=CurrentCPUTime;
+ ++nb;
+ if (CurrentCPUTime>0.002){
+ std::cout << "peak : " << CurrentCPUTime*1000 << " ms" << std::endl;
+ }
+ if (++k%10==0){
+ double moy=0;
+ for(int i=k-20;i<k;++i){
+ if (i>=0){
+ moy+=tab[i];
+ }
+ }
+ moy/=20;
+ std::cout << "mean period : " << moy*1000 << " ms" << std::endl;
+ }
+
}
}
diff --git a/src/ZMPRefTrajectoryGeneration/lssol.h b/src/ZMPRefTrajectoryGeneration/lssol.h
new file mode 100644
index 0000000000000000000000000000000000000000..1f64d1f39fb28f37b1491b9df40179c2b3dc95fd
--- /dev/null
+++ b/src/ZMPRefTrajectoryGeneration/lssol.h
@@ -0,0 +1,32 @@
+#ifndef _LSSOL_H
+#define _LSSOL_H
+
+////////////////////////////////////////////////////////////////////////////////
+///
+///\file lssol.h
+///\brief The header for the QP solver LSSOL
+///\author Lafaye Jory
+///\version 1.0
+///\date 19/08/2011
+///
+////////////////////////////////////////////////////////////////////////////////
+
+extern "C"
+{
+int lsoptn_(char *string, short string_len);
+
+int lssol_(int *mm, int *n, int *nclin, int *
+ lda, int *ldr, double *a, double *bl, double *bu,
+ double *cvec, int *istate, int *kx, double *x,
+ double *r__, double *b, int *inform__, int *iter,
+ double *obj, double *clamda, int *iw, int *leniw,
+ double *w, int *lenw);
+}
+void sendOption(std::string option){
+ char truc[500];
+ option.copy(truc,option.length());
+ lsoptn_(truc ,(short)option.length());
+}
+
+
+#endif
\ No newline at end of file
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
index caccf8082f2a2f07837e6afa5474d3e1a7eb8b11..0e59b71dab9a496f50163139f5ca406d0db91e5e 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
@@ -40,8 +40,13 @@
#include <exception>
#include <ZMPRefTrajectoryGeneration/qp-problem.hh>
+
+#include <ZMPRefTrajectoryGeneration/lssol.h>
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),
@@ -66,6 +71,15 @@ QPProblem_s::QPProblem_s():
eps_ = 1e-8;
resize_all();
+
+
+ istate_ = new int [n_+m_];
+ kx_ = new int [n_];
+
+ b_ =new double [n_];
+
+ clamda_= new double [n_+m_];
+
}
@@ -159,13 +173,10 @@ void QPProblem_s::reset()
void
-QPProblem_s::solve( Solver Solver, solution_t & Result )
+QPProblem_s::solve( Solver Solver, solution_t & Result, Tests Tests )
{
- switch(Solver)
- {
- case QLD:
- m_ = NbConstraints_+1;
+ m_ = NbConstraints_+1;
me_ = NbEqConstraints_;
mmax_ = m_+1;
n_ = NbVariables_;
@@ -178,37 +189,79 @@ QPProblem_s::solve( Solver Solver, solution_t & Result )
liwar_ = n_;
eps_ = 1e-8;
- // if (m_FastFormulationMode==QLDANDLQ)
- // m_Pb.iwar_.Array_[0]=0;
- // else
- iwar_.Array_[0]=1;
+
+ iwar_.Array_[0]=1;
Q_.stick_together(Q_dense_,n_,n_);
DU_.stick_together(DU_dense_,mmax_,n_);
-
- ql0001_(&m_, &me_, &mmax_, &n_, &nmax_, &mnn_,
+
+
+Result.resize(n_,m_);
+
+ switch(Solver)
+ {
+ case QLD:
+
+
+
+ ql0001_(&m_, &me_, &mmax_, &n_, &nmax_, &mnn_,
Q_dense_.Array_, D_.Array_, DU_dense_.Array_, DS_.Array_, XL_.Array_, XU_.Array_,
X_.Array_, U_.Array_, &iout_, &ifail_, &iprint_,
war_.Array_, &lwar_, iwar_.Array_, &liwar_, &eps_);
- Result.resize(n_,m_);
- for(int i = 0; i < n_; i++)
+ for(int i = 0; i < n_; i++)
{
Result.Solution_vec(i) = X_.Array_[i];
- Result.LBoundsLagr_vec(i) = U_.Array_[m_+i];
- Result.UBoundsLagr_vec(i) = U_.Array_[m_+n_+i];
+ Result.LBoundsLagr_vec(i) = U_.Array_[m_+i];
+ Result.UBoundsLagr_vec(i) = U_.Array_[m_+n_+i];
}
for(int i = 0; i < m_; i++)
{
Result.ConstrLagr_vec(i) = U_.Array_[i];
}
- Result.Fail = ifail_;
- Result.Print = iprint_;
+break;
+ case LSSOL:
+ 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;
+ }
+
+
+ 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_);
+
+ for(int i = 0; i < n_; i++)
+ {
+ Result.Solution_vec(i) = X_.Array_[i];
+ }
+ if (Tests==ITT || Tests==ALL){
+ std::cout << "nb itérations : " << iter_ << std::endl;
+ }
+
+ break;
- case PLDP:
- break;
}
}
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.hh b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
index 608d3b6eb40edf4a2a0b70f2c20ded285bcdc30e..1a3158e31e55de621451f95959dc2b6b2810e478 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.hh
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
@@ -58,10 +58,15 @@ namespace PatternGeneratorJRL
enum Solver
{
QLD,
- PLDP
+ LSSOL
};
-
+ enum Tests
+ {
+ NONE,
+ ALL,
+ ITT
+ };
//
//Public methods
//
@@ -121,7 +126,7 @@ namespace PatternGeneratorJRL
///
/// \param[in] Solver
/// \param[out] Result
- void solve( Solver Solver, solution_t & Result);
+ void solve( Solver Solver, solution_t & Result,Tests Tests=NONE );
/// \name Accessors and mutators
/// \{
@@ -146,6 +151,8 @@ namespace PatternGeneratorJRL
//
private:
+
+
/// \brief Release memory.
void release_memory();
@@ -283,6 +290,16 @@ namespace PatternGeneratorJRL
/// \name ql-parameters
/// \{
+
+ int *istate_;
+ int *kx_ ;
+
+ double *b_;
+
+ int inform_;
+ int iter_;
+ double obj_;
+ double *clamda_;
int m_, me_, mmax_, n_, nmax_, mnn_;
array_s<double> Q_, Q_dense_, D_, DU_, DU_dense_, DS_, XL_, XU_, X_, U_, war_;
array_s<int> iwar_;