create temporary buffer to have a bigger deque to do the PC of Kajita on 16-1 * 100ms

CMakeLists.txt

@@ -23,6 +23,7 @@ INCLUDE(cmake/cmake_metapod_configure.cmake)
 INCLUDE(cmake/base.cmake)
 INCLUDE(cmake/lapack.cmake)
 INCLUDE(cmake/cpack.cmake)
+INCLUDE(cmake/boost.cmake)
 INCLUDE(CMakeDependentOption)
 
 # Define properties of the project
@@ -53,6 +54,24 @@ ADD_OPTIONAL_DEPENDENCY("hrp2-dynamics >= 1.0.1")
 ADD_OPTIONAL_DEPENDENCY("metapod >= 1.0.7")
 MESSAGE(STATUS "hrp2-dynamics: " ${HRP2_DYNAMICS_FOUND})
 
+# Search for Boost.
+# Boost.Test is used by the test suite.
+# Boost program_options is used by the embedfile utility, which is used
+# by metapod_robotbuilder
+# Boost filesystem and regex are used by metapod_robotbuilder.
+# Boost filesystem depends on Boost system.
+SET(BOOST_COMPONENTS
+  filesystem system unit_test_framework program_options regex)
+SEARCH_FOR_BOOST()
+# If Boost is recent enough, we look for Boost timer which can be used by
+# by metapod_timer, which is in turn used by the benchmark.
+# Boost timer depends on Boost chrono and system.
+IF((BUILD_MULTI_MODEL_BENCHMARK OR BUILD_SINGLE_MODEL_BENCHMARKS)
+   AND NOT Boost_VERSION LESS 104800)
+  SET(BOOST_COMPONENTS ${BOOST_COMPONENTS} timer chrono system)
+  SEARCH_FOR_BOOST()
+ENDIF()
+
 # Search for Eigen.
 ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.0.0")
 # Eigen (at least version 3.0.5) makes gcc report conversion warnings

src/CMakeLists.txt

@@ -18,6 +18,10 @@ INCLUDE_DIRECTORIES(BEFORE ${PROJECT_SOURCE_DIR}/src)
 INCLUDE_DIRECTORIES(BEFORE ${HOME}/devel/metapod/_bin/include/)
 INCLUDE_DIRECTORIES(BEFORE ${HOME}/devel/metapod/_bin/include/metapod/)
 
+# Add Boost path to include directories.
+INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
+message (STATUS "-- Boost Dir : " ${Boost_INCLUDE_DIRS} )
+
 # add flag to compile qld.cc
 IF(WIN32)
 ADD_DEFINITIONS("/D __STDC__")
@@ -89,15 +93,14 @@ ADD_LIBRARY(jrl-walkgen SHARED ${SOURCES})
 SET_TARGET_PROPERTIES(jrl-walkgen PROPERTIES SOVERSION ${PROJECT_VERSION})
 # Define dependencies
 
-##############################################################################
+# define and search for metapod library
 SET(WITH_METAPODFROMURDF TRUE)
 SET(model_name "hrp2_14")
 # Define the simple humanoid model for the metapod algorithm
 ADD_SAMPLEURDFMODEL(${model_name})
-##############################################################################
-TARGET_LINK_LIBRARIES(jrl-walkgen metapod_${model_name} )
-
 
+TARGET_LINK_LIBRARIES(jrl-walkgen metapod_${model_name} ${Boost_LIBRARIES})
+message (STATUS "--Boost : " ${Boost_LIBRARIES} )
 
 INSTALL(TARGETS jrl-walkgen DESTINATION ${CMAKE_INSTALL_LIBDIR})
 

src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp

