From b2e20e8fdf01b5e24ba69e55a1e3bb2dbe2e44a4 Mon Sep 17 00:00:00 2001
From: Huynh Duc <huynhnduc100@gmail.com>
Date: Tue, 24 Jun 2014 15:14:39 +0200
Subject: [PATCH] Clean code 24/06/2014
---
...dRightFootTrajectoryGenerationMultiple.cpp | 22 +------
src/StepStackHandler.cpp | 7 ---
src/StepStackHandler.hh | 1 -
.../AnalyticalMorisawaCompact.cpp | 61 +++++--------------
.../AnalyticalMorisawaCompact.hh | 3 +-
tests/TestMorisawa2007.cpp | 7 +++
6 files changed, 25 insertions(+), 76 deletions(-)
diff --git a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
index 44f7d482..3d9b9fff 100644
--- a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
+++ b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
@@ -125,21 +125,12 @@ void LeftAndRightFootTrajectoryGenerationMultiple::SetAnInterval(unsigned int In
FootInitialPosition.y,
FootInitialPosition.dy);
- if (FootFinalPosition.z < FootInitialPosition.z )
aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
FootTrajectoryGenerationStandard::Z_AXIS,
m_DeltaTj[IntervalIndex],
FootFinalPosition.z,
FootInitialPosition.z,
FootInitialPosition.dz);
- else
- aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
- FootTrajectoryGenerationStandard::Z_AXIS,
- m_DeltaTj[IntervalIndex],
- FootFinalPosition.z,
- FootInitialPosition.z,
- FootInitialPosition.dz);
-
// THETA
aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
@@ -165,7 +156,6 @@ void LeftAndRightFootTrajectoryGenerationMultiple::SetAnInterval(unsigned int In
FootInitialPosition.omega2,
FootInitialPosition.domega2);
- // Init the first interval.
// X axis.
if (FootFinalPosition.z <= FootInitialPosition.z ) //down
aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
@@ -181,13 +171,6 @@ void LeftAndRightFootTrajectoryGenerationMultiple::SetAnInterval(unsigned int In
FootFinalPosition.x,
FootInitialPosition.x,
FootInitialPosition.dx);
- /* else if (FootFinalPosition.z - FootInitialPosition.z == m_StepHeight) //normal walking
- aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
- FootTrajectoryGenerationStandard::X_AXIS,
- m_DeltaTj[IntervalIndex],
- FootFinalPosition.x,
- FootInitialPosition.x,
- FootInitialPosition.dx);*/
}
void LeftAndRightFootTrajectoryGenerationMultiple::
@@ -966,8 +949,7 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
LeftFootTmpFinalPos.dz = 0;
LeftFootTmpFinalPos.stepType = -1;
- //cout << "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA " << LeftFootTmpFinalPos.z << endl;
- SetAnInterval(IntervalIndex,m_LeftFootTrajectory,
+ SetAnInterval(IntervalIndex,m_LeftFootTrajectory,
LeftFootTmpFinalPos,
LeftFootTmpFinalPos);
RightFootTmpFinalPos.z = CurrentSupportFootPosition(2,2);
@@ -991,8 +973,6 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
SupportFootAbsoluteFootPositions[i].y = CurrentSupportFootPosition(1,2);
SupportFootAbsoluteFootPositions[i].z = CurrentSupportFootPosition(2,2);
SupportFootAbsoluteFootPositions[i].theta = CurrentAbsTheta;
-cout << "x "<< SupportFootAbsoluteFootPositions[i].x << "y " << SupportFootAbsoluteFootPositions[i].y << "z " << SupportFootAbsoluteFootPositions[i].z << endl;
-
if ((!IgnoreFirst) || (i>0))
SupportFoot=-SupportFoot;
diff --git a/src/StepStackHandler.cpp b/src/StepStackHandler.cpp
index 97f7a2db..225f0184 100644
--- a/src/StepStackHandler.cpp
+++ b/src/StepStackHandler.cpp
@@ -162,13 +162,6 @@ void StepStackHandler::ReadStepStairSequenceAccordingToWalkMode(istringstream &s
aFootPosition.DeviationHipHeight << " " ,
"DebugGMFKW.dat");
- cout << "TESTTTTTTTTTT "<<aFootPosition.sx << " " <<
- aFootPosition.sy << " " <<
- aFootPosition.sz << " " <<
- aFootPosition.theta << " " <<
- aFootPosition.SStime << " " <<
- aFootPosition.DStime << " " <<
- aFootPosition.DeviationHipHeight << endl;
m_RelativeFootPositions.push_back(aFootPosition);
if (aFootPosition.sy>0)
m_KeepLastCorrectSupportFoot=-1;
diff --git a/src/StepStackHandler.hh b/src/StepStackHandler.hh
index e2317a77..c7071f50 100644
--- a/src/StepStackHandler.hh
+++ b/src/StepStackHandler.hh
@@ -67,7 +67,6 @@ namespace PatternGeneratorJRL
2: Stepping over.
3: Read a path.
4: Freeze the upper body.
- 6: Climbing stairs
*/
void SetWalkMode(int lWalkMode);
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index 80191bb6..98971023 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -435,18 +435,7 @@ namespace PatternGeneratorJRL
{
m_FeetTrajectoryGenerator->SetDeltaTj(m_DeltaTj);
- int isStepStairOn;
-
- GetIsStepStairOn(isStepStairOn);
-
- if (m_isStepStairOn == 1)
- m_FeetTrajectoryGenerator->InitializeFromRelativeSteps(m_RelativeFootPositions,
- LeftFootInitialPosition,
- RightFootInitialPosition,
- m_AbsoluteSupportFootPositions,
- IgnoreFirstRelativeFoot, false);
- else
- m_FeetTrajectoryGenerator->InitializeFromRelativeSteps_backup(m_RelativeFootPositions,
+ m_FeetTrajectoryGenerator->InitializeFromRelativeSteps(m_RelativeFootPositions,
LeftFootInitialPosition,
RightFootInitialPosition,
m_AbsoluteSupportFootPositions,
@@ -2250,6 +2239,7 @@ namespace PatternGeneratorJRL
deque<FootAbsolutePosition> & FinalRightFootAbsolutePositions)
{
unsigned int lIndexInterval,lPrevIndexInterval;
+ double static preCoMz =0;
m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(m_AbsoluteTimeReference,lIndexInterval);
lPrevIndexInterval = lIndexInterval;
@@ -2296,23 +2286,15 @@ namespace PatternGeneratorJRL
{ LTHROW("COM out of bound along Y axis.");}
m_AnalyticalZMPCoGTrajectoryY->ComputeCOMSpeed(t,aCOMPos.y[1],lIndexInterval);
+ ComputeCoMz(t,aCOMPos.z[0]);
+ aCOMPos.z[1] = (aCOMPos.z[0] - preCoMz) / m_SamplingPeriod;
+ preCoMz = aCOMPos.z[0];
- /* if (RightFootAbsPos.z <= LeftFootAbsPos.z){
- aCOMPos.z[1] = (m_InitialPoseCoMHeight + RightFootAbsPos.z - aCOMPos.z[0])/m_SamplingPeriod;
- aCOMPos.z[0] = m_InitialPoseCoMHeight + RightFootAbsPos.z;
- }
- else{
- aCOMPos.z[1] = (m_InitialPoseCoMHeight + LeftFootAbsPos.z - aCOMPos.z[0])/m_SamplingPeriod;
- aCOMPos.z[0] = m_InitialPoseCoMHeight + LeftFootAbsPos.z;
- }*/
-
-
- ComputeCoMz(t,aCOMPos.z[0], aCOMPos.z[1]);
FinalCoMPositions.push_back(aCOMPos);
ODEBUG4(aZMPPos.px << " " << aZMPPos.py << " " <<
- aCOMPos.x[0] << " " << aCOMPos.y[0] << " " <<
+ aCOMPos.x[0] << " " << aCOMPos.y[0] << " " << aCOMPos.z[0] <<" "<<
LeftFootAbsPos.x << " " << LeftFootAbsPos.y << " " << LeftFootAbsPos.z << " " <<
RightFootAbsPos.x << " " << RightFootAbsPos.y << " " << RightFootAbsPos.z << " " <<
m_SamplingPeriod,"Test.dat");
@@ -2320,13 +2302,13 @@ namespace PatternGeneratorJRL
}
- void AnalyticalMorisawaCompact::ComputeCoMz(double t, double &CoMz, double &CoMvelocity)
+ void AnalyticalMorisawaCompact::ComputeCoMz(double t, double &CoMz)
{
int Index,Index2;
double moving_time = m_RelativeFootPositions[0].SStime + m_RelativeFootPositions[0].DStime;
double deltaZ;
- double CoMzpre = CoMz;
+ // double static CoMzpre = CoMz;
double variable=0.1,variableright = 0.9 ,variableleft = 0.0;
double variableright1 = 0.9 ,variableleft1 = 0.0;
// put first leg on the stairs with decrease of CoM variable% of step height
@@ -2362,8 +2344,8 @@ namespace PatternGeneratorJRL
CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2].z;
}
- // going down et normal walk
- if (m_AbsoluteSupportFootPositions[Index2].z < m_AbsoluteSupportFootPositions[Index2-1].z && m_RelativeFootPositions[Index2].sz < 0)
+ // going down
+ else if (m_AbsoluteSupportFootPositions[Index2].z < m_AbsoluteSupportFootPositions[Index2-1].z && m_RelativeFootPositions[Index2].sz < 0)
{
deltaZ = (m_AbsoluteSupportFootPositions[Index2].z - m_AbsoluteSupportFootPositions[Index2-1].z );
if (t <= Index2*moving_time + 0.9*m_RelativeFootPositions[Index2].SStime && t >= Index2*moving_time + 0.0*m_RelativeFootPositions[Index2].SStime)
@@ -2381,30 +2363,19 @@ namespace PatternGeneratorJRL
else
CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2].z ;
}
+ else // normal walking
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2].z;
}
- else
+ else //after final step
CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions.back().z;
}
- else
+ else //first step
CoMz = m_InitialPoseCoMHeight ;
- CoMvelocity = (CoMz -CoMzpre)/m_SamplingPeriod;
+ // CoMvelocity = (CoMz - CoMzpre)/m_SamplingPeriod;
+ // cout << "v" << CoMz <<" "<<CoMzpre << endl;
}
}
- /*Index = int((t-moving_time)/(moving_time*2));
- if (Index < (m_AbsoluteSupportFootPositions.size() - 1)/2){
- deltaZ = (m_AbsoluteSupportFootPositions[2*Index + 1].z - m_AbsoluteSupportFootPositions[2*Index].z );
- if (deltaZ > 0 && t>=(2*Index+2)*moving_time && t<=(2*Index+2)*moving_time + m_RelativeFootPositions[2*Index+1].SStime)
- CoMz = t*deltaZ/m_RelativeFootPositions[0].SStime + m_InitialPoseCoMHeight - (2*Index + 2)*moving_time*deltaZ/m_RelativeFootPositions[0].SStime + m_AbsoluteSupportFootPositions[2*Index].z ;
- else if (deltaZ > 0 && t>(2*Index+2)*moving_time + m_RelativeFootPositions[2*Index+1].SStime)
- CoMz = CoMz + m_AbsoluteSupportFootPositions[2*Index].z;
- if (deltaZ < 0 && t>=(2*Index+1)*moving_time && t<=(2*Index+2)*moving_time )
- CoMz = t*deltaZ/moving_time + m_InitialPoseCoMHeight - (2*Index + 1)*moving_time*deltaZ/moving_time + m_AbsoluteSupportFootPositions[2*Index].z ;
- else if (deltaZ > 0 && t>(2*Index+2)*moving_time + m_RelativeFootPositions[2*Index+1].SStime)
- CoMz = CoMz + m_AbsoluteSupportFootPositions[2*Index].z;
-
- }*/
-
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh
index a5ced615..534b82c4 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh
@@ -555,8 +555,7 @@ namespace PatternGeneratorJRL
deque<FootAbsolutePosition> & FinalLeftFootAbsolutePositions,
deque<FootAbsolutePosition> & FinalRightFootAbsolutePositions);
-
- void ComputeCoMz(double t, double &CoMz, double &CoMvelocity);
+ void ComputeCoMz(double t, double &CoMz);
/*! \brief LU decomposition of the Z matrix. */
MAL_MATRIX_TYPE(double) m_AF;
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index 79984529..d03bae40 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -504,6 +504,13 @@ protected:
0.0 -0.19 0.0 0.0\
0.32 0.19 -0.15 0.0\
0.0 -0.19 0.0 0.0\
+ 0.2 0.19 0.0 0.0\
+ 0.2 -0.19 0.0 0.0\
+ 0.0 0.19 0.0 0.0\
+ 0.3 -0.19 0.15 0.0\
+ 0.0 0.19 0.0 0.0\
+ 0.3 -0.19 0.15 0.0\
+ 0.0 0.19 0.0 0.0\
");
aPGI.ParseCmd(strm2);
/*0.0 -0.105 0.0 0.0\
--
GitLab