diff --git a/src/PreviewControl/PreviewControl.cpp b/src/PreviewControl/PreviewControl.cpp
index 2c16ff028aeab9aa913bd0da3d4376177cc501f0..e6b860fbaa2602c8bca85283a7a63f34b8a815be 100644
--- a/src/PreviewControl/PreviewControl.cpp
+++ b/src/PreviewControl/PreviewControl.cpp
@@ -28,7 +28,7 @@
*/
#include <fstream>
-//#define _DEBUG_MODE_ON_
+#define _DEBUG_MODE_ON_
#include <Debug.hh>
#include <PreviewControl/PreviewControl.hh>
@@ -342,7 +342,7 @@ int PreviewControl::OneIterationOfPreview(MAL_MATRIX( &x, double),
{
LTHROW("ZMPPositions.size()<m_SizeOfPreviewWindow:" );
}
-
+ //cout << "Size preview = "<< ZMPPositions.size() << endl ;
for(unsigned int i=0;i<m_SizeOfPreviewWindow;i++)
ux += m_F(i,0)* ZMPPositions[lindex+i].px;
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index 3f20a4be7be38c8144f62cd3e55b6c222a9b1fe7..c5f5f475a1a5ea460fac895145af5d32cb6bd7bd 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -569,27 +569,37 @@ It also creates the analytical feet trajectories.
m_AnalyticalZMPCoGTrajectoryY->SetAbsoluteTimeReference(m_AbsoluteTimeReference);
m_FeetTrajectoryGenerator->SetAbsoluteTimeReference(m_AbsoluteTimeReference);
- m_kajitaDynamicFilter->init( m_CurrentTime, m_SamplingPeriod, m_SamplingPeriod, m_SamplingPeriod, 1.6,
- lStartingCOMState.z[0], InitLeftFootAbsolutePosition, lStartingCOMState );
+ double KajitaPCpreviewWindow = 1.6 ;
+ m_kajitaDynamicFilter->init( m_CurrentTime,
+ m_SamplingPeriod,
+ m_SamplingPeriod,
+ m_PreviewControlTime+2*m_SamplingPeriod-TimeShift,
+ KajitaPCpreviewWindow,
+ lStartingCOMState.z[0],
+ InitLeftFootAbsolutePosition,
+ lStartingCOMState );
/*! Compute the total size of the array related to the steps. */
FillQueues(m_CurrentTime,m_CurrentTime+m_PreviewControlTime-TimeShift,
ZMPPositions, COMStates,LeftFootAbsolutePositions, RightFootAbsolutePositions);
+ deque<COMState> filteredCoM = COMStates ;
- /*! Add 1.6 second to the buffers for fitering */
- unsigned int N = ZMPPositions.size();
+ /*! Add KajitaPCpreviewWindow second to the buffers for fitering */
+ unsigned int n = ZMPPositions.size();
ZMPPosition lastZMP = ZMPPositions.back();
COMState lastCoM = COMStates.back();
FootAbsolutePosition lastLF = LeftFootAbsolutePositions.back();
FootAbsolutePosition lastRF = RightFootAbsolutePositions.back();
- for (unsigned int i = 0 ; i < (m_kajitaDynamicFilter->getPreviewWindowSize_()/m_SamplingPeriod) ; ++i)
+ for (unsigned int i = 0 ; i < KajitaPCpreviewWindow/m_SamplingPeriod ; ++i)
{
ZMPPositions.push_back(lastZMP);
COMStates.push_back(lastCoM);
LeftFootAbsolutePositions.push_back(lastLF);
RightFootAbsolutePositions.push_back(lastRF);
}
-
+ unsigned int N = ZMPPositions.size();
+ int stage0 = 0 ;
+ int stage1 = 1 ;
vector <vector<double> > ZMPMB (N , vector<double> (2,0.0)) ;
for (unsigned int i = 0 ; i < N ; ++i)
{
@@ -598,30 +608,33 @@ It also creates the analytical feet trajectories.
ZMPMB[i] , stage0 , i);
}
deque<ZMPPosition> inputdeltaZMP_deq(N) ;
- deque<COMState> outputDeltaCOMTraj_deq_ ;
+ deque<COMState> outputDeltaCOMTraj_deq ;
for (unsigned int i = 0 ; i < N ; ++i)
{
inputdeltaZMP_deq[i].px = ZMPPositions[i].px - ZMPMB[i][0] ;
inputdeltaZMP_deq[i].py = ZMPPositions[i].py - ZMPMB[i][1] ;
inputdeltaZMP_deq[i].pz = 0.0 ;
inputdeltaZMP_deq[i].theta = 0.0 ;
- inputdeltaZMP_deq[i].time = m_CurrentTime + i * m_kajitaDynamicFilter->getInterpolationPeriod() ;
+ inputdeltaZMP_deq[i].time = m_CurrentTime + i * m_SamplingPeriod ;
inputdeltaZMP_deq[i].stepType = ZMPPositions[i].stepType ;
}
- m_kajitaDynamicFilter->OptimalControl(inputdeltaZMP_deq,outputDeltaCOMTraj_deq_) ;
-
- deque<COMState> filteredCoM = COMPos_deq ;
- vector <vector<double> > filteredZMPMB (COMPos_deq.size() , vector<double> (2,0.0)) ;
- for (unsigned int i = 0 ; i < filteredCoM.size() ; ++i)
+ int OptimalControlError = m_kajitaDynamicFilter->OptimalControl(inputdeltaZMP_deq,outputDeltaCOMTraj_deq) ;
+ cout << "OptimalControlError = " << OptimalControlError << endl ;
+ cout << "outputDeltaCOMTraj_deq.size() = " << outputDeltaCOMTraj_deq.size() << endl ;
+ cout << "n = " << n << endl ;
+ cout << "N = " << N << endl ;
+ vector <vector<double> > filteredZMPMB (n , vector<double> (2,0.0)) ;
+ for (unsigned int i = 0 ; i < n ; ++i)
{
+ outputDeltaCOMTraj_deq[1600].x[0] ;
for(int j=0;j<3;j++)
{
- COMPos_deq[i].x[j] += outputDeltaCOMTraj_deq_[0].x[j] ;
- COMPos_deq[i].y[j] += outputDeltaCOMTraj_deq_[0].y[j] ;
+ filteredCoM[i].x[j] += outputDeltaCOMTraj_deq[i].x[j] ;
+ filteredCoM[i].y[j] += outputDeltaCOMTraj_deq[i].y[j] ;
}
- m_kajitaDynamicFilter->ComputeZMPMB( m_SamplingPeriod, aCOMState,
- LeftFootAbsPos[0], RightFootAbsPos[0],
- ZMPMBcorrige , stage2, iteration);
+ m_kajitaDynamicFilter->ComputeZMPMB(m_SamplingPeriod, filteredCoM[i],
+ LeftFootAbsolutePositions[i], RightFootAbsolutePositions[i],
+ filteredZMPMB[i] , stage1, i);
}
m_UpperTimeLimitToUpdateStacks = m_CurrentTime;
@@ -629,78 +642,17 @@ It also creates the analytical feet trajectories.
{
m_UpperTimeLimitToUpdateStacks += m_DeltaTj[i];
}
-
-
- std::deque<ZMPPosition> ZMPPos_deq ;
- std::deque<COMState> COMPos_deq ;
- std::deque<FootAbsolutePosition> LeftFootAbsPos ;
- std::deque<FootAbsolutePosition> RightFootAbsPos ;
-
- FillQueues(m_kajitaDynamicFilter->getInterpolationPeriod(),
- m_CurrentTime, m_CurrentTime + m_kajitaDynamicFilter->getPreviewWindowSize_(),
- ZMPPos_deq, COMPos_deq, LeftFootAbsPos, RightFootAbsPos);
-
- static int iteration = 0;
- int stage0 = 0 ;
- int stage1 = 1 ;
- int stage2 = 2 ;
- vector<double> ZMPMBcontrol (2,0.0);
- m_kajitaDynamicFilter->stage0INstage1();
- m_kajitaDynamicFilter->ComputeZMPMB( m_SamplingPeriod, COMPos_deq[0],
- LeftFootAbsPos[0], RightFootAbsPos[0],
- ZMPMBcontrol , stage0, iteration);
-
- for (unsigned int i = 0 ; i < COMPos_deq.size() ; ++i)
- {
- vector<double> tmpZMPMB (2,0.0);
- m_kajitaDynamicFilter->ComputeZMPMB( m_SamplingPeriod, COMPos_deq[i],
- LeftFootAbsPos[i], RightFootAbsPos[i],
- tmpZMPMB , stage1 , i+iteration);
- ZMPMB.push_back(tmpZMPMB);
- }
-
-
-
- for (unsigned int i = 0 ; i < ZMPMB.size() ; ++i)
- {
- inputdeltaZMP_deq[i].px = ZMPPos_deq[i].px - ZMPMB[i][0] ;
- inputdeltaZMP_deq[i].py = ZMPPos_deq[i].py - ZMPMB[i][1] ;
- inputdeltaZMP_deq[i].pz = 0.0 ;
- inputdeltaZMP_deq[i].theta = 0.0 ;
- inputdeltaZMP_deq[i].time = m_CurrentTime + i * m_kajitaDynamicFilter->getInterpolationPeriod() ;
- inputdeltaZMP_deq[i].stepType = ZMPPos_deq[i].stepType ;
- }
- m_kajitaDynamicFilter->OptimalControl(inputdeltaZMP_deq,outputDeltaCOMTraj_deq_) ;
-
- COMState aCOMState = COMPos_deq[0] ;
- for(int j=0;j<3;j++)
- {
- aCOMState.x[j] += outputDeltaCOMTraj_deq_[0].x[j] ;
- aCOMState.y[j] += outputDeltaCOMTraj_deq_[0].y[j] ;
- }
- vector<double> ZMPMBcorrige (2,0.0);
- m_kajitaDynamicFilter->ComputeZMPMB( m_SamplingPeriod, aCOMState,
- LeftFootAbsPos[0], RightFootAbsPos[0],
- ZMPMBcorrige , stage2, iteration);
-
-
-
-
/// \brief Debug Purpose
/// --------------------
ofstream aof;
string aFileName;
ostringstream oss(std::ostringstream::ate);
- int iteration1000 = (int)iteration/1000;
- int iteration100 = (int)(iteration - iteration1000*1000)/100;
- int iteration10 = (int)(iteration - iteration1000*1000 - iteration100*100)/10;
- int iteration1 = (int)(iteration - iteration1000*1000 - iteration100*100 - iteration10*10 )/1;
-
+ static int iteration = 0;
+ cout << "iteration = " << iteration ;
/// \brief Debug Purpose
/// --------------------
- oss.str("ZMPMBbuffer/ZMPMBbuffer");
- oss << "_" << iteration1000 << iteration100 << iteration10 << iteration1 << ".dat";
+ oss.str("ZMPDiscretisationBuffer.dat");
aFileName = oss.str();
aof.open(aFileName.c_str(),ofstream::out);
aof.close();
@@ -708,153 +660,75 @@ It also creates the analytical feet trajectories.
aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
- for (unsigned int i = 0 ; i < COMPos_deq.size() ; ++i )
- {
- aof << i*m_SamplingPeriod << " " // 1
- << COMPos_deq[i].x[0] << " " // 2
- << COMPos_deq[i].x[1] << " " // 3
- << COMPos_deq[i].x[2] << " " // 4
- << COMPos_deq[i].y[0] << " " // 5
- << COMPos_deq[i].y[1] << " " // 6
- << COMPos_deq[i].y[2] << " " // 7
- << COMPos_deq[i].z[0] << " " // 8
- << COMPos_deq[i].z[1] << " " // 9
- << COMPos_deq[i].z[2] << " " // 10
- << COMPos_deq[i].roll[0] << " " // 11
- << COMPos_deq[i].roll[1] << " " // 12
- << COMPos_deq[i].roll[2] << " " // 13
- << COMPos_deq[i].pitch[0] << " " // 14
- << COMPos_deq[i].pitch[1] << " " // 15
- << COMPos_deq[i].pitch[2] << " " // 16
- << COMPos_deq[i].yaw[0] << " " // 17
- << COMPos_deq[i].yaw[1] << " " // 18
- << COMPos_deq[i].yaw[2] << " " // 19
- << ZMPPos_deq[i].px << " " // 20
- << ZMPPos_deq[i].py << " " // 21
- << ZMPMB[i][0] << " " // 22
- << ZMPMB[i][1] << " " // 23
- << LeftFootAbsPos[i].x << " " // 24
- << LeftFootAbsPos[i].y << " " // 25
- << LeftFootAbsPos[i].z << " " // 26
- << LeftFootAbsPos[i].theta << " " // 27
- << LeftFootAbsPos[i].omega << " " // 28
- << LeftFootAbsPos[i].dx << " " // 29
- << LeftFootAbsPos[i].dy << " " // 30
- << LeftFootAbsPos[i].dz << " " // 31
- << LeftFootAbsPos[i].dtheta << " " // 32
- << LeftFootAbsPos[i].domega << " " // 33
- << LeftFootAbsPos[i].ddx << " " // 34
- << LeftFootAbsPos[i].ddy << " " // 35
- << LeftFootAbsPos[i].ddz << " " // 36
- << LeftFootAbsPos[i].ddtheta << " " // 37
- << LeftFootAbsPos[i].ddomega << " " // 38
- << RightFootAbsPos[i].x << " " // 39
- << RightFootAbsPos[i].y << " " // 40
- << RightFootAbsPos[i].z << " " // 41
- << RightFootAbsPos[i].theta << " " // 42
- << RightFootAbsPos[i].omega << " " // 43
- << RightFootAbsPos[i].dx << " " // 44
- << RightFootAbsPos[i].dy << " " // 45
- << RightFootAbsPos[i].dz << " " // 46
- << RightFootAbsPos[i].dtheta << " " // 47
- << RightFootAbsPos[i].domega << " " // 48
- << RightFootAbsPos[i].ddx << " " // 49
- << RightFootAbsPos[i].ddy << " " // 50
- << RightFootAbsPos[i].ddz << " " // 51
- << RightFootAbsPos[i].ddtheta << " " // 52
- << RightFootAbsPos[i].ddomega << " " // 53
- << ZMPPos_deq[i].px << " " // 54
- << ZMPPos_deq[i].py << " " // 55
- << inputdeltaZMP_deq[i].px << " " // 56
- << inputdeltaZMP_deq[i].py << " " // 57
+ for (unsigned int i = 0 ; i < n ; ++i )
+ {
+ aof << i*m_SamplingPeriod << " " // 1
+ << COMStates[i].x[0] << " " // 2
+ << COMStates[i].x[1] << " " // 3
+ << COMStates[i].x[2] << " " // 4
+ << COMStates[i].y[0] << " " // 5
+ << COMStates[i].y[1] << " " // 6
+ << COMStates[i].y[2] << " " // 7
+ << COMStates[i].z[0] << " " // 8
+ << COMStates[i].z[1] << " " // 9
+ << COMStates[i].z[2] << " " // 10
+ << COMStates[i].roll[0] << " " // 11
+ << COMStates[i].roll[1] << " " // 12
+ << COMStates[i].roll[2] << " " // 13
+ << COMStates[i].pitch[0] << " " // 14
+ << COMStates[i].pitch[1] << " " // 15
+ << COMStates[i].pitch[2] << " " // 16
+ << COMStates[i].yaw[0] << " " // 17
+ << COMStates[i].yaw[1] << " " // 18
+ << COMStates[i].yaw[2] << " " // 19
+ << ZMPPositions[i].px << " " // 20
+ << ZMPPositions[i].py << " " // 21
+ << ZMPMB[i][0] << " " // 22
+ << ZMPMB[i][1] << " " // 23
+ << filteredZMPMB[i][0] << " " // 24
+ << filteredZMPMB[i][1] << " " // 25
+ << inputdeltaZMP_deq[i].px << " " // 26
+ << inputdeltaZMP_deq[i].py << " " // 27
+ << outputDeltaCOMTraj_deq[i].x[0] << " " // 28
+ << outputDeltaCOMTraj_deq[i].x[1] << " " // 29
+ << outputDeltaCOMTraj_deq[i].x[2] << " " // 30
+ << outputDeltaCOMTraj_deq[i].y[0] << " " // 31
+ << outputDeltaCOMTraj_deq[i].y[1] << " " // 32
+ << outputDeltaCOMTraj_deq[i].y[2] << " " // 33
+ << LeftFootAbsolutePositions[i].x << " " // 34
+ << LeftFootAbsolutePositions[i].y << " " // 35
+ << LeftFootAbsolutePositions[i].z << " " // 36
+ << LeftFootAbsolutePositions[i].theta << " " // 37
+ << LeftFootAbsolutePositions[i].omega << " " // 38
+ << LeftFootAbsolutePositions[i].dx << " " // 39
+ << LeftFootAbsolutePositions[i].dy << " " // 40
+ << LeftFootAbsolutePositions[i].dz << " " // 41
+ << LeftFootAbsolutePositions[i].dtheta << " " // 42
+ << LeftFootAbsolutePositions[i].domega << " " // 43
+ << LeftFootAbsolutePositions[i].ddx << " " // 44
+ << LeftFootAbsolutePositions[i].ddy << " " // 45
+ << LeftFootAbsolutePositions[i].ddz << " " // 46
+ << LeftFootAbsolutePositions[i].ddtheta << " " // 47
+ << LeftFootAbsolutePositions[i].ddomega << " " // 48
+ << RightFootAbsolutePositions[i].x << " " // 49
+ << RightFootAbsolutePositions[i].y << " " // 50
+ << RightFootAbsolutePositions[i].z << " " // 51
+ << RightFootAbsolutePositions[i].theta << " " // 52
+ << RightFootAbsolutePositions[i].omega << " " // 53
+ << RightFootAbsolutePositions[i].dx << " " // 54
+ << RightFootAbsolutePositions[i].dy << " " // 55
+ << RightFootAbsolutePositions[i].dz << " " // 56
+ << RightFootAbsolutePositions[i].dtheta << " " // 57
+ << RightFootAbsolutePositions[i].domega << " " // 58
+ << RightFootAbsolutePositions[i].ddx << " " // 59
+ << RightFootAbsolutePositions[i].ddy << " " // 60
+ << RightFootAbsolutePositions[i].ddz << " " // 61
+ << RightFootAbsolutePositions[i].ddtheta << " " // 62
+ << RightFootAbsolutePositions[i].ddomega << " " // 63
<< endl ;
}
aof.close() ;
-
-
- oss.str("ZMPMB.dat");
- aFileName = oss.str();
- if (iteration == 0)
- {
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- }
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- aof << iteration*m_SamplingPeriod << " " // 1
- << COMPos_deq[0].x[0] << " " // 2
- << COMPos_deq[0].x[1] << " " // 3
- << COMPos_deq[0].x[2] << " " // 4
- << COMPos_deq[0].y[0] << " " // 5
- << COMPos_deq[0].y[1] << " " // 6
- << COMPos_deq[0].y[2] << " " // 7
- << COMPos_deq[0].z[0] << " " // 8
- << COMPos_deq[0].z[1] << " " // 9
- << COMPos_deq[0].z[2] << " " // 10
- << COMPos_deq[0].roll[0] << " " // 11
- << COMPos_deq[0].roll[1] << " " // 12
- << COMPos_deq[0].roll[2] << " " // 13
- << COMPos_deq[0].pitch[0] << " " // 14
- << COMPos_deq[0].pitch[1] << " " // 15
- << COMPos_deq[0].pitch[2] << " " // 16
- << COMPos_deq[0].yaw[0] << " " // 17
- << COMPos_deq[0].yaw[1] << " " // 18
- << COMPos_deq[0].yaw[2] << " " // 19
- << ZMPPos_deq[0].px << " " // 20
- << ZMPPos_deq[0].py << " " // 21
- << ZMPMBcontrol[0] << " " // 22
- << ZMPMBcontrol[1] << " " // 23
- << LeftFootAbsPos[0].x << " " // 24
- << LeftFootAbsPos[0].y << " " // 25
- << LeftFootAbsPos[0].z << " " // 26
- << LeftFootAbsPos[0].theta << " " // 27
- << LeftFootAbsPos[0].omega << " " // 28
- << LeftFootAbsPos[0].dx << " " // 29
- << LeftFootAbsPos[0].dy << " " // 30
- << LeftFootAbsPos[0].dz << " " // 31
- << LeftFootAbsPos[0].dtheta << " " // 32
- << LeftFootAbsPos[0].domega << " " // 33
- << LeftFootAbsPos[0].ddx << " " // 34
- << LeftFootAbsPos[0].ddy << " " // 35
- << LeftFootAbsPos[0].ddz << " " // 36
- << LeftFootAbsPos[0].ddtheta << " " // 37
- << LeftFootAbsPos[0].ddomega << " " // 38
- << RightFootAbsPos[0].x << " " // 39
- << RightFootAbsPos[0].y << " " // 40
- << RightFootAbsPos[0].z << " " // 41
- << RightFootAbsPos[0].theta << " " // 42
- << RightFootAbsPos[0].omega << " " // 43
- << RightFootAbsPos[0].dx << " " // 44
- << RightFootAbsPos[0].dy << " " // 45
- << RightFootAbsPos[0].dz << " " // 46
- << RightFootAbsPos[0].dtheta << " " // 47
- << RightFootAbsPos[0].domega << " " // 48
- << RightFootAbsPos[0].ddx << " " // 49
- << RightFootAbsPos[0].ddy << " " // 50
- << RightFootAbsPos[0].ddz << " " // 51
- << RightFootAbsPos[0].ddtheta << " " // 52
- << RightFootAbsPos[0].ddomega << " " // 53
- << ZMPMBcorrige[0] << " " // 54
- << ZMPMBcorrige[1] << " " // 55
- << outputDeltaCOMTraj_deq_[0].x[0] << " " // 56
- << outputDeltaCOMTraj_deq_[0].x[1] << " " // 57
- << outputDeltaCOMTraj_deq_[0].x[2] << " " // 58
- << outputDeltaCOMTraj_deq_[0].y[0] << " " // 59
- << outputDeltaCOMTraj_deq_[0].y[1] << " " // 60
- << outputDeltaCOMTraj_deq_[0].y[2] << " " // 61
- << inputdeltaZMP_deq[0].px << " " // 62
- << inputdeltaZMP_deq[0].py << " " ; // 63
-
-
- aof << endl ;
- aof.close();
-
++iteration;
-
-
-
-
}
int AnalyticalMorisawaCompact::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
@@ -2782,58 +2656,7 @@ new step has to be generate.
deque<FootAbsolutePosition> & FinalLeftFootAbsolutePositions,
deque<FootAbsolutePosition> & FinalRightFootAbsolutePositions)
{
- unsigned int lIndexInterval,lPrevIndexInterval;
- m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(m_AbsoluteTimeReference,lIndexInterval);
- lPrevIndexInterval = lIndexInterval;
-
- /*! Fill in the stacks: minimal strategy only 1 reference. */
- for(double t=StartingTime; t<=EndTime; t+= m_SamplingPeriod)
- {
- m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(t,lIndexInterval,lPrevIndexInterval);
-
- /*! Feed the ZMPPositions. */
- ZMPPosition aZMPPos;
- if (!m_AnalyticalZMPCoGTrajectoryX->ComputeZMP(t,aZMPPos.px,lIndexInterval))
- LTHROW("Unable to compute ZMP along X-Axis in EndPhaseOfWalking");
-
- if (!m_AnalyticalZMPCoGTrajectoryY->ComputeZMP(t,aZMPPos.py,lIndexInterval))
- LTHROW("Unable to compute ZMP along Y-Axis in EndPhaseOfWalking");
-
- FinalZMPPositions.push_back(aZMPPos);
-
- /*! Feed the COMStates. */
- COMState aCOMPos;
- memset(&aCOMPos,0,sizeof(aCOMPos));
- if (!m_AnalyticalZMPCoGTrajectoryX->ComputeCOM(t,aCOMPos.x[0],lIndexInterval))
- { LTHROW("COM out of bound along X axis.");}
- m_AnalyticalZMPCoGTrajectoryX->ComputeCOMSpeed(t,aCOMPos.x[1],lIndexInterval);
- if (!m_AnalyticalZMPCoGTrajectoryY->ComputeCOM(t,aCOMPos.y[0],lIndexInterval))
- { LTHROW("COM out of bound along Y axis.");}
- m_AnalyticalZMPCoGTrajectoryY->ComputeCOMSpeed(t,aCOMPos.y[1],lIndexInterval);
- aCOMPos.z[0] = m_InitialPoseCoMHeight;
- FinalCoMPositions.push_back(aCOMPos);
- /*! Feed the FootPositions. */
-
- /*! Left */
- FootAbsolutePosition LeftFootAbsPos;
- memset(&LeftFootAbsPos,0,sizeof(LeftFootAbsPos));
- if (!m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(1,t,LeftFootAbsPos,lIndexInterval))
- { LTHROW("Unable to compute left foot position in EndPhaseOfWalking");}
- FinalLeftFootAbsolutePositions.push_back(LeftFootAbsPos);
-
- /*! Right */
- FootAbsolutePosition RightFootAbsPos;
- memset(&RightFootAbsPos,0,sizeof(RightFootAbsPos));
- if (!m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(-1,t,RightFootAbsPos,lIndexInterval))
- { LTHROW("Unable to compute right foot position in EndPhaseOfWalking");}
- FinalRightFootAbsolutePositions.push_back(RightFootAbsPos);
-
-
- ODEBUG4(aZMPPos.px << " " << aZMPPos.py << " " <<
- aCOMPos.x[0] << " " << aCOMPos.y[0] << " " <<
- LeftFootAbsPos.x << " " << LeftFootAbsPos.y << " " << LeftFootAbsPos.z << " " <<
- RightFootAbsPos.x << " " << RightFootAbsPos.y << " " << RightFootAbsPos.z << " " <<
- m_SamplingPeriod,"Test.dat");
- }
+ FillQueues(m_SamplingPeriod, StartingTime, EndTime, FinalZMPPositions,
+ FinalCoMPositions, FinalLeftFootAbsolutePositions, FinalRightFootAbsolutePositions);
}
}
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index 51ae3f28b9ed874e3f17bb08ca710dfd21c2a5cb..6d97f76c7c386455b9b04ad80d13869aa3b8c109 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -59,6 +59,11 @@ DynamicFilter::DynamicFilter(
jacobian_lf_ = Jacobian_LF::Jacobian::Zero();
jacobian_rf_ = Jacobian_RF::Jacobian::Zero();
+
+ sxzmp_ = 0.0 ;
+ syzmp_ = 0.0 ;
+ deltaZMPx_ = 0.0 ;
+ deltaZMPy_ = 0.0 ;
}
DynamicFilter::~DynamicFilter()
@@ -99,9 +104,10 @@ void DynamicFilter::init(
PG_N_ = (int)(previewWindowSize_/interpolationPeriod_) ;
CoMHeight_ = CoMHeight ;
- PC_->SetPreviewControlTime (previewWindowSize_ - PG_T/controlPeriod_ * interpolationPeriod_);
+ PC_->SetPreviewControlTime (previewWindowSize_);
PC_->SetSamplingPeriod (interpolationPeriod_);
PC_->SetHeightOfCoM(CoMHeight_);
+ PC_->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITH_INITIALPOS);
previousConfiguration_ = comAndFootRealization_->getHumanoidDynamicRobot()->currentConfiguration() ;
previousVelocity_ = comAndFootRealization_->getHumanoidDynamicRobot()->currentVelocity() ;
@@ -154,9 +160,10 @@ void DynamicFilter::init(
CWx = node_waist.body.iX0.r()(0,0) - inputCoMState.x[0] ;
CWy = node_waist.body.iX0.r()(1,0) - inputCoMState.y[0] ;
- aSxzmp_ = 0.0 ;
- aSyzmp_ = 0.0 ;
-
+ sxzmp_ = 0.0 ;
+ syzmp_ = 0.0 ;
+ deltaZMPx_ = 0.0 ;
+ deltaZMPy_ = 0.0 ;
return ;
}
@@ -500,35 +507,44 @@ int DynamicFilter::OptimalControl(
deque<ZMPPosition> & inputdeltaZMP_deq,
deque<COMState> & outputDeltaCOMTraj_deq_)
{
+ /// --------------------
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+ /// --------------------
+ oss.str("ZMPDiscretisationErr.dat");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ ///----
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+
if(!PC_->IsCoherent())
PC_->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITH_INITIALPOS);
- outputDeltaCOMTraj_deq_.clear();
- double deltaZMPx (0) , deltaZMPy (0) ;
- COMState aCOMState ;
+ outputDeltaCOMTraj_deq_.resize(NCtrl_);
// calcul of the preview control along the "deltaZMP_deq_"
for (unsigned i = 0 ; i < NCtrl_ ; i++ )
{
- for(int j=0;j<3;j++)
- {
- deltax_(j,0) = 0 ;
- deltay_(j,0) = 0 ;
- }
+ aof << i*controlPeriod_ << " " // 1
+ << sxzmp_ << " " // 2
+ << syzmp_ << " " // 3
+ << endl ;
PC_->OneIterationOfPreview(deltax_,deltay_,
- aSxzmp_,aSyzmp_,
+ sxzmp_,syzmp_,
inputdeltaZMP_deq,i,
- deltaZMPx, deltaZMPy, true);
- cout << "PC CoMz = " << PC_->GetHeightOfCoM()
- << " ; PC controlTime = " << PC_->PreviewControlTime()
- << " ; PC sampling period : " << PC_->SamplingPeriod() << endl ;
+ deltaZMPx_, deltaZMPy_, false);
for(int j=0;j<3;j++)
{
- aCOMState.x[j] = deltax_(j,0);
- aCOMState.y[j] = deltay_(j,0);
+ outputDeltaCOMTraj_deq_[i].x[j] = deltax_(j,0);
+ outputDeltaCOMTraj_deq_[i].y[j] = deltay_(j,0);
}
- outputDeltaCOMTraj_deq_.push_back(aCOMState);
}
+ aof.close();
+
for (unsigned int i = 0 ; i < NCtrl_ ; i++)
{
for(int j=0;j<3;j++)
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
index 6aeabba856fff5f3071d6a1eb74a43c2c22a46ea..d069fb039634bce16e7bdc3a1363d875200f2eed 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
@@ -168,6 +168,9 @@ namespace PatternGeneratorJRL
inline double getPreviewWindowSize_()
{return previewWindowSize_ ;}
+ inline void getPCerror_(double & errx, double & erry)
+ { errx = sxzmp_ ; erry = syzmp_ ; }
+
private: // Private members
/// \brief Time variables
@@ -260,7 +263,8 @@ namespace PatternGeneratorJRL
/// --------------------------------
/// \brief Pointer to the Preview Control object.
PreviewControl *PC_;
- double aSxzmp_ , aSyzmp_ ;
+ double sxzmp_ , syzmp_ ;
+ double deltaZMPx_, deltaZMPy_ ;
double CoMHeight_ ;
/// \brief State of the Preview control.
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index 12609712cb44669b18a84bf4e963ba2d6acd33fa..51530d621e907e37d47e93a427e7b1d8a18cef66 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -185,7 +185,7 @@ public:
m_clock.writeBuffer(lProfileOutput);
m_clock.displayStatistics(os,m_OneStep);
// Compare debugging files
- ComputeAndDisplayAverageError(true);
+ //ComputeAndDisplayAverageError(true);
return compareDebugFiles();
}