CoM Trajectory Generation from HMC

.github/workflows/build.yml

@@ -177,6 +177,30 @@ jobs:
         cd build
         cmake ../ && make -j2 && sudo make install || exit 1
         popd
+    - name: Upload documentation (HMC fork)
+      # Only run on master branch and for one configuration
+      if: matrix.os == 'ubuntu-18.04' && matrix.build-type == 'RelWithDebInfo' && matrix.compiler == 'gcc' && github.ref == 'refs/heads/topic/hmc' && github.repository == 'arntanguy/mc_rtc'
+      run: |
+        set -x
+        pushd .
+        cd doc
+        cp -r /usr/local/share/doc/mc_rtc/doxygen-html .
+        find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/SpaceVecAlg/doxygen-html/@https://jrl-umi3218.github.io/SpaceVecAlg/doxygen/HEAD/@g'
+        find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/RBDyn/doxygen-html/@https://jrl-umi3218.github.io/RBDyn/doxygen/HEAD/@g'
+        find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/Tasks/doxygen-html/@https://jrl-umi3218.github.io/Tasks/doxygen/HEAD/@g'
+        find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/sch-core/doxygen-html/@https://jrl-umi3218.github.io/sch-core/doxygen/HEAD/@g'
+        find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/mc_rbdyn_urdf/doxygen-html/@https://jrl-umi3218.github.io/mc_rbdyn_urdf/doxygen/HEAD/@g'
+        sudo apt-get install -qq ruby-dev ruby-bundler libxml2-dev
+        bundle install --path vendor
+        git clone -b gh-pages https://arntanguy:${{ secrets.GH_PAGES_TOKEN }}@github.com/arntanguy/mc_rtc /tmp/website
+        bundle exec jekyll build --trace -b /mc_rtc -d /tmp/website
+        cd /tmp/website
+        git add .
+        git config --global user.email "[email protected]"
+        git config --global user.name "Arnaud TANGUY (Automated CI update)"
+        git commit -m "Website from commit ${GITHUB_SHA}"
+        git push origin gh-pages
+        popd
     - name: Upload documentation
       # Only run on master branch and for one configuration
       if: matrix.os == 'ubuntu-18.04' && matrix.build-type == 'RelWithDebInfo' && matrix.compiler == 'gcc' && github.ref == 'refs/heads/master' && github.repository == 'jrl-umi3218/mc_rtc'
@@ -186,6 +210,7 @@ jobs:
         cd doc
         cp -r /usr/local/share/doc/mc_rtc/doxygen-html .
         find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/SpaceVecAlg/doxygen-html/@https://jrl-umi3218.github.io/SpaceVecAlg/doxygen/HEAD/@g'
+        find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/SpaceVecAlg/doxygen-html/@https://jrl-umi3218.github.io/SpaceVecAlg/doxygen/HEAD/@g'
         find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/RBDyn/doxygen-html/@https://jrl-umi3218.github.io/RBDyn/doxygen/HEAD/@g'
         find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/Tasks/doxygen-html/@https://jrl-umi3218.github.io/Tasks/doxygen/HEAD/@g'
         find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/sch-core/doxygen-html/@https://jrl-umi3218.github.io/sch-core/doxygen/HEAD/@g'
@@ -201,11 +226,11 @@ jobs:
         git commit -m "Website from commit ${GITHUB_SHA}"
         git push origin gh-pages
         popd
-    - name: Slack Notification
-      if: failure()
-      uses: archive/github-actions-slack@master
-      with:
-        slack-bot-user-oauth-access-token: ${{ secrets.SLACK_BOT_TOKEN }}
-        slack-channel: '#ci'
-        slack-text: >
-          [mc_rtc] Build *${{ matrix.os }}/${{ matrix.build-type }} (${{ matrix.compiler }})* failed on ${{ github.ref }}
+    # - name: Slack Notification
+    #   if: failure()
+    #   uses: archive/github-actions-slack@master
+    #   with:
+    #     slack-bot-user-oauth-access-token: ${{ secrets.SLACK_BOT_TOKEN }}
+    #     slack-channel: '#ci'
+    #     slack-text: >
+    #       [mc_rtc] Build *${{ matrix.os }}/${{ matrix.build-type }} (${{ matrix.compiler }})* failed on ${{ github.ref }}

CMakeLists.txt

@@ -33,6 +33,7 @@ set_property(GLOBAL PROPERTY USE_FOLDERS ON)
 
 project(${PROJECT_NAME} C CXX)
 set(MC_RTC_SOURCE_DIR "${PROJECT_SOURCE_DIR}")
