From 96c3a57d709bcc3d03c9dfcb3a5555204a1eda03 Mon Sep 17 00:00:00 2001
From: Andrei <andrei@checkmoruk.local>
Date: Fri, 3 Jun 2011 00:16:18 +0200
Subject: [PATCH] Smaller refactoring
---
.../ZMPVelocityReferencedQP.cpp | 16 ++++++++--------
.../generator-vel-ref.cpp | 17 +++++++++--------
2 files changed, 17 insertions(+), 16 deletions(-)
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index c5852b8e..f59c3cb9 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -293,7 +293,7 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
void
-ZMPVelocityReferencedQP::OnLine(double time,
+ZMPVelocityReferencedQP::OnLine(double Time,
deque<ZMPPosition> & FinalZMPTraj_deq,
deque<COMState> & FinalCOMTraj_deq,
deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
@@ -306,7 +306,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
// Testing if we are reaching the end of the online mode.
if ((EndingPhase_) &&
- (time>=TimeToStopOnLineMode_))
+ (Time>=TimeToStopOnLineMode_))
{ m_OnLineMode = false; }
@@ -322,7 +322,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
// UPDATE WALKING TRAJECTORIES:
// ----------------------------
- if(time + 0.00001 > UpperTimeLimitToUpdate_)
+ if(Time + 0.00001 > UpperTimeLimitToUpdate_)
{
double TotalAmountOfCPUTime=0.0,CurrentCPUTime=0.0;
struct timeval start,end;
@@ -332,7 +332,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
// UPDATE INTERNAL DATA:
// ---------------------
VRQPGenerator_->Reference(VelRef_);
- VRQPGenerator_->CurrentTime(time+TimeBuffer_);
+ VRQPGenerator_->CurrentTime(Time+TimeBuffer_);
VRQPGenerator_->CoM(CoM_());
@@ -364,7 +364,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
// COMPUTE ORIENTATIONS OF FEET FOR WHOLE PREVIEW PERIOD:
// ------------------------------------------------------
deque<double> PreviewedSupportAngles_deq;
- OrientPrw_->preview_orientations(time+TimeBuffer_,
+ OrientPrw_->preview_orientations(Time+TimeBuffer_,
VelRef_,
SupportFSM_->StepPeriod(), CurrentSupport,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
@@ -418,7 +418,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
// COMPUTE ORIENTATION OF TRUNK:
// -----------------------------
- OrientPrw_->interpolate_trunk_orientation(time+TimeBuffer_, CurrentIndex,
+ OrientPrw_->interpolate_trunk_orientation(Time+TimeBuffer_, CurrentIndex,
m_SamplingPeriod,
CurrentSupport,
FinalCOMTraj_deq);
@@ -426,8 +426,8 @@ ZMPVelocityReferencedQP::OnLine(double time,
// INTERPOLATE THE COMPUTED FEET POSITIONS:
// ----------------------------------------
- unsigned NumberStepsPrwd = PrwSupportStates_deq.back().StepNumber;
- OFTG_->interpolate_feet_positions(time+TimeBuffer_,
+ unsigned int NumberStepsPrwd = PrwSupportStates_deq.back().StepNumber;
+ OFTG_->interpolate_feet_positions(Time+TimeBuffer_,
CurrentIndex, CurrentSupport,
Result.Solution_vec[2*QP_N_], Result.Solution_vec[2*QP_N_+NumberStepsPrwd],
PreviewedSupportAngles_deq,
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index 3e957586..233cb497 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -192,6 +192,7 @@ GeneratorVelRef::initialize_matrices()
void
GeneratorVelRef::initialize_matrices( linear_inequality_t & Inequalities)
{
+
switch(Inequalities.type)
{
case IntermedQPMat::INEQ_COP:
@@ -285,10 +286,10 @@ GeneratorVelRef::build_inequalities_cop(linear_inequality_t & Inequalities,
//set constraints for the whole preview window
double SupportAngle = CurrentSupportAngle;
- const int nEdges = 4;
- double D_x[nEdges] = {0.0, 0.0, 0.0, 0.0};
- double D_y[nEdges] = {0.0, 0.0, 0.0, 0.0};
- double dc[nEdges] = {0.0, 0.0, 0.0, 0.0};
+ const int NbEdges = 4;
+ double D_x[NbEdges] = {0.0, 0.0, 0.0, 0.0};
+ double D_y[NbEdges] = {0.0, 0.0, 0.0, 0.0};
+ double dc[NbEdges] = {0.0, 0.0, 0.0, 0.0};
for( int i=1;i<=N_;i++ )
{
const support_state_t & PrwSupport = SupportStates_deq[i];
@@ -304,11 +305,11 @@ GeneratorVelRef::build_inequalities_cop(linear_inequality_t & Inequalities,
RFI->compute_linear_system( ZMPFeasibilityEdges, D_x, D_y, dc, PrwSupport );
- for(int j = 0;j < nEdges; j++)
+ for(int j = 0;j < NbEdges; j++)
{
- Inequalities.D.x.push_back((i-1)*nEdges+j,i-1,D_x[j]);
- Inequalities.D.y.push_back((i-1)*nEdges+j,i-1,D_y[j]);
- Inequalities.dc((i-1)*nEdges+j) = dc[j];
+ Inequalities.D.x.push_back((i-1)*NbEdges+j,i-1,D_x[j]);
+ Inequalities.D.y.push_back((i-1)*NbEdges+j,i-1,D_y[j]);
+ Inequalities.dc((i-1)*NbEdges+j) = dc[j];
}
}
--
GitLab