@@ -442,32 +442,44 @@ ZMPVelocityReferencedQP::OnLine(double time,
 
       // INTERPOLATE THE NEXT COMPUTED COM STATE:
       // ----------------------------------------
-      deque<ZMPPosition> ZMPTraj_deq = FinalZMPTraj_deq ;
-      deque<COMState> COMTraj_deq = FinalCOMTraj_deq ;
-      deque<FootAbsolutePosition> LeftFootTraj_deq = FinalLeftFootTraj_deq ;
-      deque<FootAbsolutePosition> RightFootTraj_deq = FinalRightFootTraj_deq ;
+      deque<ZMPPosition> ZMPTraj_deq ( FinalZMPTraj_deq.size() );
+      deque<COMState> COMTraj_deq ( FinalCOMTraj_deq.size() );
+      deque<FootAbsolutePosition> LeftFootTraj_deq ( FinalLeftFootTraj_deq.size() );
+      deque<FootAbsolutePosition> RightFootTraj_deq ( FinalRightFootTraj_deq.size() );
+      for (unsigned int i = 0 ; i < FinalZMPTraj_deq.size() ; i++ )
+      {
+        ZMPTraj_deq[i] = FinalZMPTraj_deq[i] ;
+        COMTraj_deq[i] = FinalCOMTraj_deq[i] ;
+        LeftFootTraj_deq[i] = FinalLeftFootTraj_deq[i] ;
+        RightFootTraj_deq[i] = FinalRightFootTraj_deq[i] ;
+      }
 
-      unsigned currentIndex = FinalCOMTraj_deq.size();
+      unsigned currentIndex = COMTraj_deq.size();
       unsigned numberOfSample = (unsigned)(QP_T_/m_SamplingPeriod);
+      COMTraj_deq.resize( numberOfSample + currentIndex );
+      ZMPTraj_deq.resize( numberOfSample + currentIndex );
+
       FinalCOMTraj_deq.resize( numberOfSample + currentIndex );
       FinalZMPTraj_deq.resize( numberOfSample + currentIndex );
+      FinalLeftFootTraj_deq.resize( numberOfSample + currentIndex );
+      FinalRightFootTraj_deq.resize( numberOfSample + currentIndex );
 
       if(Solution_.SupportStates_deq.size() &&  Solution_.SupportStates_deq[0].NbStepsLeft == 0)
       {
-         double jx = (FinalLeftFootTraj_deq[0].x + FinalRightFootTraj_deq[0].x)/2 - FinalCOMTraj_deq[0].x[0];
-         double jy = (FinalLeftFootTraj_deq[0].y + FinalRightFootTraj_deq[0].y)/2 - FinalCOMTraj_deq[0].y[0];
+         double jx = (LeftFootTraj_deq[0].x + RightFootTraj_deq[0].x)/2 - COMTraj_deq[0].x[0];
+         double jy = (LeftFootTraj_deq[0].y + RightFootTraj_deq[0].y)/2 - COMTraj_deq[0].y[0];
          if(fabs(jx) < 1e-3 && fabs(jy) < 1e-3) { Running_ = false; }
          const double tf = 0.75;
-         jx = 6/(tf*tf*tf)*(jx - tf*FinalCOMTraj_deq[0].x[1] - (tf*tf/2)*FinalCOMTraj_deq[0].x[2]);
-         jy = 6/(tf*tf*tf)*(jy - tf*FinalCOMTraj_deq[0].y[1] - (tf*tf/2)*FinalCOMTraj_deq[0].y[2]);
-         CoM_.Interpolation( FinalCOMTraj_deq, FinalZMPTraj_deq, currentIndex,
+         jx = 6/(tf*tf*tf)*(jx - tf*COMTraj_deq[0].x[1] - (tf*tf/2)*COMTraj_deq[0].x[2]);
+         jy = 6/(tf*tf*tf)*(jy - tf*COMTraj_deq[0].y[1] - (tf*tf/2)*COMTraj_deq[0].y[2]);
+         CoM_.Interpolation( COMTraj_deq, ZMPTraj_deq, currentIndex,
              jx, jy);
          CoM_.OneIteration( jx, jy );
       }
       else
       {
          Running_ = true;
-         CoM_.Interpolation( FinalCOMTraj_deq, FinalZMPTraj_deq, currentIndex,
+         CoM_.Interpolation( COMTraj_deq, ZMPTraj_deq, currentIndex,
              Solution_.Solution_vec[0], Solution_.Solution_vec[QP_N_] );
          CoM_.OneIteration( Solution_.Solution_vec[0],Solution_.Solution_vec[QP_N_] );
       }
@@ -477,18 +489,28 @@ ZMPVelocityReferencedQP::OnLine(double time,
       // ------------------------------
       OrientPrw_->interpolate_trunk_orientation( time, currentIndex,
           m_SamplingPeriod, Solution_.SupportStates_deq,
-          FinalCOMTraj_deq );
+          COMTraj_deq );
 
       // INTERPOLATE THE COMPUTED FOOT POSITIONS:
       // ----------------------------------------
       Robot_->generate_trajectories( time, Solution_,
           Solution_.SupportStates_deq, Solution_.SupportOrientations_deq,
-          FinalLeftFootTraj_deq, FinalRightFootTraj_deq );
+          LeftFootTraj_deq, RightFootTraj_deq );
+
+
 
       // DYNAMIC FILTER
       // --------------
       //DynamicFilter( FinalZMPTraj_deq, FinalCOMTraj_deq, FinalLeftFootTraj_deq, FinalRightFootTraj_deq );
 
+      for (unsigned int i = 0 ; i < FinalZMPTraj_deq.size() ; i++ )
+      {
+        FinalZMPTraj_deq[i] = ZMPTraj_deq[i] ;
+        FinalCOMTraj_deq[i] = COMTraj_deq[i] ;
+        FinalLeftFootTraj_deq[i] = LeftFootTraj_deq[i] ;
+        FinalRightFootTraj_deq[i] = RightFootTraj_deq[i] ;
+      }
+
 
       // Specify that we are in the ending phase.
       if (EndingPhase_ == false)