+set(DOXYGEN_EXAMPLE_PATH "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DOCDIR}/doxygen-html/examples")
 
 option(GENERATE_COVERAGE "Generate coverage data" FALSE)
 if("${CMAKE_CXX_FLAGS}" MATCHES "--coverage")
@@ -46,6 +47,7 @@ option(TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" OFF)
 
 option(BUILD_CONTROLLER_SAMPLES "Build sample controllers in the project" ON)
 option(DISABLE_CONTROLLER_TESTS "Disable controller unit tests" OFF)
+option(DISABLE_LOG_REGRESSION_TESTS "Disable tests checking regressions against downloaded logs" ON)
 option(DISABLE_ROBOT_TESTS "Disable RobotModule unit tests" OFF)
 option(ENABLE_FAST_TESTS "Run controllers tests for a few iterations" OFF)
 
@@ -56,6 +58,17 @@ option(MC_RTC_DISABLE_NETWORK "Build without network support" OFF)
 
 option(DISABLE_ROS "Build without ROS support (even if ROS was found)" OFF)
 
+# By default, neither clang nor gcc produce color output if they detect the output medium
+# is not a terminal (e.g Ninja).
+option (FORCE_COLORED_OUTPUT "Always produce ANSI-colored output (GNU/Clang only)." OFF)
+if (${FORCE_COLORED_OUTPUT})
+  if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
+    add_compile_options (-fdiagnostics-color=always)
+  elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+    add_compile_options (-fcolor-diagnostics)
+  endif()
+endif()
+
 add_project_dependency(Boost REQUIRED COMPONENTS filesystem timer)
 add_project_dependency(Tasks REQUIRED)
 add_project_dependency(mc_rbdyn_urdf REQUIRED)
@@ -156,6 +169,7 @@ include(CMakeModules/ListAllFiles.cmake)
 add_subdirectory(CMakeModules)
 add_subdirectory(doc)
 add_subdirectory(src)
+add_subdirectory(samples)
 add_subdirectory(plugins)
 if(${BUILD_TESTING})
   add_subdirectory(tests)

benchmarks/CMakeLists.txt

@@ -26,3 +26,4 @@ mc_rtc_benchmark(benchCompletionCriteria mc_control)
 mc_rtc_benchmark(benchSimulationContactSensor mc_control)
 mc_rtc_benchmark(benchRobotLoading mc_rbdyn)
 mc_rtc_benchmark(benchAllocTasks mc_tasks)
+mc_rtc_benchmark(benchHMC mc_planning)

benchmarks/benchHMC.cpp

@@ -0,0 +1,170 @@
+/*
+ * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL
+ */
+
+#include <mc_planning/LookupTable.h>
+#include <mc_planning/generator.h>
+#include <mc_rtc/constants.h>
+#include "benchmark/benchmark.h"
+#include <random>
+
+static void BM_cosh_lookuptable(benchmark::State & state)
+{
+  // Perform setup here
+  auto omega = [](double h) { return std::sqrt(mc_rtc::constants::GRAVITY / h); };
+  double dt = 0.005;
+  auto table =
+      mc_planning::LookupTable<double>(20000, omega(2.5), omega(0.01), [dt](double x) { return cosh(x * dt); });
+
+  static std::random_device rd;
+  static std::mt19937_64 gen(rd());
+  static std::uniform_real_distribution<double> dis(omega(2.5), omega(0.01));
+
+  double res = 0;
+  double val = 0;
+  for(auto _ : state)
+  {
+    val = dis(gen);
+    for(int i = 0; i < 10000; ++i)
+    {
+      // Prevent compiler from optimizing away the result
+      benchmark::DoNotOptimize(res = table(val));
+    }
+  }
+  state.SetItemsProcessed(10000 * state.iterations());
+}
+BENCHMARK(BM_cosh_lookuptable);
+
+static void BM_cosh(benchmark::State & state)
+{
+  // Perform setup here
+  auto omega = [](double h) { return std::sqrt(mc_rtc::constants::GRAVITY / h); };
+  double dt = 0.005;
+  static std::random_device rd;
+  static std::mt19937_64 gen(rd());
+  static std::uniform_real_distribution<double> dis(omega(2.5), omega(0.01));
+
+  double res = 0;
+  double val = 0;
+  for(auto _ : state)
+  {
+    val = dis(gen);
+    for(int i = 0; i < 10000; ++i)
+    {
+      benchmark::DoNotOptimize(res = cosh(val * dt));
+    }
+  }
+  state.SetItemsProcessed(10000 * state.iterations());
+}
+BENCHMARK(BM_cosh);
+
+class PreviewStepsFixture : public benchmark::Fixture
+{
+  using PreviewSteps = mc_planning::PreviewSteps<Eigen::Vector2d>;
+
+public:
+  void SetUp(const ::benchmark::State &)
+  {
+    steps = std::make_shared<PreviewSteps>();
+    steps->add({0, {0.0, 0.0}});
+    steps->add({1.5, {0.0, 1.5}});
+    steps->add({1.6, {0.0, 1.6}});
+    steps->add({2.9, {0.0, 2.9}});
+    steps->add({3.0, {0.0, 3.0}});
+    steps->add({5.0, {0.0, 3.0}});
+    steps->add({10.0, {0.0, 3.0}});
+    steps->initialize();
+  }
+
+  void TearDown(const ::benchmark::State &) {}
+
+  static constexpr unsigned nIter()
+  {
+    return n_loop * (2 * n_preview + 1);
+  }
+
+  std::shared_ptr<PreviewSteps> steps;
+  static constexpr double dt = 0.005;
+  static constexpr unsigned n_loop = 15. / dt;
+  static constexpr unsigned n_preview = 1.6 / dt;
+};
+
+BENCHMARK_DEFINE_F(PreviewStepsFixture, BM_PreviewSteps)(benchmark::State & state)
+{
+  while(state.KeepRunning())
+  {
+    for(unsigned loop = 0; loop <= n_loop; loop++)
+    {
+      auto previewStart = loop * dt;
+      steps->nextWindow(previewStart);
+
+      for(unsigned previewWindow = loop; previewWindow < loop + 2 * n_preview + 1; ++previewWindow)
+      {
+        double t = previewWindow * dt;
+        steps->update(t);
+
+        if(!steps->isLastStep())
+        {
+          mc_planning::TimedStep<Eigen::Vector2d> prev, next;
+          benchmark::DoNotOptimize(prev = steps->previous());
+          benchmark::DoNotOptimize(next = steps->next());
+        }
+      }
+    }
+  }
+  state.SetItemsProcessed(PreviewStepsFixture::nIter() * state.iterations());
+}
+BENCHMARK_REGISTER_F(PreviewStepsFixture, BM_PreviewSteps)->Unit(benchmark::kMicrosecond);
+
+class GeneratorFixture : public benchmark::Fixture
+{
+public:
+  using Generator = mc_planning::generator;
+  using PreviewSteps = mc_planning::PreviewSteps<Eigen::Vector2d>;
+  using CenteredPreviewWindow = mc_planning::CenteredPreviewWindow;
+  GeneratorFixture() : preview(1.6, dt) {}
+
+  void SetUp(const ::benchmark::State &)
+  {
+    steps.add({preview.halfDuration(), {-0.2, 0.0}});
+    steps.addRelative({preview.halfDuration(), {0.0, 0.0}});
+    steps.addRelative({0.1, {0.0, 0.0}});
+    steps.addRelative({1.6, {0.0, 0.0}});
+    steps.addRelative({0.1, {0.2, 0.095}});
+    steps.addRelative({1.6, {0.0, 0.0}});
+    steps.addRelative({0.1, {0.0, -0.19}});
+    steps.addRelative({1.6, {0.0, 0.0}});
+    steps.addRelative({0.1, {-0.2, 0.095}});
+    steps.addRelative({preview.halfDuration(), {0.0, 0.0}});
+    steps.initialize();
+    generator = std::make_shared<Generator>(preview);
+    generator->steps(steps);
+
+    n_loop = preview.index(mc_planning::Time(steps.back().t()));
+  }
+
+  void TearDown(const ::benchmark::State &) {}
+
+  std::shared_ptr<Generator> generator;
+  CenteredPreviewWindow preview;
+  PreviewSteps steps;
+  static constexpr double dt = 0.005;
+  unsigned n_loop = 0.;
+  static constexpr unsigned n_preview = 1.6 / dt;
+};
+
+BENCHMARK_DEFINE_F(GeneratorFixture, BM_Generator)(benchmark::State & state)
+{
+  for(auto _ : state)
+  {
+    for(unsigned loop = 0; loop <= n_loop; loop++)
+    {
+      generator->generate(loop);
+    }
+  }
+  state.SetItemsProcessed(n_loop * state.iterations());
+}
+BENCHMARK_REGISTER_F(GeneratorFixture, BM_Generator)->Unit(benchmark::kMicrosecond);
+
+// Run the benchmark
+BENCHMARK_MAIN();

doc/CMakeLists.txt

@@ -8,3 +8,7 @@ configure_file(_plugins/doxygen.rb.in "${CMAKE_CURRENT_SOURCE_DIR}/_plugins/doxy
 install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/_data/schemas
   DESTINATION ${JSON_DOC_INSTALL}
   FILES_MATCHING PATTERN "*.json")
+
+install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/images
+  DESTINATION ${CMAKE_INSTALL_DOCDIR}/doxygen-html
+)