diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 61f675d1b6516a5c670451289ff8f40799372c7e..1a1c59b405c43aaafdf9d4813350374ff9deb217 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -56,7 +56,7 @@ SET(SOURCES
ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.cpp
ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
ZMPRefTrajectoryGeneration/FilteringAnalyticalTrajectoryByPreviewControl.cpp
- ZMPRefTrajectoryGeneration/qp-problem.cpp
+ ZMPRefTrajectoryGeneration/problem-vel-ref.cpp
MotionGeneration/StepOverPlanner.cpp
MotionGeneration/CollisionDetector.cpp
MotionGeneration/WaistHeightVariation.cpp
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 45615e1f4fd80ac6b6c1f858209feb5029cc1add..cbc732de3e2d2f01972f1e5d2639adca5926d7ec 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -43,6 +43,7 @@
#include <iostream>
#include <fstream>
+#include <Mathematics/qld.h>
#include <ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h>
#include <Debug.h>
@@ -930,8 +931,8 @@ int ZMPVelocityReferencedQP::buildConstraintMatrices(double * &,
{
ODEBUG("localtime: " <<localtime);
- m_Pb.dumpMatrix("/tmp/DU.dat",QPProblem_s::MATRIX_DU);
- m_Pb.dumpVector("/tmp/m_Pb.DS.dat", QPProblem_s::VECTOR_DS);
+ m_Pb.dumpMatrix("/tmp/DU.dat",ProblemVelRef_s::MATRIX_DU);
+ m_Pb.dumpVector("/tmp/m_Pb.DS.dat", ProblemVelRef_s::VECTOR_DS);
ofstream aof;
char Buffer[1024];
@@ -946,7 +947,7 @@ int ZMPVelocityReferencedQP::buildConstraintMatrices(double * &,
aof.close();
sprintf(Buffer,"/tmp/DPX_%f.dat", StartingTime);
- m_Pb.dumpMatrix(Buffer,QPProblem_s::VECTOR_DS);
+ m_Pb.dumpMatrix(Buffer,ProblemVelRef_s::VECTOR_DS);
}
return 0;
@@ -1777,9 +1778,10 @@ void ZMPVelocityReferencedQP::OnLine(double time,
{
struct timeval lbegin,lend;
gettimeofday(&lbegin,0);
-
- m_Pb.solve( QPProblem_s::QLD );
-
+ ql0001_(&m_Pb.m, &m_Pb.me, &m_Pb.mmax, &m_Pb.n, &m_Pb.nmax, &m_Pb.mnn,
+ m_Pb.Q, m_Pb.D, m_Pb.DU, m_Pb.DS, m_Pb.XL, m_Pb.XU,
+ m_Pb.X, m_Pb.U, &m_Pb.iout, &m_Pb.ifail, &m_Pb.iprint,
+ m_Pb.war, &m_Pb.lwar, m_Pb.iwar, &m_Pb.liwar, &m_Pb.Eps);
gettimeofday(&lend,0);
ldt = lend.tv_sec - lbegin.tv_sec +
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
index b1b74c6c9602521afe1766358b18b4ab3d1f6d1e..c48ca7ff9646e467c6bac15c1b2679c6a9dcbe97 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
@@ -41,7 +41,7 @@
#include <PreviewControl/SupportFSM.h>
#include <FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h>
#include <ZMPRefTrajectoryGeneration/OrientationsPreview.h>
-#include <ZMPRefTrajectoryGeneration/qp-problem.hh>
+#include <ZMPRefTrajectoryGeneration/problem-vel-ref.hh>
namespace PatternGeneratorJRL
{
@@ -257,7 +257,7 @@ namespace PatternGeneratorJRL
deque<double> m_PreviewedSupportAngles;
//Final optimization problem
- QPProblem m_Pb;
+ Problem m_Pb;
SupportState_t m_CurrentSupport, m_PrwSupport;
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp b/src/ZMPRefTrajectoryGeneration/problem-vel-ref.cpp
similarity index 85%
rename from src/ZMPRefTrajectoryGeneration/qp-problem.cpp
rename to src/ZMPRefTrajectoryGeneration/problem-vel-ref.cpp
index 99b701247b42bb4a9dd66c211acf8f18dbd4829e..82e46936231c7e6c9bf62110b46c9ccc6dd56715 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
+++ b/src/ZMPRefTrajectoryGeneration/problem-vel-ref.cpp
@@ -37,10 +37,10 @@
#include <iostream>
#include <fstream>
-#include <ZMPRefTrajectoryGeneration/qp-problem.hh>
+#include <ZMPRefTrajectoryGeneration/problem-vel-ref.hh>
using namespace PatternGeneratorJRL;
-QPProblem_s::QPProblem_s():
+ProblemVelRef_s::ProblemVelRef_s():
m(0),me(0),mmax(0), n(0), nmax(0), mnn(0),
Q(0),D(0),DU(0),DS(0),XL(0),X(0),XU(0), NewX(0),
U(0),war(0), iwar(0),
@@ -50,12 +50,12 @@ QPProblem_s::QPProblem_s():
{
}
-QPProblem_s::~QPProblem_s()
+ProblemVelRef_s::~ProblemVelRef_s()
{
ReleaseMemory();
}
-void QPProblem_s::ReleaseMemory()
+void ProblemVelRef_s::ReleaseMemory()
{
if (Q!=0)
delete [] Q;
@@ -91,7 +91,7 @@ void QPProblem_s::ReleaseMemory()
free(U);
}
-void QPProblem_s::AllocateMemory()
+void ProblemVelRef_s::AllocateMemory()
{
war= new double[lwar];
iwar = new int[liwar]; // The Cholesky decomposition is done internally.
@@ -113,7 +113,7 @@ void QPProblem_s::AllocateMemory()
}
-void QPProblem_s::setDimensions(int NbOfConstraints,
+void ProblemVelRef_s::setDimensions(int NbOfConstraints,
int NbOfEqConstraints,
int QP_N,
int StepNumber)
@@ -149,25 +149,13 @@ void QPProblem_s::setDimensions(int NbOfConstraints,
}
}
-void QPProblem_s::initializeProblem()
+void ProblemVelRef_s::initializeProblem()
{
memset(DU,0,(8*m_QP_N+1)*2*(m_QP_N+m_stepNumber)*sizeof(double));
}
-void QPProblem_s::solve(int solver)
-{
- switch(solver)
- {
- case QLD:
- ql0001_(&m, &me, &mmax, &n, &nmax, &mnn,
- Q, D, DU, DS, XL, XU,
- X, U, &iout, &ifail, &iprint,
- war, &lwar, iwar, &liwar, &Eps);
- }
-}
-
-void QPProblem_s::dumpMatrix(std::ostream & aos,
+void ProblemVelRef_s::dumpMatrix(std::ostream & aos,
int type)
{
@@ -197,7 +185,7 @@ void QPProblem_s::dumpMatrix(std::ostream & aos,
}
}
-void QPProblem_s::dumpVector(std::ostream & aos,
+void ProblemVelRef_s::dumpVector(std::ostream & aos,
int type)
{
@@ -235,7 +223,7 @@ void QPProblem_s::dumpVector(std::ostream & aos,
}
-void QPProblem_s::dumpVector(const char * filename,
+void ProblemVelRef_s::dumpVector(const char * filename,
int type)
{
std::ofstream aof;
@@ -245,7 +233,7 @@ void QPProblem_s::dumpVector(const char * filename,
}
-void QPProblem_s::dumpMatrix(const char * filename,
+void ProblemVelRef_s::dumpMatrix(const char * filename,
int type)
{
std::ofstream aof;
@@ -254,7 +242,7 @@ void QPProblem_s::dumpMatrix(const char * filename,
aof.close();
}
-void QPProblem_s::dumpProblem(std::ostream &aos)
+void ProblemVelRef_s::dumpProblem(std::ostream &aos)
{
dumpMatrix(aos,MATRIX_Q);
dumpMatrix(aos,MATRIX_DU);
@@ -266,7 +254,7 @@ void QPProblem_s::dumpProblem(std::ostream &aos)
dumpVector(aos,VECTOR_DS);
}
-void QPProblem_s::dumpProblem(const char * filename)
+void ProblemVelRef_s::dumpProblem(const char * filename)
{
std::ofstream aof;
aof.open(filename,std::ofstream::out);
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.hh b/src/ZMPRefTrajectoryGeneration/problem-vel-ref.hh
similarity index 90%
rename from src/ZMPRefTrajectoryGeneration/qp-problem.hh
rename to src/ZMPRefTrajectoryGeneration/problem-vel-ref.hh
index 27f957d7c27ca169b86dbfa15f2438872138f048..a6ef4629512df89de957eceba0c48ea35628539a 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.hh
+++ b/src/ZMPRefTrajectoryGeneration/problem-vel-ref.hh
@@ -24,11 +24,11 @@
* Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
-#ifndef _QP_PROBLEM_H_
-#define _QP_PROBLEM_H_
+#ifndef _PROBLEM_VEL_REF_H_
+#define _PROBLEM_VEL_REF_H_
#include <jrl/mal/matrixabstractlayer.hh>
-#include <Mathematics/qld.h>
+
namespace PatternGeneratorJRL
{
@@ -36,7 +36,7 @@ namespace PatternGeneratorJRL
/*! \brief Final optimization problem to handle velocity reference.
This object store a standardized optimization quadratic problem.
*/
- struct QPProblem_s
+ struct ProblemVelRef_s
{
int m, me, mmax, n, nmax, mnn;
double *Q, *D, *DU, *DS, *XL, *XU, *X, *NewX, *U, *war;//For COM
@@ -45,10 +45,10 @@ namespace PatternGeneratorJRL
double Eps;
/// \brief Initialize by default an empty problem.
- QPProblem_s();
+ ProblemVelRef_s();
/// \brief Release the memory at the end only.
- ~QPProblem_s();
+ ~ProblemVelRef_s();
/// \brief Set the dimensions of the problem.
/// This method has an internal logic to
@@ -74,10 +74,6 @@ namespace PatternGeneratorJRL
/// \brief Initialize the problem
void initializeProblem();
- /// \brief Solve the problem
- void solve(int solver);
-
-
const static int MATRIX_Q=0;
const static int MATRIX_DU=1;
const static int VECTOR_D=2;
@@ -86,10 +82,6 @@ namespace PatternGeneratorJRL
const static int VECTOR_XU=5;
const static int VECTOR_DS=6;
- const static int QLD=7;
- const static int PLDP=8;
-
-
protected:
/// The method doing the real job of releasing the memory.
@@ -107,6 +99,6 @@ namespace PatternGeneratorJRL
int m_QP_N;
};
- typedef struct QPProblem_s QPProblem;
+ typedef struct ProblemVelRef_s Problem;
}
-#endif /* _QP_PROBLEM_H_ */
+#endif /* _PROBLEM_VEL_REF_H_ */