From 4747b7affb57b6af71e7dbd3483231f21cd5bff7 Mon Sep 17 00:00:00 2001
From: Kevin Giraud-Esclasse <kgiraude@laas.fr>
Date: Wed, 16 Nov 2016 19:53:23 +0100
Subject: [PATCH] 10cm goingUpStairs ok
---
cmake | 2 +-
.../FootTrajectoryGenerationStandard.cpp | 53 +++++++++++-
src/Mathematics/Bsplines.cpp | 18 ++++
src/Mathematics/Bsplines.hh | 8 ++
.../AnalyticalMorisawaCompact.cpp | 2 +-
tests/CMakeLists.txt | 8 +-
tests/TestBsplines.cpp | 86 +++++++++++++++++++
tests/TestMorisawa2007.cpp | 2 +
8 files changed, 170 insertions(+), 9 deletions(-)
diff --git a/cmake b/cmake
index 1ad98457..5663a002 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit 1ad984575ca91a4e19ce9eb9d092e92d09fcb7fb
+Subproject commit 5663a002a905b5c8d84a69d8e74044a8e34a48ed
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
index 3f4c5772..911c7e35 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
@@ -406,20 +406,67 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex, double Ti
}
else if (FinalPosition - InitPosition > epsilon )
{
- ToMP.push_back(0.5*TimeInterval);
- MP.push_back(FinalPosition+0.2*WayPoint_z);
+// ToMP.push_back(0.30*TimeInterval);
+// MP.push_back(FinalPosition+0.0*WayPoint_z);
+// ToMP.push_back(0.5*TimeInterval);
+// MP.push_back(FinalPosition+0.8*WayPoint_z);
+// deque<double> knot;
+// for (int i=0;i<4;++i){knot.push_back(0);}
+// knot.push_back(0.35*TimeInterval);
+// knot.push_back(0.35*TimeInterval);
+// for (int i=0;i<4;++i){knot.push_back(0);}
+// m_BsplinesZ->SetKnotVector(knot);
+// vector<double> controlPoints;
+// controlPoints.push_back(InitPosition);
+// controlPoints.push_back(FinalPosition);
+// controlPoints.push_back(FinalPosition+WayPoint_z);
+// controlPoints.push_back(FinalPosition);
+ deque<double> knot;
+ double TimeInterval = 1.4;
+ for (int i=0;i<8;++i){knot.push_back(0);}
+ double pourcentTime=0.15;
+ knot.push_back(pourcentTime);
+ knot.push_back(pourcentTime);
+ for (int i=0;i<8;++i){knot.push_back(1.0);}
+ m_BsplinesZ->SetKnotVector(knot);
+ vector<double> controlPoints;
+ for (int i=0 ; i<5 ; ++i)
+ {controlPoints.push_back(InitPosition);}
+ controlPoints.push_back(FinalPosition+WayPoint_z);
+ controlPoints.push_back(FinalPosition+WayPoint_z);
+ for (int i=0 ; i<5 ; ++i)
+ {controlPoints.push_back(FinalPosition);}
+
+ m_BsplinesZ->SetControlPoints(controlPoints);
+// m_BsplinesZ->FT(TimeInterval);
+// m_BsplinesZ->FP(FinalPosition);
+ double IS = 0.0 ;
+ double IA = 0.0 ;
+ double FS = 0.0 ;
+ double FA = 0.0 ;
+ m_BsplinesZ->SetParametersWithoutMPAndToMP(TimeInterval,
+ InitPosition,
+ FinalPosition,
+ IS,IA,FS,FA);
+ m_BsplinesZ->GenerateDegree();
+ m_BsplinesZ->PrintControlPoints();
+ m_BsplinesZ->PrintDegree();
+ m_BsplinesZ->PrintKnotVector();
+
}
else if ( sqrt((FinalPosition - InitPosition)*(FinalPosition - InitPosition)) <= epsilon )
{
ToMP.push_back(0.5*TimeInterval);
MP.push_back(FinalPosition+WayPoint_z);
+ m_BsplinesZ->SetParameters(TimeInterval,InitPosition,FinalPosition,ToMP,MP,InitSpeed,InitAcc);
}
else if (FinalPosition - InitPosition < -epsilon )
{
ToMP.push_back(0.6*TimeInterval);
MP.push_back(InitPosition+0.5*WayPoint_z);
+ m_BsplinesZ->SetParameters(TimeInterval,InitPosition,FinalPosition,ToMP,MP,InitSpeed,InitAcc);
}
- m_BsplinesZ->SetParameters(TimeInterval,InitPosition,FinalPosition,ToMP,MP,InitSpeed,InitAcc);
+ //m_BsplinesZ->SetParameters(TimeInterval,InitPosition,FinalPosition,ToMP,MP,InitSpeed,InitAcc);
break;
case THETA_AXIS:
diff --git a/src/Mathematics/Bsplines.cpp b/src/Mathematics/Bsplines.cpp
index 806e95aa..2382dbad 100644
--- a/src/Mathematics/Bsplines.cpp
+++ b/src/Mathematics/Bsplines.cpp
@@ -1861,3 +1861,21 @@ void BSplinesFoot::GetParameters(double &FT,
ToMP = m_ToMP ;
MP = m_MP ;
}
+
+void BSplinesFoot::SetParametersWithoutMPAndToMP(double FT,
+ double IP,
+ double FP,
+ double IS, double IA,
+ double FS, double FA)
+{
+ m_FT = FT ;
+
+ m_IP = IP ;
+ m_IS = IS ;
+ m_IA = IA ;
+
+ m_FP = FP ;
+ m_FS = FS ;
+ m_FA = FA ;
+}
+
diff --git a/src/Mathematics/Bsplines.hh b/src/Mathematics/Bsplines.hh
index 9d4ed13a..3a8e71fc 100644
--- a/src/Mathematics/Bsplines.hh
+++ b/src/Mathematics/Bsplines.hh
@@ -125,6 +125,11 @@ namespace PatternGeneratorJRL
std::vector<double> MP,
double IS = 0.0, double IA = 0.0,
double FS = 0.0, double FA = 0.0);
+ void SetParametersWithoutMPAndToMP(double FT,
+ double IP,
+ double FP,
+ double IS, double IA,
+ double FS, double FA);
/*!Compute Position at time t */
int Compute(double t, double &x, double &dx, double &ddx);
@@ -152,6 +157,9 @@ namespace PatternGeneratorJRL
double FT()
{return m_FT;}
+ void FT(double ft)
+ {m_FT=ft;}
+
double IP()
{return m_IP;}
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index d8477e58..601a4330 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -607,7 +607,6 @@ computing the analytical trajectories. */
/*! initialize the dynamic filter */
unsigned int n = COMStates.size();
double KajitaPCpreviewWindow = 1.6 ;
- m_kajitaDynamicFilter->getComAndFootRealization()->ShiftFoot(false);
m_kajitaDynamicFilter->init( m_SamplingPeriod,
m_SamplingPeriod,
n*m_SamplingPeriod,
@@ -2683,6 +2682,7 @@ new step has to be generate.
LTHROW("No foot");
vector3d corrZ ;
corrZ = aFoot->anklePosition ;
+ corrZ(2) = 0; //foot height no more equal to ankle height; TODO : remove corrZ
// after the final step we keep the same position for a while
if( Index >= m_AbsoluteSupportFootPositions.size() )
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index d9e9f243..5553ea40 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -59,10 +59,10 @@ ADD_TEST(TestOptCholesky TestOptCholesky)
## Test Bspline #
##########################
#TODO create a proper test for polynomes and bsplines
-#ADD_EXECUTABLE(TestBsplines
-# TestBsplines.cpp
-# ../src/Mathematics/Bsplines.cpp
-# )
+ADD_EXECUTABLE(TestBsplines
+ TestBsplines.cpp
+ ../src/Mathematics/Bsplines.cpp
+ )
##########################
## Test Ricatti Equation #
diff --git a/tests/TestBsplines.cpp b/tests/TestBsplines.cpp
index 127ac99b..5c7d8062 100644
--- a/tests/TestBsplines.cpp
+++ b/tests/TestBsplines.cpp
@@ -163,6 +163,92 @@ int PerformTests(int argc, char *argv[])
delete bsplineTwoWayPoint ;
bsplineTwoWayPoint = NULL ;
+
+ // Test Bspline with knots and control points
+ ///////////////////////////////////////////
+ /// \brief bsplineKnotsControl
+ ofstream aof;
+ string aFileName = "bsplineKnotsControl.dat";
+ 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 << filterprecision(m_OneStep.NbOfIt*0.005 ) << " "
+ PatternGeneratorJRL::BSplinesFoot *bsplineKnotsControl;
+ bsplineKnotsControl = new PatternGeneratorJRL::BSplinesFoot(
+ 1.4, IP, FP, ToMP, MP, IS, IA, FS, FA);
+
+ deque<double> knot;
+ double TimeInterval = 1.4;
+ for (int i=0;i<8;++i){knot.push_back(0);}
+ double pourcentTime=0.05;
+ knot.push_back(pourcentTime);
+ knot.push_back(pourcentTime);
+ for (int i=0;i<8;++i){knot.push_back(1.0);}
+ bsplineKnotsControl->SetKnotVector(knot);
+ vector<double> controlPoints;
+ double InitPosition=0.0;
+ double FinalPosition=0.1;
+ double WayPoint_z=0.07;
+ controlPoints.push_back(InitPosition);
+ controlPoints.push_back(InitPosition);
+ controlPoints.push_back(InitPosition);
+ controlPoints.push_back(InitPosition);
+ controlPoints.push_back(InitPosition);
+ controlPoints.push_back(FinalPosition);
+ controlPoints.push_back(FinalPosition+WayPoint_z);
+ controlPoints.push_back(FinalPosition);
+ controlPoints.push_back(FinalPosition);
+ controlPoints.push_back(FinalPosition);
+ controlPoints.push_back(FinalPosition);
+ controlPoints.push_back(FinalPosition);
+ //controlPoints.push_back(FinalPosition);
+ bsplineKnotsControl->SetControlPoints(controlPoints);
+ bsplineKnotsControl->FT(TimeInterval);
+ bsplineKnotsControl->GenerateDegree();
+ bsplineKnotsControl->PrintControlPoints();
+ bsplineKnotsControl->PrintDegree();
+ bsplineKnotsControl->PrintKnotVector();
+
+ FT = 1.4 ;
+ IP = 0.0 ;
+ IS = 0.0 ;
+ IA = 0.0 ;
+ FP = 0.1 ;
+ FS = 0.0 ;
+ FA = 0.0 ;
+ bsplineKnotsControl->Compute(0.0, testIP, testIS, testIA);
+ bsplineKnotsControl->Compute(TimeInterval*pourcentTime, testMP, testMS, testMA);
+ bsplineKnotsControl->Compute(TimeInterval , testFP, testFS, testFA);
+ test_IP = sqrt((IP-testIP)*(IP-testIP))<1e-08 ;
+ test_IS = sqrt((IS-testIS)*(IS-testIS))<1e-08 ;
+ test_IA = sqrt((IA-testIA)*(IA-testIA))<1e-08 ;
+ test_FP = sqrt((FP-testFP)*(FP-testFP))<1e-08 ;
+ test_FS = sqrt((FS-testFS)*(FS-testFS))<1e-08 ;
+ test_FA = sqrt((FA-testFA)*(FA-testFA))<1e-08 ;
+ bool testbsplineKnotsControl =
+ test_IP && test_IS && test_IA &&
+ test_FP && test_FS && test_FA ;
+ std::cout << "testFP : " << testFP << std::endl;
+ std::cout << "MP : [" << TimeInterval << ";" << FinalPosition << "]" << std::endl;
+
+ for (double i=0 ; i < 1400 ; ++i)
+ {
+ bsplineKnotsControl->Compute(FT*i/1400.0, testMP, testMS, testMA);
+ aof << testMP << endl;
+ }
+
+ if(!bsplineKnotsControl || !testBsplineTwoWayPoint)
+ {
+ std::cerr << "Error unexpected behaviour of bspline generation\n"
+ << "bspline with knots and control points"
+ << std::endl;
+ }
+ delete bsplineKnotsControl ;
+ bsplineKnotsControl = NULL ;
+
+
return (testBsplinenowayPoint &&
testBsplineOneWayPoint &&
testBsplineTwoWayPoint) ? 1 : 0 ;
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index 3e27a760..7b568333 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -201,6 +201,8 @@ protected:
0.0 -0.19 0.0 0.0\
0.30 0.19 0.10 0.0\
0.0 -0.19 0.0 0.0\
+ 0.30 0.19 0.10 0.0\
+ 0.0 -0.19 0.0 0.0\
");
aPGI.ParseCmd(strm2);
}
--
GitLab