From d3b47556a3f674189a030c6123b5b4a9de9717e8 Mon Sep 17 00:00:00 2001
From: Andrei Herdt <andrei.herdt@gmail.com>
Date: Tue, 25 Oct 2011 07:34:00 +0200
Subject: [PATCH] Speed up computation
- move build_invariant_part to InitOnLine()
- avoid recomputation of cop in single-body mode
---
src/PreviewControl/rigid-body-system.cpp | 228 ++++++++++--------
src/PreviewControl/rigid-body-system.hh | 30 ++-
src/PreviewControl/rigid-body.hh | 2 +-
.../ZMPVelocityReferencedQP.cpp | 47 ++--
.../ZMPVelocityReferencedQP.h | 16 +-
.../generator-vel-ref.cpp | 2 +-
.../generator-vel-ref.hh | 7 +-
src/ZMPRefTrajectoryGeneration/qp-problem.cpp | 129 ++++++----
src/ZMPRefTrajectoryGeneration/qp-problem.hh | 29 ++-
src/privatepgtypes.h | 12 +
10 files changed, 298 insertions(+), 204 deletions(-)
diff --git a/src/PreviewControl/rigid-body-system.cpp b/src/PreviewControl/rigid-body-system.cpp
index 5aeb7a8c..0ac20b08 100644
--- a/src/PreviewControl/rigid-body-system.cpp
+++ b/src/PreviewControl/rigid-body-system.cpp
@@ -29,14 +29,16 @@ using namespace std;
using namespace boost_ublas;
RigidBodySystem::RigidBodySystem( SimplePluginManager * SPM, CjrlHumanoidDynamicRobot * aHS, SupportFSM * FSM ):
- Mass_(0),CoMHeight_(0),T_(0),Tr_(0),Ta_(0),N_(0),
- OFTG_(0), FSM_(0)
+ mass_(0),CoMHeight_(0),T_(0),Tr_(0),Ta_(0),N_(0),multiBody_(false),
+ OFTG_(0), FSM_(0)
{
HDR_ = aHS;
FSM_ = FSM;
OFTG_ = new OnLineFootTrajectoryGeneration(SPM,HDR_->leftFoot());
+
+
}
@@ -63,13 +65,6 @@ RigidBodySystem::initialize( )
OFTG_->FeetDistance( 0.2 );
OFTG_->StepHeight( 0.05 );
- // Initialize dynamics
- // -------------------
- CoM_.Mass( Mass_ );
- CoM_.NbSamplingsPreviewed( N_ );
- CoM_.initialize();
- compute_dyn_cjerk();
-
// Initialize predetermined trajectories:
// --------------------------------------
initialize_trajectories();
@@ -86,18 +81,22 @@ RigidBodySystem::initialize( )
// -------------------------
// TODO: Use of bounded arrays
// TODO: Remove CoP dynamics
- bool preserve = true;
- CoPDynamicsJerk_.S.resize(N_,3 ,!preserve);
- CoPDynamicsJerk_.S.clear();
- CoPDynamicsJerk_.U.resize(N_,N_,!preserve);
- CoPDynamicsJerk_.U.clear();
- CoPDynamicsJerk_.UT.resize(N_,N_,!preserve);
- CoPDynamicsJerk_.UT.clear();
+ if(multiBody_)
+ {
+ LeftFoot_.Dynamics ( COP_POSITION ).S.resize(N_,3,false);
+ RightFoot_.Dynamics( COP_POSITION ).S.resize(N_,3,false);
+ LeftFoot_.Dynamics ( COP_POSITION ).clear();
+ RightFoot_.Dynamics( COP_POSITION ).clear();
+ }
- LeftFoot_.Dynamics ( COP_POSITION ).S.resize(N_,3,!preserve);
- RightFoot_.Dynamics( COP_POSITION ).S.resize(N_,3,!preserve);
- LeftFoot_.Dynamics ( COP_POSITION ).clear();
- RightFoot_.Dynamics( COP_POSITION ).clear();
+ CoPDynamicsJerk_.Type = COP_POSITION;
+
+ // Initialize dynamics
+ // -------------------
+ CoM_.Mass( mass_ );
+ CoM_.NbSamplingsPreviewed( N_ );
+ CoM_.initialize();
+ compute_dyn_cjerk();
}
@@ -230,31 +229,37 @@ RigidBodySystem::update( const std::deque<support_state_t> & SupportStates_deq,
const std::deque<FootAbsolutePosition> & RightFootTraj_deq )
{
-/*
- compute_foot_pol_dynamics( SupportStates_deq, LeftFoot_.Dynamics(POSITION), RightFoot_.Dynamics(POSITION) );
- compute_foot_pol_dynamics( SupportStates_deq, LeftFoot_.Dynamics(ACCELERATION), RightFoot_.Dynamics(ACCELERATION) );
+ unsigned nbStepsPreviewed = SupportStates_deq.back().StepNumber;
+ if(multiBody_)
+ {
+
+ compute_foot_pol_dynamics( SupportStates_deq, LeftFoot_.Dynamics(POSITION), RightFoot_.Dynamics(POSITION) );
+ compute_foot_pol_dynamics( SupportStates_deq, LeftFoot_.Dynamics(ACCELERATION), RightFoot_.Dynamics(ACCELERATION) );
+
+ precompute_trajectories( SupportStates_deq );
+
+ compute_dyn_cop( nbStepsPreviewed );
+
+ LeftFoot_.State().X[0] = LeftFootTraj_deq.front().x;
+ LeftFoot_.State().X[1] = LeftFootTraj_deq.front().dx;
+ LeftFoot_.State().X[2] = LeftFootTraj_deq.front().ddx;
+ LeftFoot_.State().Y[0] = LeftFootTraj_deq.front().y;
+ LeftFoot_.State().Y[1] = LeftFootTraj_deq.front().dy;
+ LeftFoot_.State().Y[2] = LeftFootTraj_deq.front().ddy;
+ RightFoot_.State().X[0] = RightFootTraj_deq.front().x;
+ RightFoot_.State().X[1] = RightFootTraj_deq.front().dx;
+ RightFoot_.State().X[2] = RightFootTraj_deq.front().ddx;
+ RightFoot_.State().Y[0] = RightFootTraj_deq.front().y;
+ RightFoot_.State().Y[1] = RightFootTraj_deq.front().dy;
+ RightFoot_.State().Y[2] = RightFootTraj_deq.front().ddy;
+
+ }
- precompute_trajectories( SupportStates_deq );
-*/
- compute_dyn_cop( SupportStates_deq );
-/*
- LeftFoot_.State().X[0] = LeftFootTraj_deq.front().x;
- LeftFoot_.State().X[1] = LeftFootTraj_deq.front().dx;
- LeftFoot_.State().X[2] = LeftFootTraj_deq.front().ddx;
- LeftFoot_.State().Y[0] = LeftFootTraj_deq.front().y;
- LeftFoot_.State().Y[1] = LeftFootTraj_deq.front().dy;
- LeftFoot_.State().Y[2] = LeftFootTraj_deq.front().ddy;
- RightFoot_.State().X[0] = RightFootTraj_deq.front().x;
- RightFoot_.State().X[1] = RightFootTraj_deq.front().dx;
- RightFoot_.State().X[2] = RightFootTraj_deq.front().ddx;
- RightFoot_.State().Y[0] = RightFootTraj_deq.front().y;
- RightFoot_.State().Y[1] = RightFootTraj_deq.front().dy;
- RightFoot_.State().Y[2] = RightFootTraj_deq.front().ddy;
-*/
return 0;
}
+
/* TODO : Move this function on another file
*
* Matrix inversion routine.
@@ -283,30 +288,32 @@ invertMatrix (const boost_ublas::matrix<T>& input, boost_ublas::matrix<T>& inver
int
-RigidBodySystem::compute_dyn_cop( const std::deque<support_state_t> & SupportStates_deq )
+RigidBodySystem::compute_dyn_cop( unsigned nbSteps )
{
- const static double GRAVITY = 9.81;
+ const double GRAVITY = 9.81;
- unsigned int NbSteps = SupportStates_deq.back().StepNumber;
- bool preserve = true;
- //TODO: Use of bounded_array to avoid dynamic allocation
- LeftFoot_. Dynamics( COP_POSITION ).U.resize(N_,NbSteps,!preserve);
- LeftFoot_. Dynamics( COP_POSITION ).UT.resize(NbSteps,N_,!preserve);
- RightFoot_.Dynamics( COP_POSITION ).U.resize(N_,NbSteps,!preserve);
- RightFoot_.Dynamics( COP_POSITION ).UT.resize(NbSteps,N_,!preserve);
- LeftFoot_. Dynamics( COP_POSITION ).clear();
- RightFoot_.Dynamics( COP_POSITION ).clear();
-
- CoPDynamicsJerk_.clear();
+ if(multiBody_)
+ {
+ //TODO: Use of bounded_array to avoid dynamic allocation
+ LeftFoot_. Dynamics( COP_POSITION ).U.resize(N_,nbSteps,false);
+ LeftFoot_. Dynamics( COP_POSITION ).UT.resize(nbSteps,N_,false);
+ RightFoot_.Dynamics( COP_POSITION ).U.resize(N_,nbSteps,false);
+ RightFoot_.Dynamics( COP_POSITION ).UT.resize(nbSteps,N_,false);
+ LeftFoot_. Dynamics( COP_POSITION ).clear();
+ RightFoot_.Dynamics( COP_POSITION ).clear();
+ }
+ CoPDynamicsJerk_.S.clear();
+ CoPDynamicsJerk_.U.clear();
+ CoPDynamicsJerk_.UT.clear();
// Add "weighted" dynamic matrices:
// --------------------------------
std::deque<rigid_body_state_t>::iterator LFTraj_it = LeftFoot_.Trajectory().begin();
std::deque<rigid_body_state_t>::iterator RFTraj_it = RightFoot_.Trajectory().begin();
std::deque<rigid_body_state_t>::iterator CoMTraj_it = CoM_.Trajectory().begin();
- std::deque<double>::iterator GRF_it = GRF_deq_.begin();
+ // std::deque<double>::iterator GRF_it = GRF_deq_.begin();
double GRF = 0.0;
for(unsigned int i = 0; i < N_; i++)
{
@@ -320,30 +327,31 @@ RigidBodySystem::compute_dyn_cop( const std::deque<support_state_t> & SupportSta
row ( CoPDynamicsJerk_.S, i ) -= row( CoM_.Dynamics(ACCELERATION).S, i ) * CoM_.Mass()*( CoMTraj_it->Z[0] )/GRF;
row ( CoPDynamicsJerk_.U, i ) -= row( CoM_.Dynamics(ACCELERATION).U, i ) * CoM_.Mass()*( CoMTraj_it->Z[0] )/GRF;
column( CoPDynamicsJerk_.UT, i ) -= row( CoM_.Dynamics(ACCELERATION).U, i ) * CoM_.Mass()*( CoMTraj_it->Z[0] )/GRF;
-/*
- * Usefull for multibody dynamics
- *
- row ( LeftFoot_.Dynamics(COP).S, i ) += row( LeftFoot_.Dynamics(POSITION).S, i ) * LeftFoot_.Mass()*( LFTraj_it->Z[2]+GRAVITY )/GRF;
- row ( LeftFoot_.Dynamics(COP).U, i ) += row( LeftFoot_.Dynamics(POSITION).U, i ) * LeftFoot_.Mass()*( LFTraj_it->Z[2]+GRAVITY )/GRF;
- column( LeftFoot_.Dynamics(COP).UT, i ) += row( LeftFoot_.Dynamics(POSITION).U, i ) * LeftFoot_.Mass()*( LFTraj_it->Z[2]+GRAVITY )/GRF;
- row ( LeftFoot_.Dynamics(COP).S, i ) -= row( LeftFoot_.Dynamics(ACCELERATION).S, i ) * LeftFoot_.Mass()*( LFTraj_it->Z[0] )/GRF;
- row ( LeftFoot_.Dynamics(COP).U, i ) -= row( LeftFoot_.Dynamics(ACCELERATION).U, i ) * LeftFoot_.Mass()*( LFTraj_it->Z[0] )/GRF;
- column( LeftFoot_.Dynamics(COP).UT, i ) -= row( LeftFoot_.Dynamics(ACCELERATION).U, i ) * LeftFoot_.Mass()*( LFTraj_it->Z[0] )/GRF;
-
- row ( RightFoot_.Dynamics(COP).S, i ) += row( RightFoot_.Dynamics(POSITION).S, i ) * RightFoot_.Mass()*( RFTraj_it->Z[2]+GRAVITY )/GRF;
- row ( RightFoot_.Dynamics(COP).U, i ) += row( RightFoot_.Dynamics(POSITION).U, i ) * RightFoot_.Mass()*( RFTraj_it->Z[2]+GRAVITY )/GRF;
- column( RightFoot_.Dynamics(COP).UT, i ) += row( RightFoot_.Dynamics(POSITION).U, i ) * RightFoot_.Mass()*( RFTraj_it->Z[2]+GRAVITY )/GRF;
- row ( RightFoot_.Dynamics(COP).S, i ) -= row( RightFoot_.Dynamics(ACCELERATION).S, i ) * RightFoot_.Mass()*( RFTraj_it->Z[0] )/GRF;
- row ( RightFoot_.Dynamics(COP).U, i ) -= row( RightFoot_.Dynamics(ACCELERATION).U, i ) * RightFoot_.Mass()*( RFTraj_it->Z[0] )/GRF;
- column( RightFoot_.Dynamics(COP).UT, i ) -= row( RightFoot_.Dynamics(ACCELERATION).U, i ) * RightFoot_.Mass()*( RFTraj_it->Z[0] )/GRF;
-*/
+
+ if(multiBody_)
+ {
+ row ( LeftFoot_.Dynamics(COP_POSITION).S, i ) += row( LeftFoot_.Dynamics(POSITION).S, i ) * LeftFoot_.Mass()*( LFTraj_it->Z[2]+GRAVITY )/GRF;
+ row ( LeftFoot_.Dynamics(COP_POSITION).U, i ) += row( LeftFoot_.Dynamics(POSITION).U, i ) * LeftFoot_.Mass()*( LFTraj_it->Z[2]+GRAVITY )/GRF;
+ column( LeftFoot_.Dynamics(COP_POSITION).UT, i ) += row( LeftFoot_.Dynamics(POSITION).U, i ) * LeftFoot_.Mass()*( LFTraj_it->Z[2]+GRAVITY )/GRF;
+ row ( LeftFoot_.Dynamics(COP_POSITION).S, i ) -= row( LeftFoot_.Dynamics(ACCELERATION).S, i ) * LeftFoot_.Mass()*( LFTraj_it->Z[0] )/GRF;
+ row ( LeftFoot_.Dynamics(COP_POSITION).U, i ) -= row( LeftFoot_.Dynamics(ACCELERATION).U, i ) * LeftFoot_.Mass()*( LFTraj_it->Z[0] )/GRF;
+ column( LeftFoot_.Dynamics(COP_POSITION).UT, i ) -= row( LeftFoot_.Dynamics(ACCELERATION).U, i ) * LeftFoot_.Mass()*( LFTraj_it->Z[0] )/GRF;
+
+ row ( RightFoot_.Dynamics(COP_POSITION).S, i ) += row( RightFoot_.Dynamics(POSITION).S, i ) * RightFoot_.Mass()*( RFTraj_it->Z[2]+GRAVITY )/GRF;
+ row ( RightFoot_.Dynamics(COP_POSITION).U, i ) += row( RightFoot_.Dynamics(POSITION).U, i ) * RightFoot_.Mass()*( RFTraj_it->Z[2]+GRAVITY )/GRF;
+ column( RightFoot_.Dynamics(COP_POSITION).UT, i ) += row( RightFoot_.Dynamics(POSITION).U, i ) * RightFoot_.Mass()*( RFTraj_it->Z[2]+GRAVITY )/GRF;
+ row ( RightFoot_.Dynamics(COP_POSITION).S, i ) -= row( RightFoot_.Dynamics(ACCELERATION).S, i ) * RightFoot_.Mass()*( RFTraj_it->Z[0] )/GRF;
+ row ( RightFoot_.Dynamics(COP_POSITION).U, i ) -= row( RightFoot_.Dynamics(ACCELERATION).U, i ) * RightFoot_.Mass()*( RFTraj_it->Z[0] )/GRF;
+ column( RightFoot_.Dynamics(COP_POSITION).UT, i ) -= row( RightFoot_.Dynamics(ACCELERATION).U, i ) * RightFoot_.Mass()*( RFTraj_it->Z[0] )/GRF;
+ }
+
CoMTraj_it++;
LFTraj_it++;
RFTraj_it++;
}
- CoPDynamicsJerk_.Um1.resize(CoPDynamicsJerk_.U.size1(),CoPDynamicsJerk_.U.size2());
- invertMatrix(CoPDynamicsJerk_.U,CoPDynamicsJerk_.Um1);
+ CoPDynamicsJerk_.Um1.resize(CoPDynamicsJerk_.U.size1(),CoPDynamicsJerk_.U.size2());
+ // invertMatrix(CoPDynamicsJerk_.U,CoPDynamicsJerk_.Um1);
return 0;
@@ -360,6 +368,7 @@ RigidBodySystem::compute_dyn_cjerk()
compute_dyn_cjerk( CoM_.Dynamics(VELOCITY) );
compute_dyn_cjerk( CoM_.Dynamics(ACCELERATION) );
compute_dyn_cjerk( CoM_.Dynamics(JERK) );
+ compute_dyn_cjerk( CoPDynamicsJerk_ );
return 0;
@@ -370,12 +379,11 @@ int
RigidBodySystem::compute_dyn_cjerk( linear_dynamics_t & Dynamics )
{
//TODO: This can be moved to RigidBody.
- bool preserve = true;
- Dynamics.U.resize(N_,N_,!preserve);
+ Dynamics.U.resize(N_,N_,false);
Dynamics.U.clear();
- Dynamics.UT.resize(N_,N_,!preserve);
+ Dynamics.UT.resize(N_,N_,false);
Dynamics.UT.clear();
- Dynamics.S.resize(N_,3,!preserve);
+ Dynamics.S.resize(N_,3,false);
Dynamics.S.clear();
switch(Dynamics.Type)
@@ -425,6 +433,17 @@ RigidBodySystem::compute_dyn_cjerk( linear_dynamics_t & Dynamics )
}
break;
case COP_POSITION:
+ for(int i=0;i<N_;i++)
+ {
+ Dynamics.S(i,0) = 1.0; Dynamics.S(i,1) = (i+1)*T_; Dynamics.S(i,2) = (i+1)*(i+1)*T_*T_*0.5-CoMHeight_/9.81;
+ for(int j=0;j<N_;j++)
+ if (j<=i)
+ Dynamics.U(i,j) = Dynamics.UT(j,i) = (1 + 3*(i-j) + 3*(i-j)*(i-j)) * T_*T_*T_/6.0 - T_*CoMHeight_/9.81;
+ else
+ Dynamics.U(i,j) = Dynamics.UT(j,i) = 0.0;
+ }
+ break;
+ // compute_dyn_cop( 0 );
break;
}
@@ -441,17 +460,16 @@ RigidBodySystem::compute_foot_zero_dynamics( const std::deque<support_state_t> &
// Resize the matrices:
// --------------------
- unsigned int NbSteps = SupportStates_deq.back().StepNumber;
+ unsigned int nbSteps = SupportStates_deq.back().StepNumber;
- bool preserve = true;
- LeftFootDynamics.U.resize(N_,NbSteps,!preserve);
+ LeftFootDynamics.U.resize(N_,nbSteps,false);
LeftFootDynamics.U.clear();
- LeftFootDynamics.UT.resize(NbSteps,N_,!preserve);
+ LeftFootDynamics.UT.resize(nbSteps,N_,false);
LeftFootDynamics.UT.clear();
LeftFootDynamics.S.clear();
- RightFootDynamics.U.resize(N_,NbSteps,!preserve);
+ RightFootDynamics.U.resize(N_,nbSteps,false);
RightFootDynamics.U.clear();
- RightFootDynamics.UT.resize(NbSteps,N_,!preserve);
+ RightFootDynamics.UT.resize(nbSteps,N_,false);
RightFootDynamics.UT.clear();
RightFootDynamics.S.clear();
@@ -498,7 +516,7 @@ RigidBodySystem::compute_foot_zero_dynamics( const std::deque<support_state_t> &
SFDynamics->S(i,0) = SFDynamics->S(i-1,0);FFDynamics->S(i,0) = FFDynamics->S(i-1,0);
SFDynamics->S(i,1) = SFDynamics->S(i-1,1);FFDynamics->S(i,1) = FFDynamics->S(i-1,1);
SFDynamics->S(i,2) = SFDynamics->S(i-1,2);FFDynamics->S(i,2) = FFDynamics->S(i-1,2);
- for(unsigned int SNb = 0; SNb < NbSteps; SNb++)
+ for(unsigned int SNb = 0; SNb < nbSteps; SNb++)
{
SFDynamics->U(i,SNb) = SFDynamics->UT(SNb,i) = SFDynamics->U(i-1,SNb);
FFDynamics->U(i,SNb) = FFDynamics->UT(SNb,i) = FFDynamics->U(i-1,SNb);
@@ -518,7 +536,7 @@ RigidBodySystem::compute_foot_zero_dynamics( const std::deque<support_state_t> &
SFDynamics->U(i,SS_it->StepNumber) = SFDynamics->UT(SS_it->StepNumber,i) = 0.0;
}
}
- else if(SS_it->StepNumber == 1 && SS_it->StepNumber < NbSteps)
+ else if(SS_it->StepNumber == 1 && SS_it->StepNumber < nbSteps)
{
if(FFDynamics->Type == POSITION)
{
@@ -534,7 +552,7 @@ RigidBodySystem::compute_foot_zero_dynamics( const std::deque<support_state_t> &
FFDynamics->S(i,0) = FFDynamics->S(i-1,0);SFDynamics->S(i,0) = SFDynamics->S(i-1,0);
FFDynamics->S(i,1) = FFDynamics->S(i-1,1);SFDynamics->S(i,1) = SFDynamics->S(i-1,1);
FFDynamics->S(i,2) = FFDynamics->S(i-1,2);SFDynamics->S(i,2) = SFDynamics->S(i-1,2);
- for(unsigned int j = 0; j<NbSteps; j++)
+ for(unsigned int j = 0; j<nbSteps; j++)
{
FFDynamics->U(i,j) = FFDynamics->UT(j,i) = FFDynamics->U(i-1,j);
SFDynamics->U(i,j) = SFDynamics->UT(j,i) = SFDynamics->U(i-1,j);
@@ -555,17 +573,16 @@ RigidBodySystem::compute_foot_pol_dynamics( const std::deque<support_state_t> &
// Resize the matrices:
// --------------------
- unsigned int NbSteps = SupportStates_deq.back().StepNumber;
+ unsigned int nbSteps = SupportStates_deq.back().StepNumber;
- bool preserve = true;
- LeftFootDynamics.U.resize(N_,NbSteps,!preserve);
+ LeftFootDynamics.U.resize(N_,nbSteps,false);
LeftFootDynamics.U.clear();
- LeftFootDynamics.UT.resize(NbSteps,N_,!preserve);
+ LeftFootDynamics.UT.resize(nbSteps,N_,false);
LeftFootDynamics.UT.clear();
LeftFootDynamics.S.clear();
- RightFootDynamics.U.resize(N_,NbSteps,!preserve);
+ RightFootDynamics.U.resize(N_,nbSteps,false);
RightFootDynamics.U.clear();
- RightFootDynamics.UT.resize(NbSteps,N_,!preserve);
+ RightFootDynamics.UT.resize(nbSteps,N_,false);
RightFootDynamics.UT.clear();
RightFootDynamics.S.clear();
@@ -614,7 +631,7 @@ RigidBodySystem::compute_foot_pol_dynamics( const std::deque<support_state_t> &
SFDynamics->S(i,0) = SFDynamics->S(i-1,0);FFDynamics->S(i,0) = FFDynamics->S(i-1,0);
SFDynamics->S(i,1) = SFDynamics->S(i-1,1);FFDynamics->S(i,1) = FFDynamics->S(i-1,1);
SFDynamics->S(i,2) = SFDynamics->S(i-1,2);FFDynamics->S(i,2) = FFDynamics->S(i-1,2);
- for(unsigned int SNb = 0; SNb < NbSteps; SNb++)
+ for(unsigned int SNb = 0; SNb < nbSteps; SNb++)
{
SFDynamics->U(i,SNb) = SFDynamics->UT(SNb,i) = SFDynamics->U(i-1,SNb);
FFDynamics->U(i,SNb) = FFDynamics->UT(SNb,i) = FFDynamics->U(i-1,SNb);
@@ -626,7 +643,7 @@ RigidBodySystem::compute_foot_pol_dynamics( const std::deque<support_state_t> &
compute_sbar( Spbar, Sabar, (SS_it->NbInstants)*T_, FSM_->StepPeriod()-T_ );
compute_ubar( Upbar, Uabar, (SS_it->NbInstants)*T_, FSM_->StepPeriod()-T_ );
- if(SS_it->StepNumber == 0 && SS_it->StepNumber < NbSteps)
+ if(SS_it->StepNumber == 0 && SS_it->StepNumber < nbSteps)
{
if(FFDynamics->Type == POSITION)
{
@@ -653,7 +670,7 @@ RigidBodySystem::compute_foot_pol_dynamics( const std::deque<support_state_t> &
SFDynamics->U(i,SS_it->StepNumber) = SFDynamics->UT(SS_it->StepNumber,i) = SFDynamics->U(i-1,SS_it->StepNumber);
}
}
- else if(SS_it->StepNumber == 1 && SS_it->StepNumber < NbSteps)
+ else if(SS_it->StepNumber == 1 && SS_it->StepNumber < nbSteps)
{
if(FFDynamics->Type == POSITION)
{
@@ -686,7 +703,7 @@ RigidBodySystem::compute_foot_pol_dynamics( const std::deque<support_state_t> &
FFDynamics->S(i,0) = FFDynamics->S(i-1,0);SFDynamics->S(i,0) = SFDynamics->S(i-1,0);
FFDynamics->S(i,1) = FFDynamics->S(i-1,1);SFDynamics->S(i,1) = SFDynamics->S(i-1,1);
FFDynamics->S(i,2) = FFDynamics->S(i-1,2);SFDynamics->S(i,2) = SFDynamics->S(i-1,2);
- for(unsigned int j = 0; j<NbSteps; j++)
+ for(unsigned int j = 0; j<nbSteps; j++)
{
FFDynamics->U(i,j) = FFDynamics->UT(j,i) = FFDynamics->U(i-1,j);
SFDynamics->U(i,j) = SFDynamics->UT(j,i) = SFDynamics->U(i-1,j);
@@ -708,16 +725,15 @@ RigidBodySystem::compute_foot_cjerk_dynamics( const std::deque<support_state_t>
// Resize the matrices:
// --------------------
- unsigned int NbSteps = SupportStates_deq.back().StepNumber;
- bool Preserve = true;
- LeftFootDynamics.U.resize(N_,NbSteps,!Preserve);
+ unsigned int nbSteps = SupportStates_deq.back().StepNumber;
+ LeftFootDynamics.U.resize(N_,nbSteps,false);
LeftFootDynamics.U.clear();
- LeftFootDynamics.UT.resize(NbSteps,N_,!Preserve);
+ LeftFootDynamics.UT.resize(nbSteps,N_,false);
LeftFootDynamics.UT.clear();
LeftFootDynamics.S.clear();
- RightFootDynamics.U.resize(N_,NbSteps,!Preserve);
+ RightFootDynamics.U.resize(N_,nbSteps,false);
RightFootDynamics.U.clear();
- RightFootDynamics.UT.resize(NbSteps,N_,!Preserve);
+ RightFootDynamics.UT.resize(nbSteps,N_,false);
RightFootDynamics.UT.clear();
RightFootDynamics.S.clear();
@@ -764,7 +780,7 @@ RigidBodySystem::compute_foot_cjerk_dynamics( const std::deque<support_state_t>
SFDynamics->S(i,0) = SFDynamics->S(i-1,0);FFDynamics->S(i,0) = FFDynamics->S(i-1,0);
SFDynamics->S(i,1) = SFDynamics->S(i-1,1);FFDynamics->S(i,1) = FFDynamics->S(i-1,1);
SFDynamics->S(i,2) = SFDynamics->S(i-1,2);FFDynamics->S(i,2) = FFDynamics->S(i-1,2);
- for(unsigned int SNb = 0; SNb < NbSteps; SNb++)
+ for(unsigned int SNb = 0; SNb < nbSteps; SNb++)
{
SFDynamics->U(i,SNb) = SFDynamics->UT(SNb,i) = SFDynamics->U(i-1,SNb);
FFDynamics->U(i,SNb) = FFDynamics->UT(SNb,i) = FFDynamics->U(i-1,SNb);
@@ -776,7 +792,7 @@ RigidBodySystem::compute_foot_cjerk_dynamics( const std::deque<support_state_t>
compute_sbar( Spbar, Sabar, (SS_it->NbInstants+1)*T_, FSM_->StepPeriod()-T_ );
compute_ubar( Upbar, Uabar, (SS_it->NbInstants+1)*T_, FSM_->StepPeriod()-T_ );
- if(SS_it->StepNumber == 0 && SS_it->StepNumber < NbSteps)
+ if(SS_it->StepNumber == 0 && SS_it->StepNumber < nbSteps)
{
if(FFDynamics->Type == POSITION)
{
@@ -803,7 +819,7 @@ RigidBodySystem::compute_foot_cjerk_dynamics( const std::deque<support_state_t>
SFDynamics->U(i,SS_it->StepNumber) = SFDynamics->UT(SS_it->StepNumber,i) = SFDynamics->U(i-1,SS_it->StepNumber);
}
}
- else if(SS_it->StepNumber == 1 && SS_it->StepNumber < NbSteps)
+ else if(SS_it->StepNumber == 1 && SS_it->StepNumber < nbSteps)
{
if(FFDynamics->Type == POSITION)
{
@@ -835,7 +851,7 @@ RigidBodySystem::compute_foot_cjerk_dynamics( const std::deque<support_state_t>
FFDynamics->S(i,0) = FFDynamics->S(i-1,0);SFDynamics->S(i,0) = SFDynamics->S(i-1,0);
FFDynamics->S(i,1) = FFDynamics->S(i-1,1);SFDynamics->S(i,1) = SFDynamics->S(i-1,1);
FFDynamics->S(i,2) = FFDynamics->S(i-1,2);SFDynamics->S(i,2) = SFDynamics->S(i-1,2);
- for(unsigned int j = 0; j<NbSteps; j++)
+ for(unsigned int j = 0; j<nbSteps; j++)
{
FFDynamics->U(i,j) = FFDynamics->UT(j,i) = FFDynamics->U(i-1,j);
SFDynamics->U(i,j) = SFDynamics->UT(j,i) = SFDynamics->U(i-1,j);
diff --git a/src/PreviewControl/rigid-body-system.hh b/src/PreviewControl/rigid-body-system.hh
index b2a6cdb3..4ddd3073 100644
--- a/src/PreviewControl/rigid-body-system.hh
+++ b/src/PreviewControl/rigid-body-system.hh
@@ -83,7 +83,7 @@ namespace PatternGeneratorJRL
/// \brief Generate final trajectories
///
- /// \param[in] Time Current time
+ /// \param[in] time Current time
/// \param[in] CurrentSupport
/// \param[in] Result Optimization result
/// \param[in] SupportStates_deq
@@ -92,7 +92,7 @@ namespace PatternGeneratorJRL
/// \param[out] RightFootTraj_deq
///
/// return 0
- int generate_trajectories( double Time, const solution_t & Result,
+ int generate_trajectories( double time, const solution_t & Result,
const std::deque<support_state_t> & SupportStates_deq, const std::deque<double> & PreviewedSupportAngles_deq,
std::deque<FootAbsolutePosition> & LeftFootTraj_deq, std::deque<FootAbsolutePosition> & RightFootTraj_deq);
@@ -122,31 +122,36 @@ namespace PatternGeneratorJRL
inline void RightFoot( const RigidBody & RightFoot )
{RightFoot_ = RightFoot;};
- inline double const & SamplingPeriodSim( ) const
+ inline double SamplingPeriodSim( ) const
{ return T_; }
inline void SamplingPeriodSim( double T )
{ T_ = T; }
- inline double const & SamplingPeriodAct( ) const
+ inline double SamplingPeriodAct( ) const
{ return Ta_; }
inline void SamplingPeriodAct( double Ta )
{ Ta_ = Ta; }
- inline unsigned const & NbSamplingsPreviewed( ) const
+ inline unsigned NbSamplingsPreviewed( ) const
{ return N_; }
inline void NbSamplingsPreviewed( unsigned N )
{ N_ = N; }
- inline double const & Mass( ) const
- { return Mass_; }
+ inline double Mass( ) const
+ { return mass_; }
inline void Mass( double Mass )
- { Mass_ = Mass; }
+ { mass_ = Mass; }
- inline double const & CoMHeight( ) const
+ inline double CoMHeight( ) const
{ return CoMHeight_; }
inline void CoMHeight( double Height )
{ CoMHeight_ = Height; }
+ inline bool multiBody( ) const
+ { return multiBody_; }
+ inline void multiBody( bool multiBody )
+ { multiBody_ = multiBody; }
+
std::deque<support_state_t> & SupportTrajectory()
{ return SupportTrajectory_deq_; }
/// \}
@@ -162,7 +167,7 @@ namespace PatternGeneratorJRL
/// \param[out] Dynamics Matrices to be filled
///
/// return 0
- int compute_dyn_cop( const std::deque<support_state_t> & SupportStates_deq );
+ int compute_dyn_cop( unsigned nbSteps );
/// \brief Initialize dynamics of the body center
/// Suppose a piecewise constant jerk
@@ -259,7 +264,7 @@ namespace PatternGeneratorJRL
std::deque<double> GRF_deq_;
/// \brief Total robot mass
- double Mass_;
+ double mass_;
/// \brief CoMHeight
double CoMHeight_;
@@ -277,6 +282,9 @@ namespace PatternGeneratorJRL
/// \brief Nb previewed samples
unsigned int N_;
+ /// \brief Multi-body mode
+ bool multiBody_;
+
/// \brief Standard polynomial trajectories for the feet.
OnLineFootTrajectoryGeneration * OFTG_;
diff --git a/src/PreviewControl/rigid-body.hh b/src/PreviewControl/rigid-body.hh
index 06799e9b..2faac350 100644
--- a/src/PreviewControl/rigid-body.hh
+++ b/src/PreviewControl/rigid-body.hh
@@ -72,7 +72,7 @@ namespace PatternGeneratorJRL
boost_ublas::matrix<double> UT;
/// \brief State matrix
- boost_ublas::matrix<double, boost_ublas::column_major> S;
+ boost_ublas::matrix<double, boost_ublas::row_major> S;
dynamics_e Type;
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index ccbfc1b8..f03ffab7 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -97,8 +97,10 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
Robot_->SamplingPeriodSim( QP_T_ );
Robot_->SamplingPeriodAct( m_SamplingPeriod );
Robot_->CoMHeight( 0.814 );
+ Robot_->multiBody(false);
Robot_->initialize( );
+
IntermedData_ = new IntermedQPMat();
VRQPGenerator_ = new GeneratorVelRef( SPM, IntermedData_, Robot_, RFI_ );
@@ -307,12 +309,19 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
CoM_(CoM);
IntermedData_->CoM(CoM_());
+ // BUILD CONSTANT PART OF THE OBJECTIVE:
+ // -------------------------------------
+ Problem_.reset();
+ Problem_.nbInvariantRows(2*QP_N_);
+ Problem_.nbInvariantCols(2*QP_N_);
+ VRQPGenerator_->build_invariant_part( Problem_ );
+
return 0;
}
void
-ZMPVelocityReferencedQP::OnLine(double Time,
+ZMPVelocityReferencedQP::OnLine(double time,
deque<ZMPPosition> & FinalZMPTraj_deq,
deque<COMState> & FinalCOMTraj_deq,
deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
@@ -325,7 +334,7 @@ ZMPVelocityReferencedQP::OnLine(double Time,
// Test if the end of the online mode has been reached.
if ((EndingPhase_) &&
- (Time>=TimeToStopOnLineMode_))
+ (time>=TimeToStopOnLineMode_))
{ m_OnLineMode = false; }
@@ -339,16 +348,18 @@ ZMPVelocityReferencedQP::OnLine(double Time,
CoM_(com);
}
+
+
// UPDATE WALKING TRAJECTORIES:
// ----------------------------
- if(Time + 0.00001 > UpperTimeLimitToUpdate_)
+ if(time + 0.00001 > UpperTimeLimitToUpdate_)
{
// UPDATE INTERNAL DATA:
// ---------------------
- Problem_.reset();
+ Problem_.reset_variant();
Solution_.reset();
- VRQPGenerator_->CurrentTime( Time );
+ VRQPGenerator_->CurrentTime( time );
VelRef_=NewVelRef_;
SupportFSM_->update_vel_reference(VelRef_, IntermedData_->SupportState());
IntermedData_->Reference( VelRef_ );
@@ -357,13 +368,13 @@ ZMPVelocityReferencedQP::OnLine(double Time,
// PREVIEW SUPPORT STATES FOR THE WHOLE PREVIEW WINDOW:
// ----------------------------------------------------
- VRQPGenerator_->preview_support_states( Time, SupportFSM_,
+ VRQPGenerator_->preview_support_states( time, SupportFSM_,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq, Solution_.SupportStates_deq );
// COMPUTE ORIENTATIONS OF FEET FOR WHOLE PREVIEW PERIOD:
// ------------------------------------------------------
- OrientPrw_->preview_orientations( Time, VelRef_,
+ OrientPrw_->preview_orientations( time, VelRef_,
SupportFSM_->StepPeriod(),
FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
Solution_ );
@@ -380,11 +391,6 @@ ZMPVelocityReferencedQP::OnLine(double Time,
VRQPGenerator_->compute_global_reference( Solution_ );
- // BUILD CONSTANT PART OF THE OBJECTIVE:
- // -------------------------------------
- VRQPGenerator_->build_invariant_part( Problem_ );
-
-
// BUILD VARIANT PART OF THE OBJECTIVE:
// ------------------------------------
VRQPGenerator_->update_problem( Problem_, Solution_.SupportStates_deq );
@@ -398,33 +404,32 @@ ZMPVelocityReferencedQP::OnLine(double Time,
// SOLVE PROBLEM:
// --------------
if (Solution_.useWarmStart)
- VRQPGenerator_->compute_warm_start( Solution_ );
+ VRQPGenerator_->compute_warm_start( Solution_ );//TODO: Move to update_problem or build_constraints?
Problem_.solve( QLD, Solution_, NONE );
if(Solution_.Fail>0)
- Problem_.dump( Time );
-
+ Problem_.dump( time );
// INTERPOLATE THE NEXT COMPUTED COM STATE:
// ----------------------------------------
- unsigned int CurrentIndex = FinalCOMTraj_deq.size();
- FinalCOMTraj_deq.resize((unsigned int)(QP_T_/m_SamplingPeriod)+CurrentIndex);
- FinalZMPTraj_deq.resize((unsigned int)(QP_T_/m_SamplingPeriod)+CurrentIndex);
- CoM_.Interpolation( FinalCOMTraj_deq, FinalZMPTraj_deq, CurrentIndex,
+ unsigned currentIndex = FinalCOMTraj_deq.size();
+ FinalCOMTraj_deq.resize( (unsigned)(QP_T_/m_SamplingPeriod)+currentIndex );
+ FinalZMPTraj_deq.resize( (unsigned)(QP_T_/m_SamplingPeriod)+currentIndex );
+ CoM_.Interpolation( FinalCOMTraj_deq, FinalZMPTraj_deq, currentIndex,
Solution_.Solution_vec[0], Solution_.Solution_vec[QP_N_] );
CoM_.OneIteration( Solution_.Solution_vec[0],Solution_.Solution_vec[QP_N_] );
// INTERPOLATE TRUNK ORIENTATION:
// ------------------------------
- OrientPrw_->interpolate_trunk_orientation( Time, CurrentIndex,
+ OrientPrw_->interpolate_trunk_orientation( time, currentIndex,
m_SamplingPeriod, Solution_.SupportStates_deq,
FinalCOMTraj_deq );
// INTERPOLATE THE COMPUTED FOOT POSITIONS:
// ----------------------------------------
- Robot_->generate_trajectories( Time, Solution_,
+ Robot_->generate_trajectories( time, Solution_,
Solution_.SupportStates_deq, Solution_.SupportOrientations_deq,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq );
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
index 187f1205..cbc94b35 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
@@ -156,6 +156,15 @@ namespace PatternGeneratorJRL
/// \brief Security margin for trajectory queues
double TimeBuffer_;
+ /// \brief Additional term on the acceleration of the CoM
+ MAL_VECTOR(PerturbationAcceleration_,double);
+
+ /// \brief Sampling period considered in the QP
+ double QP_T_;
+
+ /// \brief Nb. samplings inside preview window
+ int QP_N_;
+
/// \brief 2D LIPM to simulate the evolution of the robot's CoM.
LinearizedInvertedPendulum2D CoM_;
@@ -183,14 +192,7 @@ namespace PatternGeneratorJRL
/// \brief Previewed Solution
solution_t Solution_;
- /// \brief Additional term on the acceleration of the CoM
- MAL_VECTOR(PerturbationAcceleration_,double);
-
- /// \brief Sampling period considered in the QP
- double QP_T_;
- /// \brief Nb. samplings inside preview window
- int QP_N_;
public:
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index a9a69f7d..db617301 100755
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -403,6 +403,7 @@ GeneratorVelRef::build_constraints_cop(const linear_inequality_t & IneqCoP,
compute_term ( MM_, -1.0, IneqCoP.D.Y_mat, Robot_->DynamicsCoPJerk().U );
Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, N_ );
+
// +D*V
compute_term ( MM_, 1.0, IneqCoP.D.X_mat, IntermedData_->State().V );
// + Robot_->LeftFoot().Dynamics(COP).U + Robot_->RightFoot().Dynamics(COP).U );
@@ -573,7 +574,6 @@ GeneratorVelRef::build_constraints( QPProblem & Pb, const solution_t & Solution
build_inequalities_feet( IneqFeet, Solution.SupportStates_deq );
build_constraints_feet( IneqFeet, IntermedData_->State(), nbStepsPreviewed, Pb );
-
// Polyhedric constraints:
// -----------------------
// linear_inequality_t & IneqCoM = IntermedData_->Inequalities( INEQ_COM );
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
index 9f6a8e46..723cfe28 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
@@ -92,19 +92,18 @@ namespace PatternGeneratorJRL
/// \brief Build the constant part of the objective
///
/// \param[in] Pb
- void build_invariant_part(QPProblem & Pb);
+ void build_invariant_part( QPProblem & Pb );
/// \brief Compute the objective matrices
///
/// \param[in] Pb
/// \param[in] SupportStates_deq
- void update_problem(QPProblem & Pb, const std::deque<support_state_t> & SupportStates_deq);
-
+ void update_problem( QPProblem & Pb, const std::deque<support_state_t> & SupportStates_deq );
/// \brief Compute the initial solution vector for warm start
///
/// \param[in] Solution
- void compute_warm_start(solution_t & Solution);
+ void compute_warm_start( solution_t & Solution );
/// \name Accessors
/// \{
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
index aba45303..8a8027b0 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
@@ -51,11 +51,13 @@ using namespace PatternGeneratorJRL;
-QPProblem_s::QPProblem_s():
+QPProblem::QPProblem():
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), NbConstraints_(0),NbEqConstraints_(0),
+ nbInvariantRows_(0),nbInvariantCols_(0)
+
{
NbVariables_ = 0;
NbConstraints_ = 0;
@@ -86,20 +88,20 @@ QPProblem_s::QPProblem_s():
}
-QPProblem_s::~QPProblem_s()
+QPProblem::~QPProblem()
{
release_memory();
}
void
-QPProblem_s::release_memory()
+QPProblem::release_memory()
{
}
void
-QPProblem_s::resize_all()
+QPProblem::resize_all()
{
bool ok=false;
@@ -144,7 +146,7 @@ QPProblem_s::resize_all()
void
-QPProblem_s::clear( qp_element_e Type )
+QPProblem::clear( qp_element_e Type )
{
switch(Type)
@@ -172,7 +174,8 @@ QPProblem_s::clear( qp_element_e Type )
}
-void QPProblem_s::reset()
+void
+QPProblem::reset()
{
Q_.fill(0.0);
@@ -188,8 +191,59 @@ void QPProblem_s::reset()
}
+int
+QPProblem::reset_variant()
+{
+
+ unsigned firstRow = (nbInvariantRows_ < Q_.NbRows_) ? nbInvariantRows_ : Q_.NbRows_;
+ unsigned firstCol = 0;
+ unsigned lastCol = (nbInvariantCols_ < Q_.NbCols_) ? nbInvariantCols_ : Q_.NbCols_;
+ if( (firstRow >= Q_.NbRows_) || (firstCol >= Q_.NbCols_) )
+ return 0;
+
+ double * p_it = 0;
+ for( unsigned col = firstCol; col < lastCol; col++ )
+ {
+ p_it = &Q_.Array_[firstRow + col*Q_.NbRows_];
+ for( unsigned i = firstRow; i < Q_.NbRows_; i++ )
+ {
+ *p_it = 0.0;
+ ++p_it;
+ }
+ }
+
+ firstRow = 0;
+ firstCol = (nbInvariantCols_ < Q_.NbCols_) ? nbInvariantCols_ : Q_.NbCols_;
+ lastCol = Q_.NbCols_;
+ if( (firstRow >= Q_.NbRows_) || (firstCol >= Q_.NbCols_) )
+ return 0;
+
+ for( unsigned col = firstCol; col < lastCol; col++ )
+ {
+ p_it = &Q_.Array_[firstRow + col*Q_.NbRows_];
+ for( unsigned i = firstRow; i < Q_.NbRows_; i++ )
+ {
+ *p_it = 0.0;
+ ++p_it;
+ }
+ }
+
+ Q_dense_.fill(0.0);
+ DU_.fill(0.0);
+ DU_dense_.fill(0.0);
+ D_.fill(0.0);
+ DS_.fill(0.0);
+ NbConstraints_ = 0;
+ NbEqConstraints_ = 0;
+ NbVariables_ = 0;
+
+ return 0;
+
+}
+
+
void
-QPProblem_s::solve( solver_e Solver, solution_t & Result, const tests_e & tests )
+QPProblem::solve( solver_e Solver, solution_t & Result, const tests_e & tests )
{
m_ = NbConstraints_+1;
@@ -354,13 +408,10 @@ QPProblem_s::solve( solver_e Solver, solution_t & Result, const tests_e & tests
void
-QPProblem_s::add_term_to( qp_element_e Type, const MAL_MATRIX (&Mat, double),
+QPProblem::add_term_to( qp_element_e Type, const MAL_MATRIX (&Mat, double),
unsigned int row, unsigned int col )
{
- struct timeval start,end,start1,end1,start2,end2,start3,end3,start4,end4;
- double CurrentCPUTime=0.0;
-
array_s<double> * Array_p = 0;
switch(Type)
@@ -372,8 +423,8 @@ QPProblem_s::add_term_to( qp_element_e Type, const MAL_MATRIX (&Mat, double),
case MATRIX_DU:
Array_p = &DU_;
- NbConstraints_ = (row+Mat.size1()>NbConstraints_) ? row+Mat.size1() : NbConstraints_;
- NbVariables_ = (col+Mat.size2()>NbVariables_) ? col+Mat.size2() : NbVariables_;
+ 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;
@@ -391,36 +442,24 @@ QPProblem_s::add_term_to( qp_element_e Type, const MAL_MATRIX (&Mat, double),
}
- if (NbVariables_ > Array_p->NbCols_ )
- {
+ if( NbVariables_ > Array_p->NbCols_ )
resize_all();
- }
- if ((NbConstraints_ > DU_.NbRows_) &&
- (NbConstraints_>0))
+
+ if( (NbConstraints_ > DU_.NbRows_) && (NbConstraints_>0) )
{
- if (NbVariables_>0)
- {
- DU_.resize(2*NbConstraints_, 2*NbVariables_,true);
- }
+ if( NbVariables_>0 )
+ DU_.resize( 2*NbConstraints_, 2*NbVariables_, true );
}
- if ((NbConstraints_ > DS_.NbRows_ ) &&
- (NbConstraints_>0))
- {
+ if( (NbConstraints_ > DS_.NbRows_ ) && (NbConstraints_>0) )
DS_.resize(2*NbConstraints_,1,true);
- }
- unsigned usize = NbConstraints_+2*NbVariables_;
- if(usize>U_.NbRows_)
- {
- U_.resize(usize, 1,true);
- }
+ if( U_.NbRows_ < NbConstraints_+2*NbVariables_ )
+ U_.resize( NbConstraints_+2*NbVariables_, 1, true );
unsigned warsize = 3*NbVariables_*NbVariables_/2+10*NbVariables_+2*(NbConstraints_+1)+20000;
- if (warsize> war_.NbRows_)
- {
- war_.resize(warsize, 1,true);
- }
+ if( warsize> war_.NbRows_ )
+ war_.resize( warsize, 1, true );
double * p = Array_p->Array_;
boost_ublas::matrix<double>::const_iterator1 row_it = Mat.begin1();
@@ -444,7 +483,7 @@ QPProblem_s::add_term_to( qp_element_e Type, const MAL_MATRIX (&Mat, double),
void
-QPProblem_s::add_term_to( qp_element_e Type, const MAL_VECTOR (&Vec, double),
+QPProblem::add_term_to( qp_element_e Type, const MAL_VECTOR (&Vec, double),
unsigned row, unsigned col )
{
@@ -481,10 +520,8 @@ QPProblem_s::add_term_to( qp_element_e Type, const MAL_VECTOR (&Vec, double),
break;
}
- if((NbVariables_ > D_.NbRows_ ) && (NbVariables_>0))
- {
+ if( (NbVariables_ > D_.NbRows_ ) && (NbVariables_>0) )
resize_all();
- }
boost_ublas::vector<double>::const_iterator vec_it = Vec.begin();
double * p_it = &Array_p->Array_[row+col*Array_p->NbRows_];
@@ -511,7 +548,7 @@ QPProblem_s::add_term_to( qp_element_e Type, const MAL_VECTOR (&Vec, double),
void
-QPProblem_s::dump_solver_parameters(std::ostream & aos)
+QPProblem::dump_solver_parameters(std::ostream & aos)
{
aos << "m: " << m_ << std::endl
<< "me: " << me_ << std::endl
@@ -528,7 +565,7 @@ QPProblem_s::dump_solver_parameters(std::ostream & aos)
void
-QPProblem_s::dump( qp_element_e Type, std::ostream & aos)
+QPProblem::dump( qp_element_e Type, std::ostream & aos)
{
unsigned int NbRows=0, NbCols=0;
@@ -591,7 +628,7 @@ QPProblem_s::dump( qp_element_e Type, std::ostream & aos)
void
-QPProblem_s::dump( qp_element_e Type, const char * FileName )
+QPProblem::dump( qp_element_e Type, const char * FileName )
{
std::ofstream aof;
aof.open(FileName,std::ofstream::out);
@@ -601,7 +638,7 @@ QPProblem_s::dump( qp_element_e Type, const char * FileName )
void
-QPProblem_s::dump_problem( std::ostream &aos )
+QPProblem::dump_problem( std::ostream &aos )
{
dump(MATRIX_Q,aos);
dump(VECTOR_D,aos);
@@ -617,7 +654,7 @@ QPProblem_s::dump_problem( std::ostream &aos )
void
-QPProblem_s::dump( const char * FileName )
+QPProblem::dump( const char * FileName )
{
std::ofstream aof;
aof.open(FileName,std::ofstream::out);
@@ -627,7 +664,7 @@ QPProblem_s::dump( const char * FileName )
void
-QPProblem_s::dump( double Time )
+QPProblem::dump( double Time )
{
char Buffer[1024];
sprintf(Buffer, "/tmp/Problem_%f.dat", Time);
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.hh b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
index fa82d78e..f6158e24 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.hh
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
@@ -42,7 +42,7 @@ namespace PatternGeneratorJRL
/// \brief Final optimization problem.
/// Store and solve a quadratic problem with linear constraints.
///
- struct QPProblem_s
+ class QPProblem
{
@@ -51,11 +51,9 @@ namespace PatternGeneratorJRL
//
public:
- /// \brief Initialize by default an empty problem.
- QPProblem_s();
+ QPProblem();
- /// \brief Release the memory at the end only.
- ~QPProblem_s();
+ ~QPProblem();
/// \brief Reallocate array
///
@@ -98,9 +96,12 @@ namespace PatternGeneratorJRL
/// \param[in] type
void clear( qp_element_e Type );
- /// \brief Set matrices to zero
+ /// \brief Set all qp elements to zero
void reset();
+ /// \brief Set variant elements to zero
+ int reset_variant();
+
/// \brief Solve the optimization problem
///
/// \param[in] Solver
@@ -124,6 +125,16 @@ namespace PatternGeneratorJRL
{ NbConstraints_ = NbConstraints;};
inline unsigned int NbConstraints()
{ return NbConstraints_;};
+
+ inline void nbInvariantRows( unsigned int nbInvariantRows )
+ { nbInvariantRows_ = nbInvariantRows;};
+ inline unsigned int nbInvariantRows()
+ { return nbInvariantRows_;};
+
+ inline void nbInvariantCols( unsigned int nbInvariantCols )
+ { nbInvariantCols_ = nbInvariantCols;};
+ inline unsigned int nbInvariantCols()
+ { return nbInvariantCols_;};
/// \}
//
@@ -300,8 +311,12 @@ namespace PatternGeneratorJRL
/// \brief Number of equality constraints
unsigned int NbEqConstraints_;
+
+ /// \brief First row and column of variant Hessian part
+ unsigned nbInvariantRows_, nbInvariantCols_;
+
};
- typedef struct QPProblem_s QPProblem;
+
}
#include <ZMPRefTrajectoryGeneration/qp-problem.hxx>
#endif /* _QP_PROBLEM_H_ */
diff --git a/src/privatepgtypes.h b/src/privatepgtypes.h
index 1ec76980..db2290b0 100644
--- a/src/privatepgtypes.h
+++ b/src/privatepgtypes.h
@@ -38,6 +38,10 @@
namespace PatternGeneratorJRL
{
+ //
+ // Enum types
+ //
+
/// \name Enum types
/// \{
enum foot_type_e
@@ -96,6 +100,12 @@ namespace PatternGeneratorJRL
};
/// \}
+ //
+ // Structures
+ //
+
+ /// \name Structures
+ /// \{
/// \brief State of the center of mass
struct com_t
{
@@ -343,6 +353,8 @@ namespace PatternGeneratorJRL
};
+ /// \}
+
}
#endif /* _PATTERN_GENERATOR_INTERNAL_PRIVATE_H_ */
--
GitLab