diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index b6d361e531..f73f23ccfe 100644 --- a/.github/workflows/build.yml +++ b/.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 "arn.tanguy@gmail.com" + 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 }} diff --git a/CMakeLists.txt b/CMakeLists.txt index ea8b35bf56..2794af7920 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/benchmarks/CMakeLists.txt b/benchmarks/CMakeLists.txt index dcc7d19823..6a35e07f42 100644 --- a/benchmarks/CMakeLists.txt +++ b/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) diff --git a/benchmarks/benchHMC.cpp b/benchmarks/benchHMC.cpp new file mode 100644 index 0000000000..84706bbf82 --- /dev/null +++ b/benchmarks/benchHMC.cpp @@ -0,0 +1,170 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#include +#include +#include +#include "benchmark/benchmark.h" +#include + +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(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 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 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; + +public: + void SetUp(const ::benchmark::State &) + { + steps = std::make_shared(); + 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 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 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; + 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(preview); + generator->steps(steps); + + n_loop = preview.index(mc_planning::Time(steps.back().t())); + } + + void TearDown(const ::benchmark::State &) {} + + std::shared_ptr 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(); diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 8d66832a1b..c88790712f 100644 --- a/doc/CMakeLists.txt +++ b/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 +) diff --git a/doc/images/generator_com_generation.svg b/doc/images/generator_com_generation.svg new file mode 100644 index 0000000000..8814985f96 --- /dev/null +++ b/doc/images/generator_com_generation.svg @@ -0,0 +1,2304 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/include/mc_planning/ClampedCubicSpline.h b/include/mc_planning/ClampedCubicSpline.h new file mode 100644 index 0000000000..e1527faf3c --- /dev/null +++ b/include/mc_planning/ClampedCubicSpline.h @@ -0,0 +1,74 @@ +/* + * Copyright (c) 2009, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#pragma once +#include + +namespace mc_planning +{ +namespace motion_interpolator +{ + +template +class ClampedCubicSpline : public CubicSplineBase +{ +protected: + void setSplineCoeff(unsigned int n_base, unsigned int n_segment, unsigned int n_offset); + +public: + ClampedCubicSpline(const double & scale = 1.0, const double & eps = 1.0e-3); +}; + +template +ClampedCubicSpline::ClampedCubicSpline(const double & scale, const double & eps) +: CubicSplineBase(scale, eps, CLAMPED_CUBIC_SPLINE) +{ +} + +template +void ClampedCubicSpline::setSplineCoeff(unsigned int n_base, unsigned int n_size, unsigned int n_offset) +{ + using namespace motion_interpolator; + + typename std::deque>::const_iterator iter = InterpolatorBase::data.begin() + n_offset; + + const double & data_dot_ini = iter->vel; + const double & data_dot_end = (iter + n_size - 1)->vel; + + if(n_size <= 2) + { + CubicSplineBase::coeff[n_base].resize(n_size * 2, 0); + return; + } + + CubicSplineBase::coeff[n_base].resize(n_size * 2); + CubicSplineBase::coeff[n_base][0] = -0.5; + CubicSplineBase::coeff[n_base][n_size] = + (3.0 / ((double)((iter + 1)->t - iter->t) * InterpolatorBase::scale)) + * (((iter + 1)->pos - iter->pos) / ((double)((iter + 1)->t - iter->t) * InterpolatorBase::scale) + - data_dot_ini); + for(unsigned int i = 1; i < n_size; i++) + { + CubicSplineBase::coeff[n_base][i] = 0.0; + CubicSplineBase::coeff[n_base][i + n_size] = 0.0; + } + const double qn = 0.5; + const double un = + (3.0 / ((double)((iter + n_size - 1)->t - (iter + n_size - 2)->t) * InterpolatorBase::scale)) + * (data_dot_end + - ((iter + n_size - 1)->pos - (iter + n_size - 2)->pos) + / ((double)((iter + n_size - 1)->t - (iter + n_size - 2)->t) * InterpolatorBase::scale)); + + CubicSplineBase::makeSplineTable(n_base, n_size, n_offset, qn, un); +} +} // end of namespace motion_interpolator +} // namespace mc_planning diff --git a/include/mc_planning/CubicSplineBase.h b/include/mc_planning/CubicSplineBase.h new file mode 100644 index 0000000000..bd91daf9e4 --- /dev/null +++ b/include/mc_planning/CubicSplineBase.h @@ -0,0 +1,328 @@ +/* + * Copyright (c) 2009, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#pragma once +#include "InterpolatorBase.h" +#include +#include + +namespace mc_planning +{ +namespace motion_interpolator +{ + +template +class CubicSplineBase : public InterpolatorBase +{ +protected: + std::vector> coeff; + + void getPointToPoint(T t_, double & p, double & v, double & a, unsigned int n_segment); + + virtual void makeSplineTable(unsigned int n_base, + unsigned int n_segment, + unsigned int n_offset, + const double & qn, + const double & un); + + virtual void setSplineCoeff(unsigned int n_base, unsigned int n_segment, unsigned int n_offset) = 0; + +public: + CubicSplineBase(const double & scale = 1.0, const double & eps = 1.0e-3, interpolator_type ip_type_ = NOT_SELECTED); + + void get(T t_, double & p, double & v, double & a); + + void update(void); + + void update(T t_); + + void update(T t_from, T t_to); +}; + +template +CubicSplineBase::CubicSplineBase(const double & scale, const double & eps, interpolator_type ip_type_) +: InterpolatorBase(scale, eps, ip_type_) +{ +} + +template +void CubicSplineBase::getPointToPoint(T t_, double & p, double & v, double & a, unsigned int n_segment) +{ + using namespace motion_interpolator; + + typename std::deque>::const_iterator iter = InterpolatorBase::data.begin() + n_segment; + + const T ti = iter->t; + const T tf = (iter + 1)->t; + const double t1 = (double)(tf - ti) * InterpolatorBase::scale; + + if(t1 < InterpolatorBase::eps) + { + p = iter->pos; + v = iter->vel; + a = iter->acc; + } + else + { + const double t2 = t1 * t1; + const double t3 = t1 * t2; + const double t4 = t1 * t3; + const double t5 = t1 * t4; + + const double & pi = iter->pos; + const double & vi = iter->vel; + const double & ai = iter->acc; + const double & pf = (iter + 1)->pos; + const double & vf = (iter + 1)->vel; + const double & af = (iter + 1)->acc; + + const double & c0 = pi; + const double & c1 = vi; + const double c2 = ai / 2.0; + const double c3 = (20.0 * (pf - pi) - (8.0 * vf + 12.0 * vi) * t1 + (af - 3.0 * ai) * t2) / (2.0 * t3); + const double c4 = (-30.0 * (pf - pi) + (14.0 * vf + 16.0 * vi) * t1 - (2.0 * af - 3.0 * ai) * t2) / (2.0 * t4); + const double c5 = (12.0 * (pf - pi) - 6.0 * (vf + vi) * t1 + (af - ai) * t2) / (2.0 * t5); + + const double tn = (double)(t_ - ti) * InterpolatorBase::scale; + p = c0 + (c1 + (c2 + (c3 + (c4 + c5 * tn) * tn) * tn) * tn) * tn; + v = c1 + (2.0 * c2 + (3.0 * c3 + (4.0 * c4 + 5.0 * c5 * tn) * tn) * tn) * tn; + a = 2.0 * c2 + (6.0 * c3 + (12.0 * c4 + 20.0 * c5 * tn) * tn) * tn; + } +} + +template +void CubicSplineBase::get(T t_, double & p, double & v, double & a) +{ + using namespace motion_interpolator; + + if(InterpolatorBase::data.empty()) return; + + if(InterpolatorBase::data.size() == 1) + { + p = InterpolatorBase::data.front().pos; + v = 0.0; + a = 0.0; + return; + } + + unsigned int n_base = 0; + unsigned int n_segment_coeff = 0; + unsigned int n_segment_data = 0; + typename std::deque>::const_iterator iter = InterpolatorBase::data.begin(); + if(t_ <= iter->t) + { + t_ = iter->t; + } + else if(t_ > InterpolatorBase::data.back().t) + { + n_base = coeff.size() - 1; + n_segment_coeff = coeff[n_base].size() / 2 - 2; + n_segment_data = InterpolatorBase::data.size() - 2; + iter = InterpolatorBase::data.end() - 2; + t_ = InterpolatorBase::data.back().t; + } + else + { + while(t_ > (iter + 1)->t) + { + if((iter + 1)->boundary) + { + n_base++; + n_segment_coeff = 0; + } + else + n_segment_coeff++; + + n_segment_data++; + iter++; + } + } + + if(coeff[n_base].size() <= 4) + { + getPointToPoint(t_, p, v, a, n_segment_data); + } + else + { + double h = (double)((iter + 1)->t - iter->t) * InterpolatorBase::scale; + if(h <= 0.0) + throw "Bad input to routine splint"; + else + { + double dhi = (double)((iter + 1)->t - t_) * InterpolatorBase::scale / h; + double dlo = (double)(t_ - iter->t) * InterpolatorBase::scale / h; + + p = dhi * iter->pos + dlo * (iter + 1)->pos + + ((dhi * dhi * dhi - dhi) * coeff[n_base][n_segment_coeff] + + (dlo * dlo * dlo - dlo) * coeff[n_base][n_segment_coeff + 1]) + * (h * h) / 6.0; + v = (-iter->pos + (iter + 1)->pos) / h + + ((-3.0 * dhi * dhi + 1.0) * coeff[n_base][n_segment_coeff] + + (3.0 * dlo * dlo - 1.0) * coeff[n_base][n_segment_coeff + 1]) + * h / 6.0; + a = dhi * coeff[n_base][n_segment_coeff] + dlo * coeff[n_base][n_segment_coeff + 1]; + } + } +} + +template +void CubicSplineBase::makeSplineTable(unsigned int n_base, + unsigned int n_size, + unsigned int n_offset, + const double & qn, + const double & un) +{ + using namespace motion_interpolator; + + typename std::deque>::const_iterator iter = InterpolatorBase::data.begin() + n_offset; + for(unsigned int i = 1; i < n_size - 1; i++) + { + double sig = ((double)((iter + i)->t - (iter + i - 1)->t)) / ((double)((iter + i + 1)->t - (iter + i - 1)->t)); + double p = sig * coeff[n_base][i - 1] + 2.0; + coeff[n_base][i] = (sig - 1.0) / p; + coeff[n_base][i + n_size] = ((iter + i + 1)->pos - (iter + i)->pos) + / ((double)((iter + i + 1)->t - (iter + i)->t) * InterpolatorBase::scale) + - ((iter + i)->pos - (iter + i - 1)->pos) + / ((double)((iter + i)->t - (iter + i - 1)->t) * InterpolatorBase::scale); + coeff[n_base][i + n_size] = (6.0 * coeff[n_base][i + n_size] + / ((double)((iter + i + 1)->t - (iter + i - 1)->t) * InterpolatorBase::scale) + - sig * coeff[n_base][i - 1 + n_size]) + / p; + } + + coeff[n_base][n_size - 1] = (un - qn * coeff[n_base][n_size - 2 + n_size]) / (qn * coeff[n_base][n_size - 2] + 1.0); + for(int j = n_size - 2; j >= 0; j--) + coeff[n_base][j] = coeff[n_base][j] * coeff[n_base][j + 1] + coeff[n_base][j + n_size]; +} + +template +void CubicSplineBase::update(void) +{ + using namespace motion_interpolator; + + unsigned int n_base_max = 1; + for(typename std::deque>::const_iterator iter = InterpolatorBase::data.begin() + 1; + iter != InterpolatorBase::data.end() - 1; iter++) + { + if(iter->boundary) n_base_max++; + } + if(coeff.size() < n_base_max) coeff.resize(n_base_max); + + unsigned int n_base = 0; + unsigned int n_size = 2; + unsigned int n_data = 2; + for(typename std::deque>::const_iterator iter = InterpolatorBase::data.begin() + 1; + iter != InterpolatorBase::data.end(); iter++) + { + if(iter->boundary || (iter == InterpolatorBase::data.end() - 1)) + { + setSplineCoeff(n_base, n_size, n_data - n_size); + n_base++; + n_size = 2; + } + else + n_size++; + + n_data++; + } +} + +template +void CubicSplineBase::update(T t_) +{ + using namespace motion_interpolator; + + unsigned int n_base_max = 1; + for(typename std::deque>::const_iterator iter = InterpolatorBase::data.begin() + 1; + iter != InterpolatorBase::data.end() - 1; iter++) + { + if(iter->boundary) n_base_max++; + } + if(coeff.size() < n_base_max) coeff.resize(n_base_max); + + unsigned int n_base = 0; + unsigned int n_size = 0; + unsigned int n_data = 0; + typename std::deque>::const_iterator iter = InterpolatorBase::data.begin(); + while((iter + 2 != InterpolatorBase::data.end()) && ((t_ > (iter + 1)->t) || !(iter + 1)->boundary)) + { + if((iter + 1)->boundary) + { + n_base++; + n_size = 0; + } + else + n_size++; + + n_data++; + iter++; + } + + setSplineCoeff(n_base, n_size + 2, n_data - n_size); +} + +template +void CubicSplineBase::update(T t_from, T t_to) +{ + using namespace motion_interpolator; + + unsigned int n_base_max = 1; + for(typename std::deque>::const_iterator iter = InterpolatorBase::data.begin() + 1; + iter != InterpolatorBase::data.end() - 1; iter++) + { + if(iter->boundary) n_base_max++; + } + if(coeff.size() < n_base_max) coeff.resize(n_base_max); + + unsigned int n_base = 0; + unsigned int n_size = 0; + unsigned int n_data = 0; + typename std::deque>::const_iterator iter = InterpolatorBase::data.begin(); + while((iter + 2 != InterpolatorBase::data.end()) && ((t_from > (iter + 1)->t) || !(iter + 1)->boundary)) + { + if((iter + 1)->boundary) + { + n_base++; + n_size = 0; + } + else + n_size++; + + n_data++; + iter++; + } + + setSplineCoeff(n_base, n_size + 2, n_data - n_size); + + unsigned int n_base_to = n_base; + unsigned int n_size_to = n_size; + unsigned int n_data_to = n_data; + iter = InterpolatorBase::data.begin() + n_data; + while((iter + 2 != InterpolatorBase::data.end()) && ((t_to > (iter + 1)->t) || !(iter + 1)->boundary)) + { + if((iter + 1)->boundary) + { + if(n_base_to != n_base) setSplineCoeff(n_base_to, n_size_to + 2, n_data_to - n_size_to); + n_base_to++; + n_size_to = 0; + } + else + n_size_to++; + + n_data_to++; + iter++; + } + + if(n_base_to != n_base) setSplineCoeff(n_base_to, n_size_to + 2, n_data_to - n_size_to); +} +} // namespace motion_interpolator +} // namespace mc_planning diff --git a/include/mc_planning/InterpolatorBase.h b/include/mc_planning/InterpolatorBase.h new file mode 100644 index 0000000000..db0f62e710 --- /dev/null +++ b/include/mc_planning/InterpolatorBase.h @@ -0,0 +1,298 @@ +#pragma once +#include + +namespace mc_planning +{ +namespace motion_interpolator +{ + +typedef enum +{ + NOT_SELECTED, + // POINT_TO_POINT, + // POINT_TO_POINT_EXP, + // POINT_TO_POINT_CUBIC, + // POINT_TO_POINT_TRAPEZOID_VELOCITY_PROFILES, + // HOFFARBIB_MODEL, + // NATURAL_CUBIC_SPLINE, + CLAMPED_CUBIC_SPLINE, + // AKIMA_CUBIC_SPLINE, + // KOCHANEK_BARTELS_SPLINE, + // BSPLINE_OPEN_UNIFORM_KNOT, + // BSPLINE_OPEN_UNIFORM_KNOT_2ND, + // BSPLINE_OPEN_UNIFORM_KNOT_3RD, + // SINGLE_VIA_POINT, + // DOUBLE_VIA_POINTS, + // SEQUENTIAL_CUBIC_SPLINE, + // SEQUENTIAL_QUINTIC_SPLINE, + // TRAPEZOID_VELOCITY_PROFILES, + // STOP_EXP, + // STOP_QUINTIC_POLYNOMIALS, + // MONOTONIC_INCREASING_EXP +} interpolator_type; + +template +class InterpolatorDataType +{ +public: + bool boundary; + T t; + double pos; + double vel; + double acc; + + InterpolatorDataType(T t_ = 0.0, double pos_ = 0.0, double vel_ = 0.0, double acc_ = 0.0, bool boundary_ = false) + : boundary(boundary_), t(t_), pos(pos_), vel(vel_), acc(acc_) + { + } + + InterpolatorDataType(const InterpolatorDataType & obj) + : boundary(obj.boundary), t(obj.t), pos(obj.pos), vel(obj.vel), acc(obj.acc) + { + } +}; + +template +class InterpolatorBase +{ +private: + interpolator_type ip_type; + +protected: + const double scale; + const double eps; + + std::deque> data; + +public: + InterpolatorBase(const double & scale_ = 1.0, const double & eps = 0.001, interpolator_type ip_type = NOT_SELECTED); + + virtual ~InterpolatorBase() {} + + inline const interpolator_type & InterpolatorType(void) const + { + return ip_type; + } + + inline void clear(void) + { + data.clear(); + } + + inline unsigned int size(void) + { + return data.size(); + } + + inline void resize(unsigned int n) + { + data.resize(n); + } + + inline bool empty(void) + { + return data.empty(); + } + + inline std::deque> & getData(void) + { + return data; + } + + inline InterpolatorDataType & getData(int n) + { + return data[n]; + } + + inline InterpolatorDataType & front(void) + { + return data.front(); + } + + inline InterpolatorDataType & back(void) + { + return data.back(); + } + + inline virtual bool isFinish(const T & t_) + { + return (data.empty() || (data.back().t <= t_)) ? true : false; + } + + inline void pop_front(void) + { + if(!data.empty()) data.pop_front(); + } + + inline void pop_back(void) + { + if(!data.empty()) data.pop_back(); + } + + inline void push_back(const T & t_, const double & p) + { + data.push_back(InterpolatorDataType(t_, p)); + } + + inline void push_back(const T & t_, const double & p, const double & v) + { + data.push_back(InterpolatorDataType(t_, p, v)); + } + + inline void push_back(const T & t_, const double & p, const double & v, const double & a) + { + data.push_back(InterpolatorDataType(t_, p, v, a)); + } + + inline void push_back(const T & t_, const double & p, bool boundary) + { + data.push_back(InterpolatorDataType(t_, p, 0.0, 0.0, boundary)); + } + + inline void push_back(const T & t_, const double & p, const double & v, bool boundary) + { + data.push_back(InterpolatorDataType(t_, p, v, 0.0, boundary)); + } + + inline void push_back(const T & t_, const double & p, const double & v, const double & a, bool boundary) + { + data.push_back(InterpolatorDataType(t_, p, v, a, boundary)); + } + + inline void push_front(const T & t_, const double & p) + { + data.push_front(InterpolatorDataType(t_, p)); + } + + inline void push_front(const T & t_, const double & p, const double & v) + { + data.push_front(InterpolatorDataType(t_, p, v)); + } + + inline void push_front(const T & t_, const double & p, const double & v, const double & a) + { + data.push_front(InterpolatorDataType(t_, p, v, a)); + } + + inline void push_front(const T & t_, const double & p, bool boundary) + { + data.push_front(InterpolatorDataType(t_, p, 0.0, 0.0, boundary)); + } + + inline void push_front(const T & t_, const double & p, const double & v, bool boundary) + { + data.push_front(InterpolatorDataType(t_, p, v, 0.0, boundary)); + } + + inline void push_front(const T & t_, const double & p, const double & v, const double & a, bool boundary) + { + data.push_front(InterpolatorDataType(t_, p, v, a, boundary)); + } + + void set(const int n, const T & t, const double & p); + + void set(const int n, const T & t, const double & p, const double & v); + + void set(const int n, const T & t, const double & p, const double & v, const double & a); + + virtual void set(const int n, const T & t, const double & p, bool boundary); + + virtual void set(const int n, const T & t, const double & p, const double & v, bool boundary); + + virtual void set(const int n, const T & t, const double & p, const double & v, const double & a, bool boundary); + + double getValue(T t_); + + void get(T t_, double & p); + + void get(T t_, double & p, double & v); + + virtual void get(T t_, double & p, double & v, double & a) = 0; + + virtual void update(void){}; + + virtual void update(T t_){}; + + virtual void update(T t_from, T t_to){}; +}; + +template +InterpolatorBase::InterpolatorBase(const double & scale_, const double & eps_, interpolator_type ip_type_) +: ip_type(ip_type_), scale(scale_), eps(eps_) +{ +} + +template +void InterpolatorBase::set(int n, const T & t_, const double & p) +{ + data[n].t = t_; + data[n].pos = p; +} + +template +void InterpolatorBase::set(int n, const T & t_, const double & p, const double & v) +{ + data[n].t = t_; + data[n].pos = p; + data[n].vel = v; +} + +template +void InterpolatorBase::set(int n, const T & t_, const double & p, const double & v, const double & a) +{ + data[n].t = t_; + data[n].pos = p; + data[n].vel = v; + data[n].acc = a; +} + +template +void InterpolatorBase::set(int n, const T & t_, const double & p, bool boundary) +{ + data[n].t = t_; + data[n].pos = p; + data[n].boundary = boundary; +} + +template +void InterpolatorBase::set(int n, const T & t_, const double & p, const double & v, bool boundary) +{ + data[n].t = t_; + data[n].pos = p; + data[n].vel = v; + data[n].boundary = boundary; +} + +template +void InterpolatorBase::set(int n, const T & t_, const double & p, const double & v, const double & a, bool boundary) +{ + data[n].t = t_; + data[n].pos = p; + data[n].vel = v; + data[n].acc = a; + data[n].boundary = boundary; +} + +template +void InterpolatorBase::get(T t_, double & p) +{ + double v, a; + get(t_, p, v, a); +} + +template +void InterpolatorBase::get(T t_, double & p, double & v) +{ + double a; + get(t_, p, v, a); +} + +template +double InterpolatorBase::getValue(T t_) +{ + double p; + get(t_, p); + return p; +} +} // namespace motion_interpolator +} // namespace mc_planning diff --git a/include/mc_planning/LIPMControlByPoleAssign.h b/include/mc_planning/LIPMControlByPoleAssign.h new file mode 100644 index 0000000000..0d1fa46cd0 --- /dev/null +++ b/include/mc_planning/LIPMControlByPoleAssign.h @@ -0,0 +1,33 @@ +#pragma once +#include + +namespace mc_planning +{ +namespace linear_control_system +{ + +class MC_PLANNING_DLLAPI LIPMControlByPoleAssign : public LinearControl3 +{ +public: + void setStateVariables(double x_, double v_, double p_) + { + x << x_, v_, p_; + } + void getStateVariables(double & x_, double & v_, double & p_) + { + x_ = x(0); + v_ = x(1); + p_ = x(2); + } + void getStateVariables(double & x_, double & v_, double & p_, double & pdot) + { + x_ = x(0); + v_ = x(1); + p_ = x(2); + pdot = u; + } + void setSystemMatrices(const double alpha, const double beta, const double gamma, const double cog_height); +}; + +} // namespace linear_control_system +} // namespace mc_planning diff --git a/include/mc_planning/LIPMControlByPoleAssignWithExternalForce.h b/include/mc_planning/LIPMControlByPoleAssignWithExternalForce.h new file mode 100644 index 0000000000..00b33c3314 --- /dev/null +++ b/include/mc_planning/LIPMControlByPoleAssignWithExternalForce.h @@ -0,0 +1,29 @@ +#pragma once +#include "LIPMControlByPoleAssign.h" + +namespace mc_planning +{ +namespace linear_control_system +{ + +class MC_PLANNING_DLLAPI LIPMControlByPoleAssignWithExternalForce : public LIPMControlByPoleAssign +{ +private: + Eigen::Vector3d B2; + +public: + LIPMControlByPoleAssignWithExternalForce(void); + + void Initialize(void); + + void update(const double Fext, const double dt); + void update(const Eigen::Vector3d & x_ref, const double Fext, const double dt); + void setSystemMatrices(const double alpha, + const double beta, + const double gamma, + const double omega, + const double total_mass); +}; + +} // namespace linear_control_system +} // namespace mc_planning diff --git a/include/mc_planning/LinearControl3.h b/include/mc_planning/LinearControl3.h new file mode 100644 index 0000000000..1bb697983d --- /dev/null +++ b/include/mc_planning/LinearControl3.h @@ -0,0 +1,52 @@ +/* + * Copyright (c) 2009, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#pragma once +#include +#include + +namespace mc_planning +{ +namespace linear_control_system +{ + +// linear control for siso system +struct MC_PLANNING_DLLAPI LinearControl3 +{ + LinearControl3(); + void Initialize(); + virtual void update(); + virtual void update(const Eigen::Vector3d & x_ref_); + virtual void update(const double dt); + virtual void update(const Eigen::Vector3d & x_ref_, const double dt); + void setReference(const Eigen::Vector3d & x_ref_) + { + x_ref = x_ref_; + }; + void setReference(double x1, double x2, double x3) + { + x_ref << x1, x2, x3; + }; + +protected: + // system matrices and feedback gain + Eigen::Matrix3d A; + Eigen::Vector3d B; + Eigen::RowVector3d C; + Eigen::Vector3d K; + Eigen::Vector3d x; ///< state variable + Eigen::Vector3d x_ref; ///< reference state variable + double y; ///< output + double u; ///< input +}; +} // namespace linear_control_system +} // namespace mc_planning diff --git a/include/mc_planning/LinearTimeVariantInvertedPendulum.h b/include/mc_planning/LinearTimeVariantInvertedPendulum.h new file mode 100644 index 0000000000..6044fc967e --- /dev/null +++ b/include/mc_planning/LinearTimeVariantInvertedPendulum.h @@ -0,0 +1,248 @@ +/* + * Copyright (c) 2017, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace mc_planning +{ + +namespace linear_control_system +{ + +/** + * @brief Reference CoM position and velocity trajectory generation. + * + * Implementation of the Linear Time-Varying System (LTVS) introduced in Sec III.A of: + * + * **Online 3D CoM Trajectory Generation for Multi-Contact Locomotion Synchronizing Contact**, *Mitsuharu Morisawa et + * al., IROS 2018* + * + * Computes the horizontal long-term trajectory of the CoM along either sagittal or frontal plane. + * + * The discretized system of the centroidal dynamics in the sagital plane is obtained as + * + * \f[ + * x_{k+1} = A_k x_k + B_k u_k + * \f] + * + * where + * + * - \f[A_k = \begin{bmatrix} + * cosh(\omega_k\delta T) & \frac{sinh(\omega_k\delta T)}{\omega_k} \\ + * \omega_k sinh(\omega_k\delta T) & cosh(\omega_k \delta T) + * \end{bmatrix} + * \f] + * - CoM state position/velocity along the sagital plane + * \f[ x_k = [x_{G,k}, \dot{x}_{G,k}] \f] + * - \f[ B_k = \begin{bmatrix} + * 1- cosh(\omega_k\delta T) \\ + * - \omega_k sinh(\omega_k\delta T) + * \end{bmatrix} + * \f] + * - Desired Centroidal Momentum Pivot (CMP) [equivalent to the ZMP]. \f$ L \f$ is the number of contact links. + * \f[ u_k = \sum^L_{i=1} \alpha_{z_i,k}p_{x_i,k} - \frac{\sigma_{y,k}}{m(g+\ddot{z}_{G,k})} \f] + * - \f[ + * \omega_k = + * \sqrt{\frac{g+\ddot{z}_{G,k}}{z_{G,k}-\sum_{i=1}^{L}\alpha_{x-i,k}p_{z_i,l}}} + * \f] + * + * This system is used to compute the reference position and velocity of the CoM + * for a preview window: + * - of size \f$ N_p = 2*N+1 \f$ centered around the current time \f$ [-N,\mbox{current},N] \f$ + * - this is represented internally by arrays indexed from \f$ [0...M] \f$ with \f$ M = 2*N \f$ with the current time + * being at index \f$ N \f$ + * + * **Expects the following to be defined externally**: + * - \f$ [\omega^2_0...\omega_M^2] \f$ defined by w2() + * - Desired CMP trajectory: \f$ [u_0...u_M] \f$ defined by p_ref() + * + * The CoM state \f$ x_F \f$ after the F-th future step is + * + * \f[ + * x_F = \Phi(F,0)x_0 + \sum_{i=0}^{F-1}\Phi(F,i+1)B_i u_i + * \f] + * + * where + * + * \f[ + * \Phi(k,j) = + * \left\{ + * \begin{array}{ll} + * A_{k-1}A_{k-2}...A_j &\mbox{if } k>j \\ + * I_2 & \mbox{otherwise }(k=j=F) + * \end{array} + * \right. + * \f] + * + * Those are computed by update() as m_An, m_Bn, and the corresponding trajectory as m_X + */ +struct MC_PLANNING_DLLAPI LinearTimeVariantInvertedPendulum +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * @brief Initialization + * + * @param dt Timestep + * @param n_preview Number of future preview elements. The full size of the + * preview windows from past to future will be (2*n_preview+1) + * @param minHeight for the pendulum (used for LookupTable optimizations) + * @param maxHeight for the pendulum + * @param weight_resolution Number of data points to pre-compute for + * \f$ \omega, cosh(\omega), sinh(\omega) \f$ for height in a given range + * [minHeight, maxHeight]. This is used to optimize the otherwise costly computation of these + * functions. + */ + LinearTimeVariantInvertedPendulum(const CenteredPreviewWindow & window, + unsigned weight_resolution = 20000, + double minHeight = 0.01, + double maxHeight = 2.5); + + /** + * @brief Initialize the discretized system matrices with constant pendulum height + * + * Initalizes \f$ A_k \f$, \f$ B_k \f$ + * + * @param waist_height Pendulum height (constant over the preview window) + */ + void initMatrices(double waist_height); + + /** + * @brief Computes the reference CoM state online (position and velocity) + * + * Fills m_A, m_B, m_An, m_Bn, m_X + * Requires p_ref() and w2() to be filled first. + */ + void update(); + + /** + * @brief Generated CoM/ZMP state + */ + struct State + { + double cog_pos; ///< CoM position + double cog_vel; ///< CoM velocity + double cog_acc; ///< CoM acceleration + double p; ///< ZMP position + double pdot; ///< ZMP velocity + }; + + /*! @brief Get generated CoG/ZMP states + * @param n_time is the time in range of[-n_preview:n_preview], current time is 0 + */ + State getState(unsigned n_time) const; + + /*! @brief to get generated CoG/ZMP states + * @param[in] n_time is the time in range of[-n_preview:n_preview], current time is 0 + * @param[out] cog_pos is position of COG + * @param[out] cog_vel is velocity of COG + * @param[out] cog_acc is acceleration of COG + * @param[in] p is position of ZMP + * @param[in] pdot is velocity of ZMP + */ + void getState(unsigned n_time, double & cog_pos, double & cog_vel, double & cog_acc, double & p, double & pdot); + + /** + * Generate full trajectory (only used for tests) + */ + void generate(Eigen::VectorXd & cog_pos, + Eigen::VectorXd & cog_vel, + Eigen::VectorXd & cog_acc, + Eigen::VectorXd & p_ref); + + /** Reference CMP along the trajectory \f$ [u_0..u_M] \f$ + * + * @return Reference to the CMP array + */ + Eigen::VectorXd & p_ref() + { + return m_p_ref; + } + + /** + * @brief Value of \f$ u_k \f$ + * + * @param k Index of the value in range \f$ [0, M] \f$ + */ + double p_ref(unsigned k) + { + return m_p_ref[k]; + } + + /** Squared values of \f$ \omega_k \f$ along the preview time: \f$ [\omega_0^2 ... \omega_M^2] \f$ + */ + Eigen::VectorXd & w2() + { + return m_w2; + } + + /** + * @brief Value of \f$ \omega_k^2 \f$ + * + * @param k Index of the value in range \f$ [0, M] \f$ + */ + double & w2(unsigned k) + { + return m_w2[k]; + } + const Eigen::VectorXd & w() const + { + return m_w; + } + /** + * @brief Value of \f$ \omega_k \f$ + * + * @param k Index of the value in range \f$ [0, M] \f$ + */ + double w(unsigned k) const + { + return m_w[k]; + } + +protected: + const CenteredPreviewWindow window_; + using Matrix22 = Eigen::Matrix; + using Vector2 = Eigen::Matrix; + + boost::circular_buffer> m_A; ///< \f$ A_k \f$ + boost::circular_buffer> m_B; ///< \f$ B_k \f$ + std::vector> m_An; ///< \f$ \Phi(k, j) \f$ + std::vector> m_Bn; ///< \f$ \sum_{i=0}^{F-1}\Phi(F,i+1)B_i \f$ + std::vector> + m_X; ///< Generated trajectory for the CoM (position, velocity) + + Eigen::VectorXd m_p_ref; ///< \f$ [u_0,...,u_M] \f$ + + Eigen::VectorXd m_w2; ///< \f$ \omega_k^2 \f$ + Eigen::VectorXd m_w; ///< \f$ \omega \f$ + + /** + * @name Lookup tables for optimized computation of cosh, sinh and sqrt + **/ + ///@{ + LookupTable wTable_; ///< Pre-computed table from \f$ \omega^2 \rightarrow sqrt(\omega^2) \f$ + LookupTable chkTable_; ///< Pre-computed table from \f$ \omega^2 \rightarrow cosh(sqrt(\omega^2) * dt) \f$ + LookupTable shkTable_; ///< Pre-computed table from \f$ \omega^2 \rightarrow sinh(sqrt(\omega^2) * dt) \f$ + ///@} +}; + +} // namespace linear_control_system + +} // namespace mc_planning diff --git a/include/mc_planning/LookupTable.h b/include/mc_planning/LookupTable.h new file mode 100644 index 0000000000..2a52f2c8f3 --- /dev/null +++ b/include/mc_planning/LookupTable.h @@ -0,0 +1,147 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +#include +#include +#include + +namespace mc_planning +{ +/** + * @brief Stores precomputed values of a function for fast lookup + * @tparam T Data type for the function arguments and result value + * @tparam CheckBounds Whether to check out-of-bounds access at runtime + * + * This class provides a lookup-table implementation intended for fast evaluation of + * computationally expensive functions. This is done by pre-generating a table + * of all possible function results in a given range/resolution, and only + * accessing the stored values instead of computing the function each time. + * + * On construction, the provided funciton is evaluated for values of `x` linearely distributed between + * `min` and `max`. \f[ [f(min), ..., f(x), ..., f(max)] \in T^\mbox{resolution} \f] + * Function values \f$ f(x) \f$ may be retrieved by calling the operator()(const T & value) + * + * @warning Notes regarding accuracy. + * - Due to the discretization, precision depends heavily on the chosen + * resolution and the function evaluated. Higher resolution improves accuracy + * at the expanse of memory usage (`resolution*sizeof(T)`). Conversely, lower + * resolution decreases accuracy and memory usage. + * - Accuracy may be lower near the upper bound due to numerical errors in + * linear generation of values in the min-max range. Whenever possible, it is + * recommended to choose a conservative upper boundary. + * - Always rigourously evaluate accuracy for each specific use-case. + * - This implementation makes no assumption on the underlying structure of the + * function. Better results and perfomance could be achieved by exploiting + * these properties (such as symetry, non-uniform argument distribution, etc) + */ +template +struct MC_PLANNING_DLLAPI LookupTable +{ + /** + * @brief Evaluate and store given function results + * @see create(size_t resolution, const T & min, const T & max, MappingFunction f) + */ + template + LookupTable(size_t resolution, const T & min, const T & max, MappingFunction f, const std::string & name = "") + { + create(resolution, min, max, f, name); + } + + /** + * @brief Default constructor + * + * To initialize the lookup table, call create(size_t resolution, const T & min, const T & max, MappingFunction f) + */ + LookupTable() noexcept {}; + + /** + * @brief Evaluate and store given function results + * + * \f[ [f(min), ..., f(max)] \in T^\mbox{resolution} \f] + * + * @param resolution Number of points to compute + * @param min Minimum argument value + * @param max Maximum argument value + * @param f Function to evaluate + * @param name Human-readable description of the stored function (used for + * error reporting) + */ + template + void create(size_t resolution, const T & min, const T & max, MappingFunction f, const std::string & name = "") + { + name_ = name; + prefix_ = "[LookupTable]" + name_.size() ? "[" + name_ + "] " : " "; + if(min > max) + { + mc_rtc::log::error_and_throw(prefix_ + "Invalid range (min {} > max {})", min, max); + } + if(resolution == 0) + { + mc_rtc::log::error_and_throw(prefix_ + + "Resolution cannot be equal to zero (strictly positive)"); + } + table_.resize(resolution); + min_ = min; + max_ = max; + T oldrange = static_cast(resolution); + T newrange = (max - min); + T fracRange = newrange / oldrange; + for(size_t i = 0; i < resolution; ++i) + { + table_[i] = f((static_cast(i) * fracRange) + min); + } + rangeConversion_ = static_cast(table_.size()) / (max_ - min_); + maxIndex_ = table_.size() - 1; + } + + /** + * @brief Retrieves the computed value \f$ f(x) \f$ + * + * @note Accuracy depends on the chosen resolution. + * + * @throws std::runtime_error + * If CheckBounds=true and x is out of bounds (x < min or x > max). + * + * @param x Value to be retrieved. x must be between \f$ [min, max] \f$. + * + * @return Closest pre-computed value of \f$ f(x) \f$ + */ + const T & operator()(const T & x) const + { + if(CheckBounds) + { + if(table_.empty() || x < min_ || x > max_) + { + if(table_.empty()) + { + mc_rtc::log::error_and_throw( + prefix_ + "Uninitialized table. Please call create() before use.", min_, x, max_); + } + else + { + mc_rtc::log::error_and_throw(prefix_ + "Out of bound access ({} <= {} <= {})", min_, x, + max_); + } + } + } + else + { + assert(table_.empty() || x < min_ || x > max_); + } + auto h = std::min(static_cast(lround((x - min_) * rangeConversion_)), maxIndex_); + return table_[h]; + } + +protected: + std::vector table_; + T min_, max_; + T rangeConversion_; + size_t maxIndex_ = 0; + std::string name_ = {}; + std::string prefix_ = {}; +}; + +} // namespace mc_planning diff --git a/include/mc_planning/MathFunction.h b/include/mc_planning/MathFunction.h new file mode 100644 index 0000000000..36c445d321 --- /dev/null +++ b/include/mc_planning/MathFunction.h @@ -0,0 +1,205 @@ +/* + * Copyright (c) 2009, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#pragma once +#include + +namespace mc_planning +{ + +const double epsilonAngle = 1e-16; + +template +inline T Max(const T & a, const T & b) +{ + return ((a) > (b) ? (a) : (b)); +} + +template +inline T Min(const T & a, const T & b) +{ + return ((a) < (b) ? (a) : (b)); +} + +template +inline int Sign(const T & x) +{ + return ((x < 0) ? (-1) : ((x > 0) ? 1 : 0)); +} + +template +inline T Pow2(const T & a) +{ + return dot((a), (a)); +} + +template +inline T polynomial3(const T & t) +{ + return ((3.0 - 2.0 * (t)) * (t) * (t)); +} + +template +inline T dpolynomial3(const T & t) +{ + return ((6.0 - 6.0 * (t)) * (t)); +} + +template +inline T ddpolynomial3(const T & t) +{ + return (6.0 - 12.0 * (t)); +} + +template +inline T polynomial5(const T & t) +{ + return (((6.0 * (t)-15.0) * (t) + 10.0) * (t) * (t) * (t)); +} + +template +inline T dpolynomial5(const T & t) +{ + return (((30.0 * (t)-60.0) * (t) + 30.0) * (t) * (t)); +} + +template +inline T ddpolynomial5(const T & t) +{ + return (((120.0 * (t)-180.0) * (t) + 60.0) * (t)); +} + +#if defined(__GNUC__) +# define __ATTRIBUTE_ALWAYS_INLINE__ __attribute__((always_inline)) +#else +# define __ATTRIBUTE_ALWAYS_INLINE__ +#endif + +inline Eigen::Matrix3d __ATTRIBUTE_ALWAYS_INLINE__ rotFromRoll(const double & roll) +{ + const double c = cos(roll); + const double s = sin(roll); + Eigen::Matrix3d r; + r << 1.0, 0, 0, 0, c, -s, 0, s, c; + return r; +} + +inline Eigen::Matrix3d __ATTRIBUTE_ALWAYS_INLINE__ rotFromPitch(const double & pitch) +{ + const double c = cos(pitch); + const double s = sin(pitch); + Eigen::Matrix3d r; + r << c, 0, s, 0, 1.0, 0, -s, 0, c; + return r; +} + +inline Eigen::Matrix3d __ATTRIBUTE_ALWAYS_INLINE__ rotFromYaw(const double & yaw) +{ + const double c = cos(yaw); + const double s = sin(yaw); + Eigen::Matrix3d r; + r << c, -s, 0, s, c, 0, 0, 0, 1.0; + return r; +} + +inline Eigen::Matrix3d __ATTRIBUTE_ALWAYS_INLINE__ rotFromYrp(const double & y, const double & r, const double & p) +{ + return (Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()) + * Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())) + .toRotationMatrix(); +} + +inline Eigen::Matrix3d __ATTRIBUTE_ALWAYS_INLINE__ rotFromAngleAxis(const double & angles, const Eigen::Vector3d & axis) +{ + return Eigen::AngleAxisd(angles, axis).toRotationMatrix(); + // return rodrigues(axis, angles); +} + +inline Eigen::Matrix3d __ATTRIBUTE_ALWAYS_INLINE__ rotFromAngleAxis(const Eigen::Vector3d & omega, + const double eps = 1.0e-6) +{ +#if 0 + Eigen::Vector3d w(omega); + for( int axis = 0 ; axis <= 2 ; axis++ ){ + while( w(axis) > M_PI ) w(axis) -= M_PI; + while( w(axis) < -M_PI ) w(axis) += M_PI; + } + double th = w.norm(); + return (th > eps) ? Eigen::AngleAxisd(th, w.normalized()).toRotationMatrix() : Eigen::Matrix3d::Identity(); +#else + double th = omega.norm(); + return (th > eps) ? Eigen::AngleAxisd(th, omega.normalized()).toRotationMatrix() : Eigen::Matrix3d::Identity(); + // return (th > eps) ? rodrigues(omega.normalized(), th) : Eigen::Matrix3d::Identity(); +#endif +} + +inline Eigen::Matrix3d __ATTRIBUTE_ALWAYS_INLINE__ rotFromAngleAxis(double e1, + double e2, + double e3, + const double eps = 1.0e-6) +{ + return rotFromAngleAxis(Eigen::Vector3d(e1, e2, e3), eps); +} + +/// !!! yrpFromRot can't check singularity (p = pi/2) +Eigen::Vector3d yrpFromRot(const Eigen::Matrix3d & m); +double yawFromRot(const Eigen::Matrix3d & r); +Eigen::Vector3d omegaFromRotApproximation(const Eigen::Matrix3d & r); + +inline Eigen::Matrix3d mergeTiltWithYaw(const Eigen::Vector3d & Rtez, const Eigen::Matrix3d & R2) +{ + + Eigen::Matrix3d R_temp1, R_temp2; + const Eigen::Vector3d & ez = Eigen::Vector3d::UnitZ(); + const Eigen::Vector3d & v1 = Rtez; + Eigen::Vector3d mlxv1 = (R2.transpose() * Eigen::Vector3d::UnitX()).cross(v1); + double n2 = mlxv1.squaredNorm(); + + if(n2 > epsilonAngle * epsilonAngle) + { + R_temp1 << -Eigen::Vector3d::UnitY(), Eigen::Vector3d::UnitX(), ez; + mlxv1 /= sqrt(n2); + R_temp2 << mlxv1.transpose(), v1.cross(mlxv1).transpose(), v1.transpose(); + return R_temp1 * R_temp2; + } + else + { + mlxv1 = (R2.transpose() * Eigen::Vector3d::UnitY()).cross(v1).normalized(); + R_temp2 << mlxv1.transpose(), v1.cross(mlxv1).transpose(), v1.transpose(); + return R_temp2.transpose(); + } +} + +inline Eigen::Matrix3d mergeRoll1Pitch1WithYaw2(const Eigen::Matrix3d & R1, const Eigen::Matrix3d & R2) +{ + return mergeTiltWithYaw(R1.transpose() * Eigen::Vector3d::UnitZ(), R2); +} + +double Saturation(const double & data, const double ulimit, const double llimit); +double Threshold(const double & data, const double ulimit, const double llimit); +double NormalizedTrapezoidCurve(int n_now, int n_ini, int n_acc, int n_dec, int n_end); + +template +T LowPassFilter(const T & data, T & data_lpf, const double gain) +{ + data_lpf += (data - data_lpf) * gain; + return data_lpf; +} + +template +T HighPassFilter(const T & data, T & data_hpf, const double gain) +{ + data_hpf += (data - data_hpf * (1.0 - gain)); + return data_hpf; +} + +} // namespace mc_planning diff --git a/include/mc_planning/PreviewSteps.h b/include/mc_planning/PreviewSteps.h new file mode 100644 index 0000000000..8e1ede8181 --- /dev/null +++ b/include/mc_planning/PreviewSteps.h @@ -0,0 +1,444 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once +#include +#include +#include +#include + +namespace mc_planning +{ + +template +struct TimedStep +{ + TimedStep(double t, const Step & step) noexcept : t_(t), step_(step) {} + TimedStep() noexcept {} + + const double & t() const noexcept + { + return t_; + } + + double & t() noexcept + { + return t_; + } + + const Step & step() const noexcept + { + return step_; + } + + Step & step() noexcept + { + return step_; + } + + // For std::set + bool operator<(const TimedStep & t) const noexcept + { + return (this->t_ < t.t_); + } + + bool operator<=(const TimedStep & t) const noexcept + { + return (this->t_ <= t.t_); + } + + void operator+(const TimedStep & t) noexcept + { + t_ += t.t(); + step_ += t.step(); + } + + TimedStep & operator+=(const TimedStep & rhs) noexcept + { + t_ += rhs.t(); + step_ += rhs.step(); + return *this; + } + + void operator-(const TimedStep & t) noexcept + { + t_ += t.t(); + step_ += t.step(); + } + + TimedStep & operator-=(const TimedStep & rhs) noexcept + { + t_ -= rhs.t(); + step_ -= rhs.step(); + return *this; + } + +protected: + double t_ = 0; + Step step_; +}; + +template +bool operator<(const TimedStep & s1, const TimedStep & s2) +{ + return s1 < s2; +} + +template +std::ostream & operator<<(std::ostream & os, const TimedStep & step) +{ + os << step.t() << " -> " << step.step().transpose(); + return os; +} + +/** + * @brief Helper class to manage a list of discrete steps and provide efficient + * sequential lookup of next/previous step. + * + * Steps have the following properties: + * - Each step is defined by + * - It's start time Step::t() + * - It's value Step::step() (may be any arbitrary type) + * - Steps are discrete events and do not have a specified duration + * - Steps are sorted from oldest to most recent + * + * Example + * \code{.cpp} + * PreviewSteps steps; + * steps.add( {1.6, {-0.2, 0.0}}); + * steps.addRelative({1.6, { 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({1.6, { 0.0, 0.0}}); + * steps.initialize(); + * mc_rtc::log::info("Desired steps:\nTime\tCoM X\tCoM Y\n{}", mc_rtc::io::to_string(steps.steps(), "\n")); + * + * double dt = 0.005; + * double previewTime = 1.6; + * const unsigned n_preview = static_cast(std::lround(previewTime / dt)); + * + * // Iterate over a preview window and perform computations + * // based on the previous and next steps + * // In a real-world use, this would most likely be used to compute a + * // smooth trajectory between the discrete steps. + * auto iteratePreview = [&](double startTime) + * { + * for(unsigned i = 0; i < n_preview * 2 + 1; i++) + * { + * double currTime = startTime + i*dt; + * // Update time within the preview window + * steps.update(currTime); + * if(!steps.isLastStep()) + * { + * const auto & nextStep = steps.next(); + * const auto & prevStep = steps.previous(); + * // Do something with next and previous steps, such as interpolate + * // between them to generate a smooth reference trajectory + * } + * else + * { + * // We are past the last step, handle this case + * const auto & lastStep = steps.previous(); + * } + * } + * } + * + * // Real-time control loop + * bool run() + * { + * // Start next preview window + * steps.nextWindow(t); + * iteratePreview(t); + * t+=dt; + * // Now you can modify future steps for the next preview window + * double transitionTime = 0.2; // 0.2 second to transition + * double delay = 0.5; // Transition in 0.5s in the future. The previous step + * will be extended until the transition starts. + * steps.changeFutureSteps( + * { + * t+previewTime+delay, // Extend the previous step until this time + * {t+previewTime+delay+transitionTime, {0., -0.15}} // Transition to the + * next steps + * }); + * } + * \endcode + */ +template +struct MC_PLANNING_DLLAPI PreviewSteps +{ + using value_type = TimedStep; + using PreviewStepsSet = std::set>; + using iterator = typename PreviewStepsSet::iterator; + using reverse_iterator = typename PreviewStepsSet::reverse_iterator; + + /** + * @brief Inserts a new step + * + * A step may be added at any time, including within the current preview + * window + * + * @param step Step to add + */ + void add(const TimedStep & step) + { + if(step.t() < (*steps_.begin()).t()) + { + mc_rtc::log::error("[PreviewSteps] can't create a step before the start of the preview window"); + return; + } + steps_.insert(step); + } + + void reset(const PreviewStepsSet & steps) + { + steps_ = steps; + } + + /** + * @brief Adds a step relative to the last step + * + * @throws std::runtime_error If steps() is empty + * + * @param step Relative step to add + */ + void addRelative(const TimedStep & step) + { + if(steps_.empty()) + { + mc_rtc::log::error_and_throw("[PreviewSteps] can't add a relative step to an empty set"); + return; + } + auto last = this->back(); + last += step; + add(last); + } + + /** + * @brief Replaces all footsteps following the first of the new provided steps + * + * @param nextSteps + */ + void replaceAfter(const PreviewStepsSet & nextSteps) + { + if(nextSteps.empty()) return; + deleteAfter(nextSteps.begin()->t()); + std::copy(nextSteps.begin(), nextSteps.end(), std::inserter(steps_, steps_.begin())); + } + + void deleteAfter(double t) + { + auto it = std::find_if(steps_.begin(), steps_.end(), [t](const value_type & step) { return step.t() >= t; }); + if(it != steps_.end()) + { + steps_.erase(it, steps_.end()); + } + } + + void changeFutureSteps(double startTransition, const std::vector & steps) + { + mc_rtc::log::info("Delete after {}", startTransition); + deleteAfter(startTransition); + // Extend previous step until start of transition + auto transitionStep = back(); + mc_rtc::log::info("Duplicating previous step {}", transitionStep); + transitionStep.t() = startTransition; + mc_rtc::log::info("Pre-Transition step {}", transitionStep); + steps_.insert(transitionStep); + mc_rtc::log::info("Inserting steps"); + for(const auto & step : steps) + { + mc_rtc::log::info("Step {}", step); + steps_.insert(step); + } + } + + /** + * @brief To be called after adding the initial footsteps + * + * Requires at least one footstep. + * + * @throws std::runtime_error if there isn't at least one footstep + */ + void initialize() + { + if(steps_.size() < 1) + { + mc_rtc::log::error_and_throw("[PreviewSteps] requires at least one step"); + } + else if(steps_.size() == 1) + { + previous_ = steps_.begin(); + next_ = previous_; + } + else + { + previous_ = steps_.begin(); + next_ = std::next(previous_); + } + } + + /** + * @name Real-time functions + * These functions are intended to be called sequentially while iterating over the preview window in the following + * way:
  • nextWindow(double t): Sets up a new preview window starting at time t, updates the starting step, and + * removes old steps prior to t
  • for each time in the preview window:
    • next(), previous() provide the + * previous/next step for the current time. Warning: if the last step is reached (`isLastStep() == true`), then next() + * = previous()
    • update(double t): move along the preview window
      • + *
    + *
    • + *
  • Repeat with the next preview window
    • + *
+ * @{ + */ + + /** + * @brief Updates the time along the preview window + * + * Finds the previous and next step for the current time in the preview window + * + * @param t Curent time within the preview window + */ + void update(double t) noexcept + { + if(!isLastStep() && t >= next_->t()) + { + previous_ = next_; + ++next_; + } + } + + /** + * @brief Whether we reached the last step + * + * @waning When on the last step, there is no next step available, and next() == previous() + */ + bool isLastStep() const noexcept + { + return next_ == steps_.end(); + } + + /** + * @brief Moves to the next preview window starting at time t + * + * Removes old footsteps outside of the window, and update previous/next + * steps accordingly + * + * @param t Time of the start of the preview window + */ + void nextWindow(double t) + { + // Check whether old footsteps need to be removed + if(steps_.size() > 2 && t > std::next(steps_.begin())->t()) + { + steps_.erase(steps_.begin()); + } + + previous_ = steps_.begin(); + next_ = std::next(previous_); + } + + /** + * @brief Returns the previous step w.r.t last call to update(double t) + * + * @return Next step + */ + const TimedStep & previous() const noexcept + { + return *previous_; + } + + /** + * @brief Returns the next step w.r.w last call to update(double t) + * + * @throws std::runtime_error If no next step is available. Use isLastStep() + * to check beforehand. + * + * @return Next step + */ + const TimedStep & next() const + { + if(!isLastStep()) + { + return *next_; + } + else + { + mc_rtc::log::error_and_throw("Can't access a step past the end"); + } + } + /** @} */ // end of group real-time + + /** + * @brief Get the closest footstep prior to time t + * + * @param t Lookup time + * + * @note This funciton is not intended for real-time performace and has a + * complexity in `O(steps)` + * + * @return Nearest past footstep with time <= t + */ + const TimedStep & previous(double t) const noexcept + { + if(t >= back().t()) return back(); + auto it = + std::find_if(steps_.rbegin(), steps_.rend(), [&t](const TimedStep & step) { return step.t() <= t; }); + return (it != steps_.rend()) ? *it : front(); + } + + /** + * @brief Get the closest future footstep after time t + * + * @param t Lookup time + * + * @note This function is not intended for real-time performace and has a + * complexity in `O(steps)`. + * + * @return Nearest future footstep with step.time > t, or last element if t is + * later than the last step + */ + const TimedStep & next(double t) const + { + auto it = std::find_if(steps_.begin(), steps_.end(), [&t](const TimedStep & step) { return step.t() > t; }); + return (it != steps_.end()) ? *it : back(); + } + + const PreviewStepsSet & steps() const noexcept + { + return steps_; + } + + const TimedStep & back() const noexcept + { + return *std::prev(steps_.end()); + } + + const TimedStep & front() const noexcept + { + return *steps_.begin(); + } + + bool isValid() const noexcept + { + return steps_.size() > 1; + } + +protected: + PreviewStepsSet steps_; // Ordered set of pairs of [time, Step] + + /** + * @name Real-time update + * @{ + */ + typename PreviewStepsSet::iterator previous_ = std::end(steps_); ///< Previous footstep + typename PreviewStepsSet::iterator next_ = std::end(steps_); ///< Next footstep + ///@} +}; + +} // namespace mc_planning diff --git a/include/mc_planning/PreviewWindow.h b/include/mc_planning/PreviewWindow.h new file mode 100644 index 0000000000..091272f8a7 --- /dev/null +++ b/include/mc_planning/PreviewWindow.h @@ -0,0 +1,463 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once +#include +#include +#include + +namespace mc_planning +{ + +struct CenteredPreviewWindow; +struct PreviewWindowView; + +namespace internal +{ + +/** + * @brief Provides a stronger type to disallow implicit conversions + * + * See https://foonathan.net/2016/10/strong-typedefs/ + */ +template +class strong_typedef +{ +public: + strong_typedef() : value_() {} + + explicit strong_typedef(const T & value) : value_(value) {} + + explicit strong_typedef(T && value) noexcept(std::is_nothrow_move_constructible::value) : value_(std::move(value)) + { + } + + // Allow implicit conversion to T + operator T &() noexcept + { + return value_; + } + + // Allow implicit conversion to T const + operator const T &() const noexcept + { + return value_; + } + + T operator()() const noexcept + { + return value_; + } + + friend void swap(strong_typedef & a, strong_typedef & b) noexcept + { + using std::swap; + swap(static_cast(a), static_cast(b)); + } + +protected: + T value_; +}; + +template +struct strong_typedef_operators +{ + friend T operator+(const T & lhs, const T & rhs) + { + return T{lhs() + rhs()}; + } + friend T operator-(const T & lhs, const T & rhs) + { + return T{lhs() - rhs()}; + } + friend T operator+(const T & lhs, const underlying_t & rhs) + { + return T{lhs() + rhs}; + } + friend T operator-(const T & lhs, const underlying_t & rhs) + { + return T{lhs() - rhs}; + } +}; + +} // namespace internal + +struct Time : internal::strong_typedef, internal::strong_typedef_operators +{ + using strong_typedef::strong_typedef; // make constructors available + Time operator+(const Time & t) + { + return Time{value_ + t}; + } +}; +struct Index : internal::strong_typedef, internal::strong_typedef_operators +{ + using strong_typedef::strong_typedef; // make constructors available +}; + +/** + * @brief Element returned by CenteredPreviewWindow::iterator providing the current + * index and allowing to conveniently get the corresponding time + */ +struct MC_PLANNING_DLLAPI PreviewElement +{ + /** + * @brief Create an element (meant to be used by iterators) + * + * @param window Preview window to which this element refers to + * @param index Global index of the element + */ + explicit PreviewElement(const PreviewWindowView & window, Index index) noexcept : window_(window), index_(index) {} + + explicit PreviewElement(const PreviewWindowView & window, unsigned index) noexcept : window_(window), index_(index) {} + + /** + * @brief Returns the global index starting at `window.startIndex()` + */ + Index index() const noexcept; + /** + * @brief Returns the time corresponding to index() + */ + Time time() const noexcept; + + /** + * @brief Returns the index local to the preview window + * + * @return Index between \f$ [0, 2*N] \f$ where `N` is the preview size + */ + Index localIndex() const noexcept; + + /** + * @brief Returns the time corresponding to localIndex() + */ + Time localTime() const noexcept; + +protected: + const PreviewWindowView & window_; ///< Window being iterated over + Index index_{0u}; ///< Global index +}; + +/** + * @brief Stores parameters related to a preview window + * + * This preview window is centered around the current element \f$ [-N, current, N] \f$ where `N` is the number of + * elements before and after the current value. The full preview window is of size `2*N+1`. The timestep is used to + * compute correspondances between discrete index of values stored in an (external) container and time. + * + * The @ref range_views functions can be used to obtain PreviewWindowView iterable ranges over past/future/all preview + * elements for a window starting at any time. + * + * \see PreviewWindowView iterable ranges over a floating preview window + * + * For practical use, see: + * \see mc_planning::generator + * \see mc_planning::linear_control_system::LinearTimeVariantInvertedPendulum + */ +struct MC_PLANNING_DLLAPI CenteredPreviewWindow +{ + /** + * @brief Constructs a window of duration `2*halfTime` with the current + * element at the center. + * + * @param halfTime Half-duration of the window + * @param dt Timesep + * @param startTime Time at which the window starts + */ + explicit CenteredPreviewWindow(Time halfTime, Time dt) noexcept + : halfSize_(static_cast(std::lround(halfTime / dt))), fullSize_(2 * halfSize_ + 1), dt_(dt) + { + } + + explicit CenteredPreviewWindow(double halfTime, double dt) noexcept : CenteredPreviewWindow(Time{halfTime}, Time{dt}) + { + } + + /** + * @brief Timestep + */ + Time dt() const noexcept + { + return dt_; + } + + /** + * @brief Number of future or past elements. + * + * The preview window is of size `2*halfSize_+1` + */ + Index halfSize() const noexcept + { + return halfSize_; + } + + /** + * @brief Duration corresponding to halfSize() + */ + Time halfDuration() const noexcept + { + return Time{halfSize_ * dt_}; + } + + /** + * @brief Full size of the preview window + */ + Index size() const noexcept + { + return fullSize_; + } + + /** + * @brief Full duration of the preview window + */ + Time duration() const noexcept + { + return Time{(size() - 1u) * dt_}; + } + + /** + * @brief Converts between time and index + * + * @param t Time + * + * @return Index corresponding to the provided time + */ + Index index(Time t) const noexcept + { + return Index{static_cast(std::lround(t / dt_))}; + } + + /** + * @brief Converts between index and time + * + * @param index Index + * + * @return Time corresponding to the provided index + */ + Time time(Index index) const noexcept + { + return Time{index * dt_}; + } + + /** Index of the central element */ + Index center() const noexcept + { + return halfSize_; + } + + /** Index of last element */ + Index last() const noexcept + { + return fullSize_ - 1u; + } + + /** + * @defgoup range_views + * Returns predefined views to the past/future of the preview window starting at the specified time/index + * - all: all elements in the preview window + * - past: all past elements, present excluded + * - pastInclusive: all past elements, present included + * - future: all future elements, present excluded + * - futureInclusive: all future elements, present included + * @{ + */ + /** All preview elements with a preview window starting at provided index */ + PreviewWindowView all(Index index) const noexcept; + /** All preview elements with a preview window starting at provided time */ + PreviewWindowView all(Time startTime) const noexcept; + /** All preview elements with a preview window starting at index 0 */ + PreviewWindowView all() const noexcept; + /** Past preview elements (excluding present) with a preview window starting at provided index */ + PreviewWindowView past(Index index) const noexcept; + /** Past preview elements (excluding present) with a preview window starting at provided time */ + PreviewWindowView past(Time startTime) const noexcept; + /** Past preview elements (excluding present) with a preview window starting at index 0 */ + PreviewWindowView past() const noexcept; + /** Past preview elements (including present) with a preview window starting at provided index */ + PreviewWindowView pastInclusive(Index index) const noexcept; + /** Past preview elements (including present) with a preview window starting at provided index */ + PreviewWindowView pastInclusive(Time startTime) const noexcept; + /** Past preview elements (including present) with a preview window starting at index 0 */ + PreviewWindowView pastInclusive() const noexcept; + /** Future preview elements (excluding present) with a preview window starting at provided index */ + PreviewWindowView future(Index index) const noexcept; + /** Future preview elements (excluding present) with a preview window starting at provided time */ + PreviewWindowView future(Time startTime) const noexcept; + /** Future preview elements (excluding present) with a preview window starting at index 0 */ + PreviewWindowView future() const noexcept; + /** Future preview elements (including present) with a preview window starting at provided index */ + PreviewWindowView futureInclusive(Index index) const noexcept; + /** Future preview elements (including present) with a preview window starting at provided time */ + PreviewWindowView futureInclusive(Time startTime) const noexcept; + /** Future preview elements (including present) with a preview window starting at index 0 */ + PreviewWindowView futureInclusive() const noexcept; + /// @} + +protected: + Index halfSize_{0u}; ///< Number of future or past element + Index fullSize_{0u}; ///< 2*halfSize_+1 + Time dt_{0.005}; ///< Timestep +}; + +/** + * Provides an iterable view on a preview window for convenient safe access to + * ranges of elements + * + * For example: + * \code{.cpp} + * // Create an iterable preview window range with a window of time `2*1.6`, + * // timestep of 0.005 + * CenteredPreviewWindow window(1.6, 0.005); + * mc_rtc::log::info("Start time: {:.3f}, index: {}\n" + * "End time: {:.3f}, index: {}\n" + * "Current time: {:.3f}, index: {}", + * window.startTime(), window.startIndex(), + * window.endTime(), window.endIndex(), + * window.currentTime(), window.currentIndex()); + * + * // Iterate over a view to the preview window starting at time t=0.5 and + * // containing all past elements in the preview window + * for(const auto & w : window.past(Time{0.5})) + * { + * mc_rtc::log::info("Preview element with index: {}, time: {:.3f}", w.index(), w.time()); + * // Example: Access some external container using the index + * // auto value = container[w.index()]; + * } + * + * // Iterate over a view to the preview window starting at time t=1.6 and + * // containing all elements (past, present and future) in the preview window + * for(const auto & w : window.all(1.6)) + * { + * mc_rtc::log::info( + * "Preview element with global index: {}, time: {:.3f}\n" + * " local index: {}, time: {:.3f}\n" + * , w.index(), w.time() + * , w.localIndex(), w.localTime() + * ); + * // Global time starts at 1.6s, while local time is between [0, window.duration()] + * } + * \endcode + **/ +struct PreviewWindowView +{ + using iterator = RangeElementIterator; + using const_iterator = RangeElementIterator; + + explicit PreviewWindowView(const CenteredPreviewWindow & window, + Index absoluteStart, + Index localStart, + Index localEnd) + : window_(window), absoluteStart_(absoluteStart), localStart_(localStart), localEnd_(localEnd) + { + } + + /** @brief Index corresponding to time t */ + Index index(Time t) const noexcept + { + return window_.index(t); + } + + /** @brief Time corresponding to index */ + Time time(Index index) const noexcept + { + return window_.time(index); + } + + /** + * @name Range iterators for computing index/time along the preview window + * @{ + */ + iterator begin() noexcept + { + return iterator{*this, startIndex()}; + } + iterator end() noexcept + { + return iterator{*this, endIndex() + 1u}; + } + + const_iterator cbegin() noexcept + { + return const_iterator{*this, startIndex()}; + } + const_iterator cend() noexcept + { + return const_iterator{*this, endIndex() + 1u}; + } + // @} + + /** + * @brief Time at the start of the preview window view + */ + inline Time startTime() const noexcept + { + return Time{(absoluteStart_ + localStart_) * window_.dt()}; + } + + /** + * @brief Index of the start of the preview window view + */ + Index startIndex() const noexcept + { + return absoluteStart_ + localStart_; + } + + /** + * @brief Index of the end of the preview window view + */ + Time endTime() const noexcept + { + return Time{endIndex() * window_.dt()}; + } + + /** + * @brief Index of the end of the preview window view + */ + Index endIndex() const noexcept + { + return absoluteStart_ + localEnd_; + } + + /** + * @brief Full size of the preview window view + */ + Index size() const noexcept + { + return Index{localEnd_ - localStart_ + 1u}; + } + + /** + * @brief Full duration of the preview window view + */ + Time duration() const noexcept + { + return Time{(size() - 1u) * window_.dt()}; + } + + /** + * @brief Index of the center of the preview window + * @note May be outside of this view + */ + Index nowIndex() const noexcept + { + return absoluteStart_ + window_.halfSize(); + } + + /** + * @brief Time of the center of the preview window + * @note May be outside of this view + */ + Time nowTime() const noexcept + { + return Time{nowIndex() * window_.dt()}; + } + +protected: + const CenteredPreviewWindow & window_; ///< Preview window parameters + Index absoluteStart_{ + 0}; ///< Index at which the preview window starts. The view is defined relative to the start of the preivew window + Index localStart_{0}; ///< Index within the preview window where it starts + Index localEnd_{0}; ///< Index past end of the view +}; + +} // namespace mc_planning diff --git a/include/mc_planning/RangeElementIterator.h b/include/mc_planning/RangeElementIterator.h new file mode 100644 index 0000000000..ce98e7b830 --- /dev/null +++ b/include/mc_planning/RangeElementIterator.h @@ -0,0 +1,73 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once +#include +#include +#include + +namespace mc_planning +{ + +/** + * @brief Virtual iterator for iterating over a range of value + * and accessing elements of the parent class + * + * \see CenteredPreviewWindow for a practical use + */ +template +struct MC_PLANNING_DLLAPI RangeElementIterator +{ +private: + struct IncrementHolder + { + explicit IncrementHolder(unsigned value) noexcept : index_(value) {} + unsigned operator*() noexcept + { + return index_; + } + + private: + unsigned index_; + }; + +public: + using value_type = RetT; + /* deduce const qualifier from bool Const parameter */ + using reference = typename std::conditional::type; + using pointer = typename std::conditional::type; + using iterator_category = std::input_iterator_tag; + using difference_type = void; + + explicit RangeElementIterator(const ParentT & window, unsigned value) noexcept : window_(window), index_(value) {} + value_type operator*() const + { + return value_type(window_, index_); + } + bool operator==(const RangeElementIterator & other) const noexcept + { + return index_ == other.index_; + } + bool operator!=(const RangeElementIterator & other) const noexcept + { + return !(*this == other); + } + IncrementHolder operator++(int) noexcept + { + IncrementHolder ret(index_); + ++*this; + return ret; + } + RangeElementIterator & operator++() noexcept + { + ++index_; + return *this; + } + +private: + const ParentT & window_; + unsigned index_; +}; + +} // namespace mc_planning diff --git a/include/mc_planning/State.h b/include/mc_planning/State.h new file mode 100644 index 0000000000..3729bb80ff --- /dev/null +++ b/include/mc_planning/State.h @@ -0,0 +1,87 @@ +/* + * Copyright (c) 2009, @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#pragma once +#include +#include + +namespace mc_planning +{ + +class MC_PLANNING_DLLAPI StateP +{ +public: + Eigen::Vector3d P; ///< position + + /** + @brief constructor + */ + StateP(); + + StateP(const Eigen::Vector3d & p); + + StateP(const StateP & i_state); + + StateP & operator=(const StateP & i_state); + + friend std::ostream & operator<<(std::ostream & os, const StateP & s); + + void Initialize(); + + void Copy(const StateP & i_state); +}; + +class MC_PLANNING_DLLAPI StatePV : virtual public StateP +{ +public: + Eigen::Vector3d V; ///< velocity + + /** + @brief constructor + */ + StatePV(); + + StatePV(const Eigen::Vector3d & p, const Eigen::Vector3d & v); + + StatePV(const StatePV & i_state); + + StatePV & operator=(const StatePV & i_state); + + friend std::ostream & operator<<(std::ostream & os, const StatePV & s); + + void Initialize(); + + void Copy(const StatePV & i_state); +}; + +class MC_PLANNING_DLLAPI StatePVA : virtual public StatePV +{ +public: + Eigen::Vector3d Vdot; ///< acceleration + + /** + @brief constructor + */ + StatePVA(); + + StatePVA(const Eigen::Vector3d & p, const Eigen::Vector3d & v, const Eigen::Vector3d & a); + + StatePVA(const StatePVA & i_state); + + StatePVA & operator=(const StatePVA & i_state); + + friend std::ostream & operator<<(std::ostream & os, const StatePVA & s); + + void Initialize(); + + void Copy(const StatePVA & i_state); +}; +} // namespace mc_planning diff --git a/include/mc_planning/generator.h b/include/mc_planning/generator.h new file mode 100644 index 0000000000..c4d84256d5 --- /dev/null +++ b/include/mc_planning/generator.h @@ -0,0 +1,347 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mc_planning +{ + +/** + * @brief Utility class to generate long and short term trajectories for the CoM. + * + * Generate trajectories as introduced in: + * **Online 3D CoM Trajectory Generation for Multi-Contact Locomotion Synchronizing Contact**, *Mitsuharu Morisawa et + * al., IROS 2018* + * + * @see mc_planning::linear_control_system::LinearTimeVariantInvertedPendulum long-term trajectory generation + * @see mc_planning::linear_control_system::LIPMControlByPoleAssignWithExternalForce short-term trajectory generation to + * continuously track the long-term trajectory + * + * + * Example + * Let's see with a simple example how this generator can be used in practice. + * We will generate a simple CoM trajectory in the sagittal and lateral + * direction by providing discrete desired ZMP positions along with their + * timing. + * See the corresponding mc_rtc sample testCoMGeneration and benchmark. + * + * \include mc_sample_CoMGeneration.cpp + * + * The generated trajectory should look like the figure below. + * @image html images/generator_com_generation.svg width=100% + */ +struct MC_PLANNING_DLLAPI generator +{ + using PreviewStepsSet = PreviewSteps::PreviewStepsSet; + + /** + * @brief Construct a simple trajectory generator + * + * @param preview Preview window parameters (size, timestep) + * @mass Robot mass + * @waist_height Initial height of the waist (constant for now) + */ + generator(const CenteredPreviewWindow & preview, double mass = 60, double waist_height = 0.8); + + /** + * @brief Generate the long and short term trajectories + * + * @param n_time current time + */ + void generate(unsigned n_time); + + /** + * @brief Add to Logger + */ + void addToLogger(mc_rtc::Logger & logger); + /** + * @brief Remove from Logger + */ + void removeFromLogger(mc_rtc::Logger & logger); + + /** + * @name Step management + * Provide sagital and lateral ZMP reference as steps define by a set of (time, Vector2d). + * Steps can be changed at any time using these function. Note that in general steps + * should be changed in the future (after the middle of the preview window). + * If the next steps are suddently changed at the current time, this will + * cause a ZMP jerk (the CoM trajectory remains continuous). Prefer to change steps later in the future whenever + * possible to achieve smoother trajectories. + * @{ + */ + /** + * @brief Desired steps + * @see setSteps(const std::vector & steps) + */ + const PreviewSteps & steps() const + { + return m_steps; + } + + /** + * @brief Sets desired reference steps as (time, CoM X, CoM Y) + * + * @param steps Desired steps from which the trajectory will be generated + */ + void steps(const PreviewSteps & steps) + { + m_steps = steps; + } + + /** + * @brief Change future steps and ensures continuous trajectory + * + * @note The previous footstep will be extended until `transitionTime`, and + * all future steps currently in the trajectory will be removed + * + * @param transitionTime Time at which the transition should occur + * @param futureSteps Future steps + */ + void changeFutureSteps(double transitionTime, const std::vector> & futureSteps) + { + m_steps.changeFutureSteps(transitionTime, futureSteps); + } + ///@} + + /** + * @name Poles of the short-term trajectory + * Typical values range from \f$ [1,1,150] \f$ to \f$ [1,1,300] \f$. + * The bigger the poles are, the the faster the short-term trajectory will converge to the long-term trajectory. + * Too high a value could lead to jerk when the long-term trajectory is not continuous + * @{ + */ + + /** + * @brief Sets the poles for the short-term trajectory + * + * + * @param poles Poles for the short-term trajectory + */ + void polesX(const Eigen::Vector3d & poles) + { + m_poles[0] = poles; + } + + /** @brief Gets the short-term trajectory poles */ + const Eigen::Vector3d & polesX() const + { + return m_poles[0]; + } + + /** + * @brief Sets the poles for the short-term trajectory + * + * @see polesX(const Eigen::Vector3d & poles) + */ + void polesY(const Eigen::Vector3d & poles) + { + m_poles[0] = poles; + } + + /** @brief Gets the short-term trajectory poles */ + const Eigen::Vector3d & polesY() const + { + return m_poles[0]; + } + ///@} + + /** + * @name Ideal CoG/ZMP generated by the long-term trajectory. + * @{ + */ + /** Ideal CoG position generated by the long-term trajectory. */ + const Eigen::Vector3d & IdealCOGPosition() const + { + return m_COG_ideal.P; + } + /** Ideal CoG velocity generated by the long-term trajectory. */ + const Eigen::Vector3d & IdealCOGVelocity() const + { + return m_COG_ideal.V; + } + /** Ideal CoG acceleration generated by the long-term trajectory. */ + const Eigen::Vector3d & IdealCOGAcceleration() const + { + return m_COG_ideal.Vdot; + } + /** Ideal ZMP position generated by the long-term trajectory. */ + const Eigen::Vector3d & IdealZMPPosition() const + { + return m_Pcalpha_ideal; + } + /** Ideal ZMP velocity generated by the long-term trajectory. */ + const Eigen::Vector3d & IdealZMPVelocity() const + { + return m_Vcalpha_ideal; + } + ///@} + + /** + * @name Compenstated CoG/ZMP generated by the short-term trajectory + * + * The short-term trajectory is tracking the long-term trajectory. Usually, + * both long-term and short-term trajectories are close to each other, but + * when discontinuities of the long-term trajectory happen (mostly due to + * change of steps in the current preview window), then this compensation + * makes sure that the long-term trajectory is tracked smoothly without + * discontinuities. + * @{ + */ + /** @brief Compensation of the COG position due to the short-term trajectory */ + const Eigen::Vector3d & CompensatedCOGPosition() const + { + return m_COG_cmp.P; + } + /** @brief Compensation of the COG velocity due to the short-term trajectory */ + const Eigen::Vector3d & CompensatedCOGVelocity() const + { + return m_COG_cmp.V; + } + /** @brief Compensation of the COG acceleration due to the short-term trajectory */ + const Eigen::Vector3d & CompensatedCOGAcceleration() const + { + return m_COG_cmp.Vdot; + } + /** @brief Compensation of the ZMP position due to the short-term trajectory */ + const Eigen::Vector3d & CompensatedZMPPosition() const + { + return m_Pcalpha_cmp; + } + /** @brief Compensation of the ZMP velocity due to the short-term trajectory */ + const Eigen::Vector3d & CompensatedZMPVelocity() const + { + return m_Vcalpha_cmp; + } + ///@} + + /** + * @name Output CoG/ZMP such that the short-term trajectory tracks the long-term trajectory. + * + * This output is a combination of the ideal (long-term) and compensated + * (short-term) output that ensures continuous COG trajectories that will + * track the long-term trajectory. + * @{ + */ + /** + * @brief Compensation of the COG position due to the short-term trajectory + */ + /** @brief Output COG position at current time computed by the short-term trajectory to best track the long-term + * trajectory */ + const Eigen::Vector3d & OutputCOGPosition() const + { + return m_COG_out.P; + } + /** @brief Output COG velocity at current time computed by the short-term trajectory to best track the long-term + * trajectory */ + const Eigen::Vector3d & OutputCOGVelocity() const + { + return m_COG_out.V; + } + /** @brief Output COG acceleration at current time computed by the short-term trajectory to best track the long-term + * trajectory */ + const Eigen::Vector3d & OutputCOGAcceleration() const + { + return m_COG_out.Vdot; + } + + /** @brief Output ZMP acceleration at current time computed by the short-term trajectory to best track the long-term + * trajectory */ + const Eigen::Vector3d & OutputZMPPosition() const + { + return m_Pcalpha_out; + } + ///@} + +private: + /** + * @brief Setup CoM with constant height along the trajectory + * and iterpolate its position/velocity/acceleration + * + * Sets m_COG_ideal for the current time + * + * @param n_current Index of the start of the preview window + */ + void setupCOGHeight(unsigned n_current); + + /** + * @brief Setup reference trajectories based on the desired steps + * + * Sets m_ipm_long.px_ref, m_ipm_long.py_ref, m_ipm_long.w2 and m_Pcalpha_ideal + * + * @param n_current Index of the start of the preview window + */ + void setupTimeTrajectories(unsigned n_current); + + /** + * @brief Solve long and short term trajectories from the reference time + * trajectories + * + * Computes m_COG_out, m_Pcalpha_out, m_Pcalpha_motion_out, m_COG_cmp + */ + void generateTrajectories(); + +private: + CenteredPreviewWindow preview_; ///< Iteratable preview window and parameters + std::shared_ptr> m_ComInterp = nullptr; + std::array m_ipm_long; + std::array m_ipm_short; + /**< Poles for the short-term trajectory. + * Typical range [1,1,150] ... [1,1,300] + */ + std::array m_poles = {Eigen::Vector3d{1., 1., 150.}, Eigen::Vector3d{1., 1., 150.}}; + + StatePVA m_COG_ideal_pre; + /** + * Ideal COG trajectory + * - X, Y components are computed by the long term trajectories m_ipm_long + * - Z component is interpolated by m_ComInterp + */ + StatePVA m_COG_ideal; + StatePVA m_COG_cmp; + StatePVA m_COG_out; + + /** + * @name ZMP-related + * @{ + */ + Eigen::Vector3d m_Pcalpha_ideal_pre = Eigen::Vector3d::Zero(); + Eigen::Vector3d m_Pcalpha_ideal = Eigen::Vector3d::Zero(); + Eigen::Vector3d m_Pcalpha_cmp = Eigen::Vector3d::Zero(); + Eigen::Vector3d m_Vcalpha_ideal_pre = Eigen::Vector3d::Zero(); + Eigen::Vector3d m_Vcalpha_ideal = Eigen::Vector3d::Zero(); + Eigen::Vector3d m_Vcalpha_cmp = Eigen::Vector3d::Zero(); + Eigen::Vector3d m_Pcalpha_out = Eigen::Vector3d::Zero(); + Eigen::Vector3d m_Pcalpha_motion_out = Eigen::Vector3d::Zero(); + /// @} + + double m_omega_valpha = 0.0; + double m_mass = 60.; ///< Robot mass + double m_waist_height; ///< Height of the weight (constant for now) + + std::array m_virtual_height; + Eigen::VectorXd m_cog_height; ///< CoM height + Eigen::VectorXd m_cog_dot_height; ///< CoM velocity + Eigen::VectorXd m_cog_ddot_height; ///< CoM acceleration + + // unsigned m_n_steps = 0; ///< Current step + // std::vector m_steps; ///< (time, com_x, com_y) + PreviewSteps m_steps; +}; + +} // namespace mc_planning + +/** + * @example mc_sample_CoMGeneration.cpp + * This sample generates a dynamic trajectory for the CoM and ZMP based on + * desired ZMP step inputs. + * + * The generated trajectory should look like the figure below. + * @image html images/generator_com_generation.svg width=100% + * + * \see mc_planning::generator + */ diff --git a/include/mc_rtc/eigen_traits.h b/include/mc_rtc/eigen_traits.h new file mode 100644 index 0000000000..88917beaf6 --- /dev/null +++ b/include/mc_rtc/eigen_traits.h @@ -0,0 +1,25 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once +#include +#include + +namespace mc_rtc +{ +namespace internal +{ +namespace is_eigen_matrix_detail +{ +// These functions are never defined. +template +std::true_type test(const Eigen::MatrixBase *); +std::false_type test(...); +} // namespace is_eigen_matrix_detail +template +struct is_eigen_matrix : public decltype(is_eigen_matrix_detail::test(std::declval())) +{ +}; +} // namespace internal +} // namespace mc_rtc diff --git a/include/mc_rtc/gui/types.h b/include/mc_rtc/gui/types.h index 2e52599f6a..d66727e66b 100644 --- a/include/mc_rtc/gui/types.h +++ b/include/mc_rtc/gui/types.h @@ -120,6 +120,7 @@ struct MC_RTC_GUI_DLLAPI Color static const Color Yellow; static const Color Gray; static const Color LightGray; + static const Color Purple; static const std::map ColorMap; }; } // namespace gui diff --git a/include/mc_rtc/io_utils.h b/include/mc_rtc/io_utils.h index 01a0541107..2a91dd609c 100644 --- a/include/mc_rtc/io_utils.h +++ b/include/mc_rtc/io_utils.h @@ -3,6 +3,7 @@ */ #pragma once +#include #include #include #include @@ -10,9 +11,9 @@ namespace mc_rtc { + namespace io { - /** Converts a container to a string * * @tparam Container A container whose underlying type is convertible to @@ -26,20 +27,23 @@ namespace io * @anchor to_string */ template::value, int>::type = 0> + typename std::enable_if::value + && !mc_rtc::internal::is_eigen_matrix::value, + int>::type = 0> std::string to_string(const Container & c, const std::string & delimiter = ", ") { if(c.cbegin() == c.cend()) { return ""; } - std::string out = *c.cbegin(); + std::stringstream out; + out << *c.cbegin(); for(auto it = std::next(c.cbegin()); it != c.cend(); ++it) { - out += delimiter; - out += *it; + out << delimiter; + out << *it; } - return out; + return out.str(); } /** @@ -69,40 +73,112 @@ std::string to_string(const Container & c, return out.str(); } -/** Converts a container to a string +/** + * @brief Converts a container to a string * * Example: - *\code{.cpp} + * \code{.cpp} * const auto availabeRobots = mc_rtc::io::to_string(robots(), [](const mc_rbdyn::Robot & r) -> const std::string & { - *return r.name(); }); - \endcode + * return r.name(); }); + * \endcode * * @tparam Container An iterable container whose unerlying type is convertible * to std::string. The container must define Container::value_type. * * @param c Container to convert - * @param get_value Lambda or functor that converts a single element from the container (of type Container::value_type) - to std::string + * @param get_value Lambda or functor that transforms each container element (of type Container::value_type) to an + * element convertible to std::string. This may be used to return elements of types non directly convertible to + * std::string, or to apply transformations to the data before returning it. * * @return A string of all the container elements. + * + * @anchor to_string_transform */ template::value, int>::type = 0> + typename std::enable_if::value + && !mc_rtc::internal::is_eigen_matrix::value, + int>::type = 0> std::string to_string(const Container & c, Callback && get_value, const std::string & delimiter = ", ") { if(c.cbegin() == c.cend()) { return ""; } - std::string out = get_value(*c.cbegin()); + std::stringstream out; + out << get_value(*c.cbegin()); for(auto it = std::next(c.cbegin()); it != c.cend(); ++it) { - out += delimiter; - out += get_value(*it); + out << delimiter; + out << get_value(*it); } - return out; + return out.str(); } +/** + * @name Specializations for Eigen types + */ +///@{ + +/** + * @brief Specialization of @ref to_string_transform for Eigen matrix/vector types + * @param fmt Eigen formatting options + * + * @see @ref to_string_transform + */ +template::value + && mc_rtc::internal::is_eigen_matrix::value, + int>::type = 0> +std::string to_string(const Container & c, + Callback && get_value, + const std::string & delimiter = ", ", + const Eigen::IOFormat & fmt = Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, "\t")) +{ + if(c.cbegin() == c.cend()) + { + return ""; + } + std::stringstream out; + out << get_value(*c.cbegin()).format(fmt); + for(auto it = std::next(c.cbegin()); it != c.cend(); ++it) + { + out << delimiter; + out << get_value(*it).format(fmt); + } + return out.str(); +} + +/** + * @brief Variant for Eigen matrix/vector types + * @param fmt Eigen formatting options + * + * @see @ref to_string + */ +template< + typename Container, + typename std::enable_if::value, int>::type = 0> +std::string to_string(const Container & c, + const std::string & delimiter = ", ", + const Eigen::IOFormat & fmt = + Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, "\t", " ", "", "", "", "")) +{ + if(c.cbegin() == c.cend()) + { + return ""; + } + std::stringstream out; + out << (*c.cbegin()).format(fmt); + for(auto it = std::next(c.cbegin()); it != c.cend(); ++it) + { + out << delimiter; + out << (*it).format(fmt); + } + return out.str(); +} + +///@} + } // namespace io } // namespace mc_rtc diff --git a/include/mc_rtc/log/utils.h b/include/mc_rtc/log/utils.h index 7a7f0107a7..4e5da1d297 100644 --- a/include/mc_rtc/log/utils.h +++ b/include/mc_rtc/log/utils.h @@ -57,36 +57,68 @@ struct GetLogType static constexpr mc_rtc::log::LogType type = mc_rtc::log::LogType::None; }; -#define IMPL_MAPPING(CPPT, ENUMV) \ +#define IMPL_TYPE_TO_ENUM_MAPPING(CPPT, ENUMV) \ template<> \ struct GetLogType \ { \ static constexpr mc_rtc::log::LogType type = mc_rtc::log::LogType::ENUMV; \ } -IMPL_MAPPING(bool, Bool); -IMPL_MAPPING(int8_t, Int8_t); -IMPL_MAPPING(int16_t, Int16_t); -IMPL_MAPPING(int32_t, Int32_t); -IMPL_MAPPING(int64_t, Int64_t); -IMPL_MAPPING(uint8_t, Uint8_t); -IMPL_MAPPING(uint16_t, Uint16_t); -IMPL_MAPPING(uint32_t, Uint32_t); -IMPL_MAPPING(uint64_t, Uint64_t); -IMPL_MAPPING(float, Float); -IMPL_MAPPING(double, Double); -IMPL_MAPPING(std::string, String); -IMPL_MAPPING(Eigen::Vector2d, Vector2d); -IMPL_MAPPING(Eigen::Vector3d, Vector3d); -IMPL_MAPPING(Eigen::Vector6d, Vector6d); -IMPL_MAPPING(Eigen::VectorXd, VectorXd); -IMPL_MAPPING(Eigen::Quaterniond, Quaterniond); -IMPL_MAPPING(sva::PTransformd, PTransformd); -IMPL_MAPPING(sva::ForceVecd, ForceVecd); -IMPL_MAPPING(sva::MotionVecd, MotionVecd); -IMPL_MAPPING(sva::ImpedanceVecd, MotionVecd); - -#undef IMPL_MAPPING +IMPL_TYPE_TO_ENUM_MAPPING(bool, Bool); +IMPL_TYPE_TO_ENUM_MAPPING(int8_t, Int8_t); +IMPL_TYPE_TO_ENUM_MAPPING(int16_t, Int16_t); +IMPL_TYPE_TO_ENUM_MAPPING(int32_t, Int32_t); +IMPL_TYPE_TO_ENUM_MAPPING(int64_t, Int64_t); +IMPL_TYPE_TO_ENUM_MAPPING(uint8_t, Uint8_t); +IMPL_TYPE_TO_ENUM_MAPPING(uint16_t, Uint16_t); +IMPL_TYPE_TO_ENUM_MAPPING(uint32_t, Uint32_t); +IMPL_TYPE_TO_ENUM_MAPPING(uint64_t, Uint64_t); +IMPL_TYPE_TO_ENUM_MAPPING(float, Float); +IMPL_TYPE_TO_ENUM_MAPPING(double, Double); +IMPL_TYPE_TO_ENUM_MAPPING(std::string, String); +IMPL_TYPE_TO_ENUM_MAPPING(Eigen::Vector2d, Vector2d); +IMPL_TYPE_TO_ENUM_MAPPING(Eigen::Vector3d, Vector3d); +IMPL_TYPE_TO_ENUM_MAPPING(Eigen::Vector6d, Vector6d); +IMPL_TYPE_TO_ENUM_MAPPING(Eigen::VectorXd, VectorXd); +IMPL_TYPE_TO_ENUM_MAPPING(Eigen::Quaterniond, Quaterniond); +IMPL_TYPE_TO_ENUM_MAPPING(sva::PTransformd, PTransformd); +IMPL_TYPE_TO_ENUM_MAPPING(sva::ForceVecd, ForceVecd); +IMPL_TYPE_TO_ENUM_MAPPING(sva::MotionVecd, MotionVecd); +IMPL_TYPE_TO_ENUM_MAPPING(sva::ImpedanceVecd, MotionVecd); +IMPL_TYPE_TO_ENUM_MAPPING(std::vector, VectorDouble); +#undef IMPL_TYPE_TO_ENUM_MAPPING + +template +struct GetType; + +#define IMPL_ENUM_TO_TYPE_MAPPING(CPPT, ENUMV) \ + template<> \ + struct GetType \ + { \ + using type = CPPT; \ + }; +IMPL_ENUM_TO_TYPE_MAPPING(bool, Bool); +IMPL_ENUM_TO_TYPE_MAPPING(int8_t, Int8_t); +IMPL_ENUM_TO_TYPE_MAPPING(int16_t, Int16_t); +IMPL_ENUM_TO_TYPE_MAPPING(int32_t, Int32_t); +IMPL_ENUM_TO_TYPE_MAPPING(int64_t, Int64_t); +IMPL_ENUM_TO_TYPE_MAPPING(uint8_t, Uint8_t); +IMPL_ENUM_TO_TYPE_MAPPING(uint16_t, Uint16_t); +IMPL_ENUM_TO_TYPE_MAPPING(uint32_t, Uint32_t); +IMPL_ENUM_TO_TYPE_MAPPING(uint64_t, Uint64_t); +IMPL_ENUM_TO_TYPE_MAPPING(float, Float); +IMPL_ENUM_TO_TYPE_MAPPING(double, Double); +IMPL_ENUM_TO_TYPE_MAPPING(std::string, String); +IMPL_ENUM_TO_TYPE_MAPPING(Eigen::Vector2d, Vector2d); +IMPL_ENUM_TO_TYPE_MAPPING(Eigen::Vector3d, Vector3d); +IMPL_ENUM_TO_TYPE_MAPPING(Eigen::Vector6d, Vector6d); +IMPL_ENUM_TO_TYPE_MAPPING(Eigen::VectorXd, VectorXd); +IMPL_ENUM_TO_TYPE_MAPPING(Eigen::Quaterniond, Quaterniond); +IMPL_ENUM_TO_TYPE_MAPPING(sva::PTransformd, PTransformd); +IMPL_ENUM_TO_TYPE_MAPPING(sva::ForceVecd, ForceVecd); +IMPL_ENUM_TO_TYPE_MAPPING(sva::MotionVecd, MotionVecd); +IMPL_ENUM_TO_TYPE_MAPPING(std::vector, VectorDouble); +#undef IMPL_ENUM_TO_TYPE_MAPPING template struct GetLogType> diff --git a/include/mc_rtc/time_utils.h b/include/mc_rtc/time_utils.h new file mode 100644 index 0000000000..7a2f2ff559 --- /dev/null +++ b/include/mc_rtc/time_utils.h @@ -0,0 +1,75 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once +#include +#include + +namespace mc_rtc +{ +/** + * @brief Class to measure the execution time of a callable + */ +template, + class ClockT = typename std::conditional::type> +struct measure +{ + /** + * @brief Returns the exectution time of the provided function (in chrono + * type system) + * + * @param func Function to evaluate + * @param args Arguments to the function + * + * @return Time elapsed + */ + template + static TimeT execution(F && func, Args &&... args) + { + auto start = ClockT::now(); + func(std::forward(args)...); + return std::chrono::duration_cast(ClockT::now() - start); + } +}; +using measure_s = measure>; +using measure_ms = measure>; +using measure_us = measure>; +using measure_ns = measure>; + +/** + * @brief Class to measure execution time + */ +template, + class ClockT = typename std::conditional::type> +struct Stopwatch +{ + using TimeP = typename TimeT::time_point; + TimeP startTime_; + + Stopwatch() + { + startTime_ = ClockT::now(); + } + + inline void start() + { + startTime_ = ClockT::now(); + } + + inline TimeT elapsed() + { + auto elapsed = std::chrono::duration_cast(ClockT::now() - startTime_); + return elapsed; + } +}; +using Stopwatch_s = Stopwatch>; +using Stopwatch_ms = Stopwatch>; +using Stopwatch_us = Stopwatch>; +using Stopwatch_ns = Stopwatch>; + +} // namespace mc_rtc diff --git a/include/mc_tasks/lipm_stabilizer/StabilizerTask.h b/include/mc_tasks/lipm_stabilizer/StabilizerTask.h index 8f83e5114e..9c1335a02a 100644 --- a/include/mc_tasks/lipm_stabilizer/StabilizerTask.h +++ b/include/mc_tasks/lipm_stabilizer/StabilizerTask.h @@ -805,7 +805,14 @@ struct MC_TASKS_DLLAPI StabilizerTask : public MetaTask const mc_rbdyn::Robots & realRobots_; unsigned int robotIndex_; - /** Stabilizer targets */ + /** + * @name Computed stabilized targets + * + * - comTarget_, comdTarget_ and comddTarget_ are modified by ZMPCC + * - zmpTarget_ is the result of wrench distribution + * - dcmTarget_ is computed from the above values + * @{ + **/ Eigen::Vector3d comTargetRaw_ = Eigen::Vector3d::Zero(); Eigen::Vector3d comTarget_ = Eigen::Vector3d::Zero(); Eigen::Vector3d comdTarget_ = Eigen::Vector3d::Zero(); @@ -813,8 +820,23 @@ struct MC_TASKS_DLLAPI StabilizerTask : public MetaTask Eigen::Vector3d zmpTarget_ = Eigen::Vector3d::Zero(); Eigen::Vector3d zmpdTarget_ = Eigen::Vector3d::Zero(); Eigen::Vector3d dcmTarget_ = Eigen::Vector3d::Zero(); - double omega_; + /// @} + /** + * @name Stabilizer desired reference (only used for logging) + * + * These values are set by calling target() or staticTarget(). + * @note This reference must follow a dynamically consistent trajectory + * @{ + */ + Eigen::Vector3d comRef_ = Eigen::Vector3d::Zero(); ///< Desired CoM position + Eigen::Vector3d comdRef_ = Eigen::Vector3d::Zero(); ///< Desired CoM velocity + Eigen::Vector3d comddRef_ = Eigen::Vector3d::Zero(); ///< Desired CoM acceleration + Eigen::Vector3d zmpRef_ = Eigen::Vector3d::Zero(); ///< Desired ZMP target + Eigen::Vector3d dcmRef_ = Eigen::Vector3d::Zero(); ///< Desired DCM + ///@} + + double omega_ = 0.; ///< Pendulum frequency double t_ = 0.; /**< Time elapsed since the task is running */ protected: @@ -841,6 +863,9 @@ struct MC_TASKS_DLLAPI StabilizerTask : public MetaTask Eigen::Vector3d measuredDCMUnbiased_ = Eigen::Vector3d::Zero(); /// DCM unbiased (only used for logging) sva::ForceVecd measuredNetWrench_ = sva::ForceVecd::Zero(); + Eigen::Vector3d desiredZMP_ = Eigen::Vector3d::Zero(); ///< Desired ZMP based on DCM error + Eigen::Vector3d distributedZMP_ = Eigen::Vector3d::Zero(); ///< Distributed desired ZMP (target) + bool zmpccOnlyDS_ = true; /**< Only apply ZMPCC in double support */ ZMPCC zmpcc_; /**< Compute CoM offset due to ZMPCC compensation */ diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt new file mode 100644 index 0000000000..5d4f67b0a8 --- /dev/null +++ b/samples/CMakeLists.txt @@ -0,0 +1,20 @@ +macro(add_example NAME) + add_executable(${NAME} ${NAME}.cpp) + target_link_libraries(${NAME} PUBLIC ${ARGN}) +endmacro() + +macro(add_example_doc NAME) + file(RELATIVE_PATH MC_RTC_SAMPLE_PATH ${PROJECT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/${NAME}.in.cpp) + set(MC_RTC_SAMPLE_COMMAND ${NAME}) + configure_file(${NAME}.in.cpp + ${CMAKE_CURRENT_BINARY_DIR}/${NAME}.cpp) + add_example(${NAME} ${ARGN}) + + install(TARGETS ${NAME} DESTINATION bin) + + install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${NAME}.cpp + DESTINATION ${CMAKE_INSTALL_DOCDIR}/doxygen-html/examples + ) +endmacro() + +add_subdirectory(CoMGenerator) diff --git a/samples/CoMGenerator/CMakeLists.txt b/samples/CoMGenerator/CMakeLists.txt new file mode 100644 index 0000000000..0141977a5c --- /dev/null +++ b/samples/CoMGenerator/CMakeLists.txt @@ -0,0 +1 @@ +add_example_doc(mc_sample_CoMGeneration mc_planning) diff --git a/samples/CoMGenerator/mc_sample_CoMGeneration.in.cpp b/samples/CoMGenerator/mc_sample_CoMGeneration.in.cpp new file mode 100644 index 0000000000..e81fc2af57 --- /dev/null +++ b/samples/CoMGenerator/mc_sample_CoMGeneration.in.cpp @@ -0,0 +1,66 @@ +// This sample was generated from @MC_RTC_SAMPLE_PATH@ +// You can try it by running: +// $ @MC_RTC_SAMPLE_COMMAND@ + +#include +#include +#include +#include + +using namespace mc_planning; + +int main(int, char **) +{ + double dt = 0.005; + CenteredPreviewWindow preview(1.6, dt); + + // Trajectory of size 2*n_preview+1 + generator com_traj(preview); + + auto prev_time = preview.halfDuration(); + PreviewSteps steps; + steps.add({prev_time, {-0.2, 0.0}}); + steps.addRelative({prev_time, {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({prev_time, {0.0, 0.0}}); + steps.initialize(); + + com_traj.steps(steps); + mc_rtc::log::info("Desired steps:\nTime\tZMP X\tZMP Y\n{}", mc_rtc::io::to_string(steps.steps(), "\n")); + + // Setup logging + mc_rtc::Logger logger(mc_rtc::Logger::Policy::NON_THREADED, "/tmp", "mc_rtc-test"); + logger.start("CoMGenerator", dt); + com_traj.addToLogger(logger); + + unsigned n_loop = preview.index(Time(com_traj.steps().back().t())) - preview.halfSize(); + + /* + * Generate the CoM trajectory based on the parameters in com_traj + * In practice this would be done at every iteration of the controller, we + * use a loop here to emulate this. + */ + auto comGeneration = [&]() { + for(unsigned loop = 0; loop <= n_loop; loop++) + { + com_traj.generate(loop); + logger.log(); + } + }; + + // Run and measure the comGeneration loop + auto duration = mc_rtc::measure_ms::execution(comGeneration).count(); + mc_rtc::log::info("Desired steps:\nTime\tCoM X\tCoM Y\n{}", mc_rtc::io::to_string(com_traj.steps().steps(), "\n")); + mc_rtc::log::info("calc time = {} (ms)", duration); + mc_rtc::log::info("ave. calc time = {} (ms)", duration / static_cast(n_loop)); + mc_rtc::log::success("end of com trajectory generation"); + + com_traj.removeFromLogger(logger); + return 0; +} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 88590c6686..12a92320fd 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -408,11 +408,32 @@ install_mc_rtc_lib(mc_tasks) set(mc_planning_SRC mc_planning/Pendulum.cpp +mc_planning/LinearTimeVariantInvertedPendulum.cpp +mc_planning/LinearControl3.cpp +mc_planning/LIPMControlByPoleAssign.cpp +mc_planning/LIPMControlByPoleAssignWithExternalForce.cpp +mc_planning/MathFunction.cpp +mc_planning/State.cpp +mc_planning/generator.cpp +mc_planning/PreviewWindow.cpp ) +set(mc_planning_HDR_DIR + $$) + set(mc_planning_HDR -../include/mc_planning/Pendulum.h -../include/mc_planning/api.h + ${mc_planning_HDR_DIR}/Pendulum.h + ${mc_planning_HDR_DIR}/api.h + ${mc_planning_HDR_DIR}/LinearTimeVariantInvertedPendulum.h + ${mc_planning_HDR_DIR}/LinearControl3.h + ${mc_planning_HDR_DIR}/LIPMControlByPoleAssign.h + ${mc_planning_HDR_DIR}/LIPMControlByPoleAssignWithExternalForce.h + ${mc_planning_HDR_DIR}/InterpolatorBase.h + ${mc_planning_HDR_DIR}/CubicSplineBase.h + ${mc_planning_HDR_DIR}/ClampedCubicSpline.h + ${mc_planning_HDR_DIR}/generator.h + ${mc_planning_HDR_DIR}/PreviewSteps.h + ${mc_planning_HDR_DIR}/PreviewWindow.h ) add_library(mc_planning SHARED ${mc_planning_SRC} ${mc_planning_HDR}) diff --git a/src/mc_control/CMakeLists.txt b/src/mc_control/CMakeLists.txt index aacc7f7e96..51487db9ab 100644 --- a/src/mc_control/CMakeLists.txt +++ b/src/mc_control/CMakeLists.txt @@ -52,6 +52,7 @@ if(${BUILD_CONTROLLER_SAMPLES}) add_subdirectory(samples/Admittance) add_subdirectory(samples/Impedance) add_subdirectory(samples/ExternalForces) + add_subdirectory(samples/CoMTrajectoryGeneration) endif() add_subdirectory(HalfSitPose) diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/CMakeLists.txt b/src/mc_control/samples/CoMTrajectoryGeneration/CMakeLists.txt new file mode 100644 index 0000000000..218a90be9d --- /dev/null +++ b/src/mc_control/samples/CoMTrajectoryGeneration/CMakeLists.txt @@ -0,0 +1,13 @@ +add_subdirectory(src) + +set(AROBASE "@") +set(CoMTrajectoryGeneration_STATES_INSTALL_PREFIX "${MC_CONTROLLER_INSTALL_PREFIX}/fsm/states") +set(CoMTrajectoryGeneration_STATES_DATA_INSTALL_PREFIX "${MC_CONTROLLER_INSTALL_PREFIX}/fsm/states/data") +set(CoMTrajectoryGeneration_INIT_STATE "Pause_2s") +# set(MC_STATES_DEFAULT_INSTALL_PREFIX "${MC_CONTROLLER_INSTALL_PREFIX}/fsm/states") +# set(MC_STATES_INSTALL_PREFIX "${MC_CONTROLLER_INSTALL_PREFIX}/${PROJECT_NAME}/states") +configure_file(etc/CoMTrajectoryGeneration.in.yaml "${CMAKE_CURRENT_BINARY_DIR}/etc/CoMTrajectoryGeneration.yaml") +unset(AROBASE) + +install(FILES "${CMAKE_CURRENT_BINARY_DIR}/etc/CoMTrajectoryGeneration.yaml" + DESTINATION "${MC_CONTROLLER_INSTALL_PREFIX}/etc") diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml b/src/mc_control/samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml new file mode 100644 index 0000000000..676e93f687 --- /dev/null +++ b/src/mc_control/samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml @@ -0,0 +1,75 @@ +--- +# If true, the FSM transitions are managed by an external tool +Managed: false +# If true and the FSM is self-managed, transitions should be triggered +StepByStep: false +# Change idle behaviour, if true the state is kept until transition, +# otherwise the FSM holds the last state until transition +IdleKeepState: true +# Where to look for state libraries +StatesLibraries: +- "@CoMTrajectoryGeneration_STATES_INSTALL_PREFIX@" +# Where to look for state files +StatesFiles: +- "@CoMTrajectoryGeneration_STATES_INSTALL_PREFIX@/data" +# If true, state factory will be more verbose +VerboseStateFactory: false +# Additional robots to load +robots: + ground: + module: env/ground +# General constraints, always on +constraints: +- type: contact +- type: kinematics + damper: [0.1, 0.01, 0.5] +- type: compoundJoint +# Collision constraint +collisions: +- type: collision + useMinimal: true +# Initial set of contacts +contacts: [] + +# Implement some additional text states +states: + SmoothStart: + base: Parallel + states: [Stabilizer::Standing, Pause_2s] + CoMTrajectoryGenerationTracking: + base: Parallel + states: [CoMTrajectoryGeneration_Initial, StabilizerStandingTrackCoM] + configs: + StabilizerStandingTrackCoM: + StabilizerConfig: + type: lipm_stabilizer + leftFootSurface: LeftFootCenter + rightFootSurface: RightFootCenter + enabled: true + contacts: [Left, Right] + admittance: + maxVel: + linear: [0.3,0.3,0.3] + angular: [0.5,0.5,0.5] + Left: + rotation: [0,0,0] + height: 0 + Right: + rotation: [0,0,0] + height: 0 + +# Transitions map +transitions: +- [Pause_2s, OK, SmoothStart, Auto] +- [SmoothStart, OK, CoMTrajectoryGenerationTracking, Auto] +- [CoMTrajectoryGenerationTracking, OK, CoMTrajectoryGenerationTracking, Auto] + +# Initial state +init: Pause_2s + +# State observation +ObserverPipelines: +- name: MainPipeline + observers: + - type: Encoder + - type: KinematicInertial diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/CMakeLists.txt b/src/mc_control/samples/CoMTrajectoryGeneration/src/CMakeLists.txt new file mode 100644 index 0000000000..eedc064e58 --- /dev/null +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/CMakeLists.txt @@ -0,0 +1,18 @@ +set(controller_SRC + CoMTrajectoryGeneration.cpp +) + +set(controller_HDR + CoMTrajectoryGeneration.h +) + +set(CONTROLLER_NAME CoMTrajectoryGeneration) +add_library(${CONTROLLER_NAME} SHARED ${controller_SRC} ${controller_HDR}) +set_target_properties(${CONTROLLER_NAME} PROPERTIES COMPILE_FLAGS "-DMC_CONTROL_EXPORTS") +target_link_libraries(${CONTROLLER_NAME} PUBLIC mc_rtc::mc_control_fsm) +install(TARGETS ${CONTROLLER_NAME} DESTINATION ${MC_CONTROLLER_INSTALL_PREFIX}/..) + +add_controller(${CONTROLLER_NAME}_controller lib.cpp "") +target_link_libraries(${CONTROLLER_NAME}_controller PUBLIC ${CONTROLLER_NAME}) + +add_subdirectory(states) diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp b/src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp new file mode 100644 index 0000000000..a7493577d4 --- /dev/null +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp @@ -0,0 +1,23 @@ +#include "CoMTrajectoryGeneration.h" + +CoMTrajectoryGeneration::CoMTrajectoryGeneration(mc_rbdyn::RobotModulePtr rm, + double dt, + const mc_rtc::Configuration & config) +: mc_control::fsm::Controller(rm, dt, config) +{ + + mc_rtc::log::success("CoMTrajectoryGeneration init done "); +} + +bool CoMTrajectoryGeneration::run() +{ + return mc_control::fsm::Controller::run(); +} + +void CoMTrajectoryGeneration::reset(const mc_control::ControllerResetData & reset_data) +{ + mc_control::fsm::Controller::reset(reset_data); + datastore().make_call("KinematicAnchorFrame::" + robot().name(), [](const mc_rbdyn::Robot & robot) { + return sva::interpolate(robot.surfacePose("LeftFootCenter"), robot.surfacePose("RightFootCenter"), 0.5); + }); +} diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.h b/src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.h new file mode 100644 index 0000000000..904ca0de4d --- /dev/null +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.h @@ -0,0 +1,18 @@ +#pragma once + +#include +#include + +#include "api.h" + +struct CoMTrajectoryGeneration_DLLAPI CoMTrajectoryGeneration : public mc_control::fsm::Controller +{ + CoMTrajectoryGeneration(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config); + + bool run() override; + + void reset(const mc_control::ControllerResetData & reset_data) override; + +private: + mc_rtc::Configuration config_; +}; \ No newline at end of file diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/api.h b/src/mc_control/samples/CoMTrajectoryGeneration/src/api.h new file mode 100644 index 0000000000..0f8d1c5c04 --- /dev/null +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/api.h @@ -0,0 +1,35 @@ +#pragma once + +#if defined _WIN32 || defined __CYGWIN__ +# define CoMTrajectoryGeneration_DLLIMPORT __declspec(dllimport) +# define CoMTrajectoryGeneration_DLLEXPORT __declspec(dllexport) +# define CoMTrajectoryGeneration_DLLLOCAL +#else +// On Linux, for GCC >= 4, tag symbols using GCC extension. +# if __GNUC__ >= 4 +# define CoMTrajectoryGeneration_DLLIMPORT __attribute__((visibility("default"))) +# define CoMTrajectoryGeneration_DLLEXPORT __attribute__((visibility("default"))) +# define CoMTrajectoryGeneration_DLLLOCAL __attribute__((visibility("hidden"))) +# else +// Otherwise (GCC < 4 or another compiler is used), export everything. +# define CoMTrajectoryGeneration_DLLIMPORT +# define CoMTrajectoryGeneration_DLLEXPORT +# define CoMTrajectoryGeneration_DLLLOCAL +# endif // __GNUC__ >= 4 +#endif // defined _WIN32 || defined __CYGWIN__ + +#ifdef CoMTrajectoryGeneration_STATIC +// If one is using the library statically, get rid of +// extra information. +# define CoMTrajectoryGeneration_DLLAPI +# define CoMTrajectoryGeneration_LOCAL +#else +// Depending on whether one is building or using the +// library define DLLAPI to import or export. +# ifdef CoMTrajectoryGeneration_EXPORTS +# define CoMTrajectoryGeneration_DLLAPI CoMTrajectoryGeneration_DLLEXPORT +# else +# define CoMTrajectoryGeneration_DLLAPI CoMTrajectoryGeneration_DLLIMPORT +# endif // CoMTrajectoryGeneration_EXPORTS +# define CoMTrajectoryGeneration_LOCAL CoMTrajectoryGeneration_DLLLOCAL +#endif // CoMTrajectoryGeneration_STATIC \ No newline at end of file diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/lib.cpp b/src/mc_control/samples/CoMTrajectoryGeneration/src/lib.cpp new file mode 100644 index 0000000000..9095d578f9 --- /dev/null +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/lib.cpp @@ -0,0 +1,3 @@ +#include "CoMTrajectoryGeneration.h" + +CONTROLLER_CONSTRUCTOR("CoMTrajectoryGeneration", CoMTrajectoryGeneration) diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/states/CMakeLists.txt b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/CMakeLists.txt new file mode 100644 index 0000000000..bc1ae00932 --- /dev/null +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/CMakeLists.txt @@ -0,0 +1,25 @@ +set(FSM_STATES_INSTALL_DIR "${MC_CONTROLLER_INSTALL_PREFIX}/fsm/states") +set(FSM_HDR_DIR "${PROJECT_SOURCE_DIR}/samples/mc_control/fsm/states") +set(FSM_HDR_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/include/mc_control/fsm/states") + +macro(add_fsm_state state_name state_SRC state_HDR) + add_library(${state_name} SHARED ${state_SRC} ${state_HDR} "${PROJECT_SOURCE_DIR}/include/mc_control/fsm/states/api.h") + set_target_properties(${state_name} PROPERTIES FOLDER controllers/fsm/states) + set_target_properties(${state_name} PROPERTIES OUTPUT_NAME ${state_name}) + set_target_properties(${state_name} PROPERTIES PREFIX "") + set_target_properties(${state_name} PROPERTIES COMPILE_FLAGS "-DMC_CONTROL_FSM_STATE_EXPORTS") + target_link_libraries(${state_name} PUBLIC mc_control_fsm) + install(TARGETS ${state_name} EXPORT mc_rtc_fsm_states DESTINATION ${FSM_STATES_INSTALL_DIR}) + install(FILES ${state_HDR} DESTINATION ${FSM_HDR_INSTALL_DIR}) +endmacro() + +macro(add_fsm_state_simple state_name) + add_fsm_state(${state_name} ${state_name}.cpp ${CMAKE_CURRENT_SOURCE_DIR}/${state_name}.h) +endmacro() + +add_fsm_state_simple(CoMTrajectoryGeneration_Initial) +target_link_libraries(CoMTrajectoryGeneration_Initial PUBLIC mc_planning) + +add_fsm_state_simple(StabilizerStandingTrackCoM) + +# add_fsm_data_directory(data) diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp new file mode 100644 index 0000000000..ddb63596c4 --- /dev/null +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp @@ -0,0 +1,138 @@ +#include "CoMTrajectoryGeneration_Initial.h" +#include +#include +#include "../CoMTrajectoryGeneration.h" + +namespace mc_samples +{ + +void CoMTrajectoryGeneration_Initial::configure(const mc_rtc::Configuration & config) +{ + config("previewTime", previewTime_); +} + +void CoMTrajectoryGeneration_Initial::start(mc_control::fsm::Controller & ctl) +{ + preview_ = mc_planning::CenteredPreviewWindow(previewTime_, ctl.timeStep); + /// XXX should this be CoM height or waist height? + double waist_height = ctl.realRobot().com().z(); + + leftFootPos_ = ctl.robot().surfacePose("LeftFootCenter").translation(); + rightFootPos_ = ctl.robot().surfacePose("RightFootCenter").translation(); + feetCenterPos_ = (leftFootPos_ + rightFootPos_) / 2; + + double leftY = leftFootPos_.y() - 0.04; + double rightY = rightFootPos_.y() + 0.04; + double centerY = feetCenterPos_.y(); + const Eigen::Vector3d & currCoM = ctl.robot().com(); + //clang-format off + steps_.reset({{0.0, {currCoM.x(), currCoM.y()}}, + {0.2, {0.0, centerY}}, + {2, {0.0, centerY}}, + {2.2, {0.0, rightY}}, + {4, {0.0, rightY}}, + {4.2, {0.0, leftY}}, + {6, {0.0, leftY}}, + {6.2, {0.0, rightY}}, + {8, {0.0, rightY}}, + {8.2, {0.0, centerY}}}); + steps_.initialize(); + //clang-format on + + comGenerator_ = std::make_shared(preview_, ctl.robot().mass(), waist_height); + comGenerator_->addToLogger(ctl.logger()); + comGenerator_->steps(steps_); + updateSteps(); + + using Color = mc_rtc::gui::Color; + using Style = mc_rtc::gui::plot::Style; + ctl.gui()->addPlot( + "Trajectory (Y)", mc_rtc::gui::plot::X("t", [this]() { return t_; }), + mc_rtc::gui::plot::Y("Output CoM", [this]() { return comGenerator_->OutputCOGPosition().y(); }, Color::Magenta), + mc_rtc::gui::plot::Y("Ideal CoM", [this]() { return comGenerator_->IdealCOGPosition().y(); }, Color::Red, + Style::Dashed), + mc_rtc::gui::plot::Y("Output ZMP", [this]() { return comGenerator_->OutputZMPPosition().y(); }, Color::Purple), + mc_rtc::gui::plot::Y("Ideal ZMP", [this]() { return comGenerator_->IdealZMPPosition().y(); }, Color::Blue, + Style::Dashed)); + + ctl.gui()->addElement( + {"CoMTrajectoryGeneration"}, + mc_rtc::gui::NumberInput("Delay", [this]() { return delay_; }, [this](double delay) { delay_ = delay; }), + mc_rtc::gui::NumberInput("Transition time", [this]() { return transition_; }, + [this](double transition) { transition_ = transition; }), + mc_rtc::gui::Button("Left", + [this, leftY]() { + auto curr = t_ + previewTime_ + delay_; + comGenerator_->changeFutureSteps(curr, {{curr + transition_, {0, leftY}}}); + updateSteps(); + }), + mc_rtc::gui::Button("Right", + [this, rightY]() { + auto curr = t_ + previewTime_ + delay_; + comGenerator_->changeFutureSteps(curr, {{curr + transition_, {0, rightY}}}); + updateSteps(); + }), + mc_rtc::gui::Button("Center", + [this, centerY]() { + auto curr = t_ + previewTime_ + delay_; + comGenerator_->changeFutureSteps(curr, {{curr + transition_, {0, centerY}}}); + updateSteps(); + }), + mc_rtc::gui::Button("Left-Right-Left", [this, centerY, leftY, rightY]() { + auto curr = t_ + previewTime_ + delay_; + std::vector> futureSteps; + auto t = curr + transition_; + futureSteps.push_back({t, {0, leftY}}); + t += 2; + futureSteps.push_back({t, {0, leftY}}); + t += transition_; + futureSteps.push_back({t, {0, rightY}}); + t += 2; + futureSteps.push_back({t, {0, rightY}}); + t += transition_; + futureSteps.push_back({t, {0, centerY}}); + comGenerator_->changeFutureSteps(curr, futureSteps); + updateSteps(); + })); + + ctl.logger().addLogEntry("perf_CoMGenerator[ms]", [this]() { return generationTime_; }); +} + +void CoMTrajectoryGeneration_Initial::updateSteps() +{ + mc_rtc::log::info("Time start: {}", t_); + mc_rtc::log::info("Time curr : {}", t_ + previewTime_); + mc_rtc::log::info("Desired steps:\nTime\tCoM X\tCoM Y\n{}", + mc_rtc::io::to_string(comGenerator_->steps().steps(), "\n")); +} + +bool CoMTrajectoryGeneration_Initial::run(mc_control::fsm::Controller & ctl) +{ + /* Generate trajectory starting at iter_ time (start of the past preview window) + * The trajectory result is computed for the current time (iter_+previewSize_) + */ + auto generateTrajectory = [this]() { comGenerator_->generate(iter_); }; + generationTime_ = mc_rtc::measure_ms::execution(generateTrajectory).count(); + + // Provide targets to the stabilizer (running in another state) + if(ctl.datastore().has("StabilizerStandingTrackCoM::target")) + { + ctl.datastore().call("StabilizerStandingTrackCoM::target", comGenerator_->OutputCOGPosition(), + comGenerator_->OutputCOGVelocity(), comGenerator_->OutputCOGAcceleration(), + comGenerator_->OutputZMPPosition()); + } + + iter_++; + t_ += ctl.timeStep; + output("OK"); + return false; +} + +void CoMTrajectoryGeneration_Initial::teardown(mc_control::fsm::Controller & ctl) +{ + comGenerator_->removeFromLogger(ctl.logger()); +} + +} // namespace mc_samples + +EXPORT_SINGLE_STATE("CoMTrajectoryGeneration_Initial", mc_samples::CoMTrajectoryGeneration_Initial) diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h new file mode 100644 index 0000000000..93f2e1285b --- /dev/null +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h @@ -0,0 +1,45 @@ +#pragma once + +#include +#include + +namespace mc_samples +{ + +struct CoMTrajectoryGeneration_Initial : mc_control::fsm::State +{ + + void configure(const mc_rtc::Configuration & config) override; + + void start(mc_control::fsm::Controller & ctl) override; + + bool run(mc_control::fsm::Controller & ctl) override; + + void teardown(mc_control::fsm::Controller & ctl) override; + +private: + void updateSteps(); + +private: + mc_planning::CenteredPreviewWindow preview_{0, 0}; + Eigen::Vector3d polesX_ = {1.0, 1.0, 150.0}; + Eigen::Vector3d polesY_ = {1.0, 1.0, 150.0}; + double previewTime_ = 1.6; + + unsigned iter_ = 0; ///< Number of iterations elapsed since started + double t_ = 0; ///< Time elapsed since started + double generationTime_ = 0; ///< Time spent generating the trajectory + + double delay_ = 0.5; ///< Delay before moving manually + double transition_ = 0.2; ///< Transition time between steps (interpolation) + + Eigen::Vector3d leftFootPos_ = Eigen::Vector3d::Zero(); + Eigen::Vector3d rightFootPos_ = Eigen::Vector3d::Zero(); + Eigen::Vector3d feetCenterPos_ = Eigen::Vector3d::Zero(); + +private: + std::shared_ptr comGenerator_; + mc_planning::PreviewSteps steps_; ///< Foot steps defined as time, ZMP_x, ZMP_y +}; + +} // namespace mc_samples diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp new file mode 100644 index 0000000000..3965fe53ea --- /dev/null +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp @@ -0,0 +1,66 @@ +#include "StabilizerStandingTrackCoM.h" +#include +#include "../CoMTrajectoryGeneration.h" + +#include +#include + +namespace mc_samples +{ + +void StabilizerStandingTrackCoM::configure(const mc_rtc::Configuration & config) +{ + config_.load(config); +} + +void StabilizerStandingTrackCoM::start(mc_control::fsm::Controller & ctl) +{ + // create stabilizer task from config + if(!config_.has("StabilizerConfig")) + { + config_.add("StabilizerConfig"); + } + config_("StabilizerConfig").add("type", "lipm_stabilizer"); + + stabilizerTask_ = mc_tasks::MetaTaskLoader::load( + ctl.solver(), config_("StabilizerConfig")); + // FIXME seems to hang when solution is not feasible + ctl.solver().addTask(stabilizerTask_); + stabilizerTask_->staticTarget(ctl.realRobot().com()); + + ctl.datastore().make_call("StabilizerStandingTrackCoM::target", + [this](const Eigen::Vector3d & com, const Eigen::Vector3d & comd, + const Eigen::Vector3d & comdd, + const Eigen::Vector3d & zmp) { stabilizerTask_->target(com, comd, comdd, zmp); }); + // Update anchor frame for the KinematicInertial observer + ctl.datastore().remove("KinematicAnchorFrame::" + ctl.robot().name()); + ctl.datastore().make_call("KinematicAnchorFrame::" + ctl.robot().name(), + [this](const mc_rbdyn::Robot & robot) { return stabilizerTask_->anchorFrame(robot); }); +} + +bool StabilizerStandingTrackCoM::run(mc_control::fsm::Controller & ctl) +{ + output("OK"); + return false; +} + +void StabilizerStandingTrackCoM::teardown(mc_control::fsm::Controller & ctl) +{ + ctl.datastore().remove("StabilizerStandingTrackCoM::target"); + + ctl.datastore().remove("KinematicAnchorFrame::" + ctl.robot().name()); + + // Ensure that the anchor frame remains continuous even after this state has + // been destroyed + double leftFootRatio = stabilizerTask_->leftFootRatio(); + std::string leftSurface = stabilizerTask_->footSurface(mc_tasks::lipm_stabilizer::ContactState::Left); + std::string rightSurface = stabilizerTask_->footSurface(mc_tasks::lipm_stabilizer::ContactState::Right); + ctl.datastore().make_call("KinematicAnchorFrame::" + ctl.robot().name(), [leftFootRatio, leftSurface, rightSurface]( + const mc_rbdyn::Robot & robot) { + return sva::interpolate(robot.surfacePose(leftSurface), robot.surfacePose(rightSurface), leftFootRatio); + }); +} + +} // namespace mc_samples + +EXPORT_SINGLE_STATE("StabilizerStandingTrackCoM", mc_samples::StabilizerStandingTrackCoM) diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.h b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.h new file mode 100644 index 0000000000..15838912be --- /dev/null +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.h @@ -0,0 +1,32 @@ +#pragma once + +#include + +namespace mc_tasks +{ +namespace lipm_stabilizer +{ +struct StabilizerTask; +} +} // namespace mc_tasks + +namespace mc_samples +{ + +struct StabilizerStandingTrackCoM : mc_control::fsm::State +{ + + void configure(const mc_rtc::Configuration & config) override; + + void start(mc_control::fsm::Controller & ctl) override; + + bool run(mc_control::fsm::Controller & ctl) override; + + void teardown(mc_control::fsm::Controller & ctl) override; + +private: + std::shared_ptr stabilizerTask_ = nullptr; + mc_rtc::Configuration config_; +}; + +} // namespace mc_samples diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/states/data/states.json b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/data/states.json new file mode 100644 index 0000000000..7a73a41bfd --- /dev/null +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/data/states.json @@ -0,0 +1,2 @@ +{ +} \ No newline at end of file diff --git a/src/mc_control/samples/LIPMStabilizer/etc/custom_plot.json b/src/mc_control/samples/LIPMStabilizer/etc/custom_plot.json index 754445140c..794252b140 100644 --- a/src/mc_control/samples/LIPMStabilizer/etc/custom_plot.json +++ b/src/mc_control/samples/LIPMStabilizer/etc/custom_plot.json @@ -1,24 +1,24 @@ [ - ["3D CoM", "t", ["lipm_stabilizer_jvrc1_target_pendulum_com_x", "lipm_stabilizer_jvrc1_controlRobot_com_x", "lipm_stabilizer_jvrc1_realRobot_com_x", "lipm_stabilizer_jvrc1_target_pendulum_com_y", "lipm_stabilizer_jvrc1_controlRobot_com_y", "lipm_stabilizer_jvrc1_realRobot_com_y", "lipm_stabilizer_jvrc1_target_pendulum_com_z", "lipm_stabilizer_jvrc1_controlRobot_com_z", "lipm_stabilizer_jvrc1_realRobot_com_z"], [], [], []], - ["3D DCM", "t", ["lipm_stabilizer_jvrc1_target_pendulum_dcm_x", "lipm_stabilizer_jvrc1_realRobot_dcm_x", "lipm_stabilizer_jvrc1_target_pendulum_dcm_y", "lipm_stabilizer_jvrc1_realRobot_dcm_y"], [], [], []], - ["Foot pressure both", "t", ["StabilizerTasks_cop_left_measured_wrench_fz", "StabilizerTasks_cop_left_target_wrench_fz", "StabilizerTasks_cop_right_measured_wrench_fz", "StabilizerTasks_cop_right_target_wrench_fz"], [], [], []], - ["Foot pressure left", "t", ["StabilizerTasks_cop_left_measured_wrench_fz", "StabilizerTasks_cop_left_target_wrench_fz"], [], [], []], - ["Foot pressure right", "t", ["StabilizerTasks_cop_right_measured_wrench_fz", "StabilizerTasks_cop_right_target_wrench_fz"], [], [], []], - ["Measured CoM-ZMP X", "t", ["lipm_stabilizer_jvrc1_realRobot_zmp_x", "lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_realRobot_com_x"], [], [], []], - ["Measured CoM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_realRobot_zmp_y", "lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_realRobot_com_y"], [], [], []], - ["Measured DCM-ZMP X", "t", ["lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_realRobot_dcm_x", "lipm_stabilizer_jvrc1_realRobot_zmp_x"], [], [], []], - ["Measured DCM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_realRobot_dcm_y", "lipm_stabilizer_jvrc1_realRobot_zmp_y"], [], [], []], - ["Reference CoM-ZMP X", "t", ["lipm_stabilizer_jvrc1_target_pendulum_zmp_x", "lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_target_pendulum_com_x"], [], [], []], - ["Reference CoM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_target_pendulum_zmp_y", "lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_target_pendulum_com_y"], [], [], []], - ["Reference DCM-ZMP X", "t", ["lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_target_pendulum_dcm_x", "lipm_stabilizer_jvrc1_target_pendulum_zmp_x"], [], [], []], - ["Reference DCM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_target_pendulum_dcm_y", "lipm_stabilizer_jvrc1_target_pendulum_zmp_y"], [], [], []], - ["Tracking CoM X", "t", ["lipm_stabilizer_jvrc1_target_pendulum_com_x", "lipm_stabilizer_jvrc1_controlRobot_com_x", "lipm_stabilizer_jvrc1_realRobot_com_x"], [], [], []], - ["Tracking CoM Y", "t", ["lipm_stabilizer_jvrc1_target_pendulum_com_y", "lipm_stabilizer_jvrc1_controlRobot_com_y", "lipm_stabilizer_jvrc1_realRobot_com_y"], [], [], []], - ["Tracking CoM Z", "t", ["lipm_stabilizer_jvrc1_target_pendulum_com_z", "lipm_stabilizer_jvrc1_controlRobot_com_z", "lipm_stabilizer_jvrc1_realRobot_com_z"], [], [], []], - ["Tracking DCM X", "t", ["lipm_stabilizer_jvrc1_target_pendulum_dcm_x", "lipm_stabilizer_jvrc1_realRobot_dcm_x"], [], [], []], - ["Tracking DCM Y", "t", ["lipm_stabilizer_jvrc1_target_pendulum_dcm_y", "lipm_stabilizer_jvrc1_realRobot_dcm_y"], [], [], []], - ["Tracking DCM-ZMP X", "t", ["lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_target_pendulum_dcm_x", "lipm_stabilizer_jvrc1_realRobot_dcm_x", "lipm_stabilizer_jvrc1_target_pendulum_zmp_x", "lipm_stabilizer_jvrc1_realRobot_zmp_x"], [], [], []], - ["Tracking DCM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_target_pendulum_dcm_y", "lipm_stabilizer_jvrc1_realRobot_dcm_y", "lipm_stabilizer_jvrc1_target_pendulum_zmp_y", "lipm_stabilizer_jvrc1_realRobot_zmp_y"], [], [], []], - ["Tracking ZMP X", "t", ["lipm_stabilizer_jvrc1_target_pendulum_zmp_x", "lipm_stabilizer_jvrc1_realRobot_zmp_x"], [], [], []], - ["Tracking ZMP Y", "t", ["lipm_stabilizer_jvrc1_target_pendulum_zmp_y", "lipm_stabilizer_jvrc1_realRobot_zmp_y"], [], [], []] + ["3D CoM", "t", ["lipm_stabilizer_jvrc1_stabilized_com_x", "lipm_stabilizer_jvrc1_robotControl_com_x", "lipm_stabilizer_jvrc1_robotReal_com_x", "lipm_stabilizer_jvrc1_stabilized_com_y", "lipm_stabilizer_jvrc1_robotControl_com_y", "lipm_stabilizer_jvrc1_robotReal_com_y", "lipm_stabilizer_jvrc1_stabilized_com_z", "lipm_stabilizer_jvrc1_robotControl_com_z", "lipm_stabilizer_jvrc1_robotReal_com_z"], [], [], []], + ["3D DCM", "t", ["lipm_stabilizer_jvrc1_stabilized_dcm_x", "lipm_stabilizer_jvrc1_robotReal_dcm_x", "lipm_stabilizer_jvrc1_stabilized_dcm_y", "lipm_stabilizer_jvrc1_robotReal_dcm_y"], [], [], []], + ["Foot pressure both", "t", ["lipm_stabilizer_jvrc1_tasks_cop_left_measured_wrench_fz", "lipm_stabilizer_jvrc1_tasks_cop_left_target_wrench_fz", "lipm_stabilizer_jvrc1_tasks_cop_right_measured_wrench_fz", "lipm_stabilizer_jvrc1_tasks_cop_right_target_wrench_fz"], [], [], []], + ["Foot pressure left", "t", ["lipm_stabilizer_jvrc1_tasks_cop_left_measured_wrench_fz", "lipm_stabilizer_jvrc1_tasks_cop_left_target_wrench_fz"], [], [], []], + ["Foot pressure right", "t", ["lipm_stabilizer_jvrc1_tasks_cop_right_measured_wrench_fz", "lipm_stabilizer_jvrc1_tasks_cop_right_target_wrench_fz"], [], [], []], + ["Measured CoM-ZMP X", "t", ["lipm_stabilizer_jvrc1_robotReal_zmp_x", "lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_robotReal_com_x"], [], [], []], + ["Measured CoM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_robotReal_zmp_y", "lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_robotReal_com_y"], [], [], []], + ["Measured DCM-ZMP X", "t", ["lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_robotReal_dcm_x", "lipm_stabilizer_jvrc1_robotReal_zmp_x"], [], [], []], + ["Measured DCM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_robotReal_dcm_y", "lipm_stabilizer_jvrc1_robotReal_zmp_y"], [], [], []], + ["Reference CoM-ZMP X", "t", ["lipm_stabilizer_jvrc1_stabilized_zmp_x", "lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_stabilized_com_x"], [], [], []], + ["Reference CoM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_stabilized_zmp_y", "lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_stabilized_com_y"], [], [], []], + ["Reference DCM-ZMP X", "t", ["lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_stabilized_dcm_x", "lipm_stabilizer_jvrc1_stabilized_zmp_x"], [], [], []], + ["Reference DCM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_stabilized_dcm_y", "lipm_stabilizer_jvrc1_stabilized_zmp_y"], [], [], []], + ["Tracking CoM X", "t", ["lipm_stabilizer_jvrc1_stabilized_com_x", "lipm_stabilizer_jvrc1_robotControl_com_x", "lipm_stabilizer_jvrc1_robotReal_com_x"], [], [], []], + ["Tracking CoM Y", "t", ["lipm_stabilizer_jvrc1_stabilized_com_y", "lipm_stabilizer_jvrc1_robotControl_com_y", "lipm_stabilizer_jvrc1_robotReal_com_y"], [], [], []], + ["Tracking CoM Z", "t", ["lipm_stabilizer_jvrc1_stabilized_com_z", "lipm_stabilizer_jvrc1_robotControl_com_z", "lipm_stabilizer_jvrc1_robotReal_com_z"], [], [], []], + ["Tracking DCM X", "t", ["lipm_stabilizer_jvrc1_stabilized_dcm_x", "lipm_stabilizer_jvrc1_robotReal_dcm_x"], [], [], []], + ["Tracking DCM Y", "t", ["lipm_stabilizer_jvrc1_stabilized_dcm_y", "lipm_stabilizer_jvrc1_robotReal_dcm_y"], [], [], []], + ["Tracking DCM-ZMP X", "t", ["lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_stabilized_dcm_x", "lipm_stabilizer_jvrc1_robotReal_dcm_x", "lipm_stabilizer_jvrc1_stabilized_zmp_x", "lipm_stabilizer_jvrc1_robotReal_zmp_x"], [], [], []], + ["Tracking DCM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_stabilized_dcm_y", "lipm_stabilizer_jvrc1_robotReal_dcm_y", "lipm_stabilizer_jvrc1_stabilized_zmp_y", "lipm_stabilizer_jvrc1_robotReal_zmp_y"], [], [], []], + ["Tracking ZMP X", "t", ["lipm_stabilizer_jvrc1_stabilized_zmp_x", "lipm_stabilizer_jvrc1_robotReal_zmp_x"], [], [], []], + ["Tracking ZMP Y", "t", ["lipm_stabilizer_jvrc1_stabilized_zmp_y", "lipm_stabilizer_jvrc1_robotReal_zmp_y"], [], [], []] ] diff --git a/src/mc_planning/LIPMControlByPoleAssign.cpp b/src/mc_planning/LIPMControlByPoleAssign.cpp new file mode 100644 index 0000000000..38a488de4c --- /dev/null +++ b/src/mc_planning/LIPMControlByPoleAssign.cpp @@ -0,0 +1,24 @@ +#include + +namespace mc_planning +{ +namespace linear_control_system +{ + +void LIPMControlByPoleAssign::setSystemMatrices(const double alpha, + const double beta, + const double gamma, + const double omega2) +{ + A << 0.0, 1.0, 0.0, omega2, 0.0, -omega2, 0.0, 0.0, 0.0; + + B << 0.0, 0.0, 1.0; + + C << 0.0, 0.0, 1.0; + + K << (alpha + beta + gamma) + alpha * beta * gamma / omega2, + (alpha * beta + beta * gamma + gamma * alpha) / omega2 + 1.0, -(alpha + beta + gamma); +} + +} // namespace linear_control_system +} // namespace mc_planning diff --git a/src/mc_planning/LIPMControlByPoleAssignWithExternalForce.cpp b/src/mc_planning/LIPMControlByPoleAssignWithExternalForce.cpp new file mode 100644 index 0000000000..7a5d35c004 --- /dev/null +++ b/src/mc_planning/LIPMControlByPoleAssignWithExternalForce.cpp @@ -0,0 +1,49 @@ +#include + +namespace mc_planning +{ +namespace linear_control_system +{ + +LIPMControlByPoleAssignWithExternalForce::LIPMControlByPoleAssignWithExternalForce(void) : B2(Eigen::Vector3d::Zero()) +{ + LinearControl3::Initialize(); +} + +void LIPMControlByPoleAssignWithExternalForce::Initialize(void) +{ + B2.setZero(); + LinearControl3::Initialize(); +} + +void LIPMControlByPoleAssignWithExternalForce::setSystemMatrices(const double alpha, + const double beta, + const double gamma, + const double omega2, + const double total_mass) +{ + LIPMControlByPoleAssign::setSystemMatrices(alpha, beta, gamma, omega2); + + B2 << 0.0, 1.0 / total_mass, 0.0; +} + +void LIPMControlByPoleAssignWithExternalForce::update(const double Fext, const double dt) +{ + y = (C * x)(0); + + double u = K.dot(x); + Eigen::Vector3d dx(A * x + B * u + B2 * Fext); + x += dx * dt; +} + +void LIPMControlByPoleAssignWithExternalForce::update(const Eigen::Vector3d & x_ref, const double Fext, const double dt) +{ + y = (C * x)(0); + + double u = K.dot(x - x_ref); + Eigen::Vector3d dx(A * x + B * u + B2 * Fext); + x += dx * dt; +} + +} // namespace linear_control_system +} // namespace mc_planning diff --git a/src/mc_planning/LinearControl3.cpp b/src/mc_planning/LinearControl3.cpp new file mode 100644 index 0000000000..a6b73859d7 --- /dev/null +++ b/src/mc_planning/LinearControl3.cpp @@ -0,0 +1,66 @@ +/* + * Copyright (c) 2009, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#include + +namespace mc_planning +{ + +namespace linear_control_system +{ + +LinearControl3::LinearControl3() +{ + LinearControl3::Initialize(); +} + +void LinearControl3::Initialize() +{ + A.setZero(); + B.setZero(); + C.setZero(); + K.setZero(); + x.setZero(); + x_ref.setZero(); + y = 0.0; + u = 0.0; +} + +void LinearControl3::update() +{ + u = -K.dot(x_ref - x); + x = A * x + B * u; + y = C.dot(x); +} + +void LinearControl3::update(const Eigen::Vector3d & x_ref_) +{ + x_ref = x_ref_; + update(); +} + +void LinearControl3::update(const double dt) +{ + u = -K.dot(x_ref - x); + Eigen::Vector3d dx(A * x + B * u); + x += dx * dt; + y = C.dot(x); +} + +void LinearControl3::update(const Eigen::Vector3d & x_ref_, const double dt) +{ + x_ref = x_ref_; + update(dt); +} + +} // namespace linear_control_system +} // namespace mc_planning diff --git a/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp b/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp new file mode 100644 index 0000000000..5640cd53ce --- /dev/null +++ b/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp @@ -0,0 +1,222 @@ +#include +#include +#include + +namespace cst = mc_rtc::constants; + +namespace mc_planning +{ + +namespace linear_control_system +{ + +LinearTimeVariantInvertedPendulum::LinearTimeVariantInvertedPendulum(const CenteredPreviewWindow & window, + unsigned weight_resolution, + double minHeight, + double maxHeight) +: window_(window) +{ + if(weight_resolution == 0) + { + mc_rtc::log::error_and_throw("[LinearTimeVariantInvertedPendulum::Initialize] Invalid " + "weight resolution {} (must be >0, recommended value 20000) ", + weight_resolution); + } + + /** Create lookup tables for sqrt(w2), cosh(w*dt), sinh(w*dt) */ + auto omega2 = [](double h) { return mc_rtc::constants::GRAVITY / h; }; + auto omega = [](double w2) { return std::sqrt(w2); }; + auto chk = [this](double w2) { return cosh(wTable_(w2) * window_.dt()); }; + auto shk = [this](double w2) { return sinh(wTable_(w2) * window_.dt()); }; + double minOmega2 = omega2(maxHeight); + double maxOmega2 = omega2(minHeight); + wTable_.create(weight_resolution, minOmega2, maxOmega2, omega, "omega^2 -> sqrt(w2)"); + chkTable_.create(weight_resolution, minOmega2, maxOmega2, chk, "omega^2 -> cosh(sqrt(omega^2 * dt)"); + shkTable_.create(weight_resolution, minOmega2, maxOmega2, shk, "omega^2 -> sinh(sqrt(omega^2 * dt)"); + + unsigned previewSize = window_.size(); + m_A.resize(previewSize, Matrix22::Identity()); + m_An.resize(previewSize, Matrix22::Identity()); + m_B.resize(previewSize, Vector2::Zero()); + m_Bn.resize(previewSize, Vector2::Zero()); + + m_X.resize(previewSize, Vector2::Zero()); + m_p_ref.setZero(previewSize); + m_w2.setZero(previewSize); + m_w.setZero(previewSize); +} + +void LinearTimeVariantInvertedPendulum::initMatrices(double waist_height) +{ + const double w2 = cst::GRAVITY / waist_height; + + // Initialize A matrix corresponding to the given pendulum height + const double w = wTable_(w2); + const double ch = chkTable_(w2); + const double sh = shkTable_(w2); + + auto init_m22 = [&]() { + Eigen::Matrix2d A; + A << ch, sh / w, w * sh, ch; + + for(unsigned i = 0; i < window_.size(); i++) + { + m_A.push_front(A); + } + }; + + auto init_v2 = [&]() { + Vector2 v; + v << 1 - ch, -w * sh; + + for(unsigned i = 0; i < window_.size(); i++) + { + m_B.push_front(v); + } + }; + + // Initialize the whole preview window at constant height + init_m22(); + init_v2(); +} + +void LinearTimeVariantInvertedPendulum::update() +{ + // set newest matrices + { + double w2 = m_w2[window_.last()]; + const double w = wTable_(w2); + const double ch = chkTable_(w2); + const double sh = shkTable_(w2); + m_w[window_.last()] = w; + Matrix22 Anew; + Anew << ch, sh / w, w * sh, ch; + Vector2 Bnew; + Bnew << 1 - ch, -w * sh; + m_A.push_back(Anew); + m_B.push_back(Bnew); + } + + for(unsigned i = 0; i < window_.halfSize(); i++) + { + m_w[i] = wTable_(m_w2[i]); + } + + // update future matrices + for(unsigned i = window_.center(); i < window_.last(); i++) + { + const double w2 = m_w2[i]; + const double w = wTable_(w2); + const double ch = chkTable_(w2); + const double sh = shkTable_(w2); + m_w[i] = w; + m_A[i] << ch, sh / w, w * sh, ch; + m_B[i] << 1 - ch, -w * sh; + } + + unsigned lastIdx = window_.last(); + m_An[lastIdx] = m_A[lastIdx]; + m_Bn[lastIdx] = m_B[lastIdx]; + + Vector2 Bsum(m_Bn[lastIdx] * m_p_ref(lastIdx)); + for(int i = static_cast(lastIdx) - 1; i >= 0; i--) + { + unsigned idx = static_cast(i); + m_An[idx].noalias() = m_An[idx + 1u] * m_A[idx]; + m_Bn[idx].noalias() = m_An[idx + 1u] * m_B[idx]; + Bsum.noalias() += m_Bn[idx] * m_p_ref(idx); + } + + Vector2 Xg(m_p_ref(0), m_p_ref(lastIdx)); + Vector2 Vg; + Vg << -(m_An[0](0, 0) * Xg(0) - Xg(1) + Bsum(0)) / m_An[0](0, 1), + -m_An[0](1, 1) * (m_An[0](0, 0) * Xg(0) - Xg(1) + Bsum(0)) / m_An[0](0, 1) + m_An[0](1, 0) * Xg(0) + Bsum(1); + + m_X[0] << Xg(0), Vg(0); // set initial states + for(unsigned i = 0; i < window_.last(); i++) + { + m_X[i + 1] = (m_A[i] * m_X[i] + m_B[i] * m_p_ref(i)).eval(); + } +} + +void LinearTimeVariantInvertedPendulum::generate(Eigen::VectorXd & cog_pos, + Eigen::VectorXd & cog_vel, + Eigen::VectorXd & cog_acc, + Eigen::VectorXd & p_ref) +{ + // update future matrices + for(unsigned i = 0; i < window_.size(); i++) + { + const double w2 = m_w2[i]; + const double w = wTable_(w2); + const double ch = chkTable_(w2); + const double sh = shkTable_(w2); + m_w[i] = w; + m_A[i] << ch, sh / w, w * sh, ch; + m_B[i] << 1 - ch, -w * sh; + } + + auto lastIdx = window_.last(); + m_An[lastIdx] = m_A[lastIdx]; + m_Bn[lastIdx] = m_B[lastIdx]; + + Vector2 Bsum(m_Bn[lastIdx] * m_p_ref(lastIdx)); + for(int i = static_cast(lastIdx) - 1; i >= 0; i--) + { + unsigned idx = static_cast(i); + m_An[idx].noalias() = m_An[idx + 1u] * m_A[idx]; + m_Bn[idx].noalias() = m_An[idx + 1u] * m_B[idx]; + Bsum.noalias() += m_Bn[idx] * m_p_ref(idx); + } + + Vector2 Xg(m_p_ref(0), m_p_ref(lastIdx)); + Vector2 Vg; + Vg << -(m_An[0](0, 0) * Xg(0) - Xg(1) + Bsum(0)) / m_An[0](0, 1), + -m_An[0](1, 1) * (m_An[0](0, 0) * Xg(0) - Xg(1) + Bsum(0)) / m_An[0](0, 1) + m_An[0](1, 0) * Xg(0) + Bsum(1); + + Vector2 X(Xg(0), Vg(0)); + for(unsigned i = 0; i < lastIdx; i++) + { + cog_pos(i) = X(0); + cog_vel(i) = X(1); + cog_acc(i) = m_w2[i] * (cog_pos(i) - m_p_ref(i)); + p_ref(i) = m_p_ref(i); + + X = (m_A[i] * X + m_B[i] * m_p_ref(i)).eval(); + } + cog_pos(lastIdx) = X(0); + cog_vel(lastIdx) = X(1); + cog_acc(lastIdx) = m_w2[lastIdx] * (cog_pos(lastIdx) - m_p_ref(lastIdx)); + p_ref(lastIdx) = m_p_ref(lastIdx); +} + +LinearTimeVariantInvertedPendulum::State LinearTimeVariantInvertedPendulum::getState(unsigned n_time) const +{ + const auto t = window_.center() + n_time; + State s; + s.p = m_p_ref(t); + s.pdot = (s.p - m_p_ref[t - 1u]) / window_.dt(); + s.cog_pos = m_X[t](0); + s.cog_vel = m_X[t](1); + s.cog_acc = m_w2[t] * (s.cog_pos - s.p); + return s; +} + +void LinearTimeVariantInvertedPendulum::getState(unsigned n_time, + double & cog_pos, + double & cog_vel, + double & cog_acc, + double & p, + double & pdot) +{ + const auto t = window_.center() + n_time; + p = m_p_ref(t); + pdot = (p - m_p_ref[t - 1u]) / window_.dt(); + cog_pos = m_X[t](0); + cog_vel = m_X[t](1); + cog_acc = m_w2[t] * (cog_pos - p); +} + +} // namespace linear_control_system + +} // namespace mc_planning diff --git a/src/mc_planning/MathFunction.cpp b/src/mc_planning/MathFunction.cpp new file mode 100644 index 0000000000..fb63f4d0d3 --- /dev/null +++ b/src/mc_planning/MathFunction.cpp @@ -0,0 +1,72 @@ +/* + * Copyright (c) 2009, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#include + +namespace mc_planning +{ + +Eigen::Vector3d yrpFromRot(const Eigen::Matrix3d & m) +{ + double roll = atan2(-m(1, 2), m(2, 2)); + double cr = cos(roll); + double pitch = atan2(cr * m(0, 2), m(2, 2)); + double cp = cos(pitch), sr = sin(roll); + double yaw = atan2(cp * (cr * m(1, 0) + sr * m(2, 0)), m(0, 0)); + + return Eigen::Vector3d(roll, pitch, yaw); +} + +double yawFromRot(const Eigen::Matrix3d & r) +{ + return atan2(r(1, 0), r(0, 0)); +} + +Eigen::Vector3d omegaFromRotApproximation(const Eigen::Matrix3d & r) +{ + const double k = -0.5; + return Eigen::Vector3d((r(1, 2) - r(2, 1)) * k, (r(2, 0) - r(0, 2)) * k, (r(0, 1) - r(1, 0)) * k); +} + +double Saturation(const double & data, const double ulimit, const double llimit) +{ + if(data > ulimit) + return ulimit; + else if(data < llimit) + return llimit; + return data; +} + +double Threshold(const double & data, const double ulimit, const double llimit) +{ + if(data > ulimit) + return data - ulimit; + else if(data < llimit) + return data - llimit; + return 0.0; +} + +double NormalizedTrapezoidCurve(int n_now, int n_ini, int n_acc, int n_dec, int n_end) +{ + if(n_now < n_ini) + return 0.0; + else if(n_now <= n_acc) + return polynomial3((double)((n_now - n_ini)) / (double)((n_acc - n_ini))); + else if(n_now <= n_dec) + return 1.0; + else if(n_now <= n_end) + return polynomial3((double)((n_end - n_now)) / (double)((n_end - n_dec))); + + return 0.0; +} + +} // namespace mc_planning diff --git a/src/mc_planning/PreviewWindow.cpp b/src/mc_planning/PreviewWindow.cpp new file mode 100644 index 0000000000..e5834c6624 --- /dev/null +++ b/src/mc_planning/PreviewWindow.cpp @@ -0,0 +1,105 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#include + +namespace mc_planning +{ + +PreviewWindowView CenteredPreviewWindow::all(Index startIndex) const noexcept +{ + return PreviewWindowView{*this, startIndex, Index{0}, fullSize_ - 1u}; +} + +PreviewWindowView CenteredPreviewWindow::all(Time startTime) const noexcept +{ + return PreviewWindowView{*this, index(startTime), Index{0}, fullSize_ - 1u}; +} + +PreviewWindowView CenteredPreviewWindow::all() const noexcept +{ + return all(Index{0}); +} + +PreviewWindowView CenteredPreviewWindow::past(Index startIndex) const noexcept +{ + return PreviewWindowView{*this, startIndex, Index{0}, halfSize() - 1u}; +} + +PreviewWindowView CenteredPreviewWindow::past(Time startTime) const noexcept +{ + return PreviewWindowView{*this, index(startTime), Index{0}, halfSize() - 1u}; +} + +PreviewWindowView CenteredPreviewWindow::past() const noexcept +{ + return past(Index{0}); +} + +PreviewWindowView CenteredPreviewWindow::pastInclusive(Index startIndex) const noexcept +{ + return PreviewWindowView{*this, startIndex, Index{0}, Index{halfSize()}}; +} + +PreviewWindowView CenteredPreviewWindow::pastInclusive(Time startTime) const noexcept +{ + return PreviewWindowView{*this, index(startTime), Index{0}, Index{halfSize()}}; +} + +PreviewWindowView CenteredPreviewWindow::pastInclusive() const noexcept +{ + return pastInclusive(Index{0}); +} + +PreviewWindowView CenteredPreviewWindow::future(Index startIndex) const noexcept +{ + return PreviewWindowView{*this, startIndex, halfSize() + 1u, fullSize_ - 1u}; +} + +PreviewWindowView CenteredPreviewWindow::future(Time startTime) const noexcept +{ + return PreviewWindowView{*this, index(startTime), halfSize() + 1u, fullSize_ - 1u}; +} + +PreviewWindowView CenteredPreviewWindow::future() const noexcept +{ + return future(Index{0}); +} + +PreviewWindowView CenteredPreviewWindow::futureInclusive(Index startIndex) const noexcept +{ + return PreviewWindowView{*this, startIndex, halfSize(), fullSize_ - 1u}; +} + +PreviewWindowView CenteredPreviewWindow::futureInclusive(Time startTime) const noexcept +{ + return PreviewWindowView{*this, index(startTime), halfSize(), fullSize_ - 1u}; +} + +PreviewWindowView CenteredPreviewWindow::futureInclusive() const noexcept +{ + return futureInclusive(Index{0}); +} + +Index PreviewElement::index() const noexcept +{ + return index_; +} + +Time PreviewElement::time() const noexcept +{ + return window_.time(Index(index_)); +} + +Index PreviewElement::localIndex() const noexcept +{ + return Index{index_ - window_.startIndex()}; +} + +Time PreviewElement::localTime() const noexcept +{ + return window_.time(Index(index_ - window_.startIndex())); +} + +} // namespace mc_planning diff --git a/src/mc_planning/State.cpp b/src/mc_planning/State.cpp new file mode 100644 index 0000000000..f9ec0261cb --- /dev/null +++ b/src/mc_planning/State.cpp @@ -0,0 +1,138 @@ +/* + * Copyright (c) 2009, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#include +#include +// #include +// #include +// + +namespace mc_planning +{ + +//////////////////////////////////////////////////////////// +StateP::StateP() +{ + StateP::Initialize(); +} + +StateP::StateP(const Eigen::Vector3d & p) : P(p) {} + +StateP::StateP(const StateP & i_state) +{ + StateP::Copy(i_state); +} + +StateP & StateP::operator=(const StateP & i_state) +{ + StateP::Copy(i_state); + return (*this); +} + +std::ostream & operator<<(std::ostream & os, const StateP & s) +{ + os << "P = " << s.P.transpose() << std::endl; + ; + return os; +} + +void StateP::Initialize() +{ + P.setZero(); +} + +void StateP::Copy(const StateP & i_state) +{ + P = i_state.P; +} + +//////////////////////////////////////////////////////////// +StatePV::StatePV() +{ + StatePV::Initialize(); +} + +StatePV::StatePV(const Eigen::Vector3d & p, const Eigen::Vector3d & v) : StateP(p), V(v) {} + +StatePV::StatePV(const StatePV & i_state) +{ + StatePV::Copy(i_state); +} + +StatePV & StatePV::operator=(const StatePV & i_state) +{ + StatePV::Copy(i_state); + return (*this); +} + +std::ostream & operator<<(std::ostream & os, const StatePV & s) +{ + os << "P = " << s.P.transpose() << std::endl; + os << "V = " << s.V.transpose() << std::endl; + ; + return os; +} + +void StatePV::Initialize() +{ + StateP::Initialize(); + V.setZero(); +} + +void StatePV::Copy(const StatePV & i_state) +{ + StateP::Copy(i_state); + V = i_state.V; +} + +//////////////////////////////////////////////////////////// +StatePVA::StatePVA() +{ + StatePVA::Initialize(); +} + +StatePVA::StatePVA(const Eigen::Vector3d & p, const Eigen::Vector3d & v, const Eigen::Vector3d & a) +: StatePV(p, v), Vdot(a) +{ +} + +StatePVA::StatePVA(const StatePVA & i_state) +{ + StatePVA::Copy(i_state); +} + +StatePVA & StatePVA::operator=(const StatePVA & i_state) +{ + StatePVA::Copy(i_state); + return (*this); +} + +std::ostream & operator<<(std::ostream & os, const StatePVA & s) +{ + os << "P = " << s.P.transpose() << std::endl; + os << "V = " << s.V.transpose() << std::endl; + os << "Vdot = " << s.Vdot.transpose() << std::endl; + return os; +} + +void StatePVA::Initialize() +{ + StatePV::Initialize(); + Vdot.setZero(); +} + +void StatePVA::Copy(const StatePVA & i_state) +{ + StatePV::Copy(i_state); + Vdot = i_state.Vdot; +} +} // namespace mc_planning diff --git a/src/mc_planning/generator.cpp b/src/mc_planning/generator.cpp new file mode 100644 index 0000000000..afd3aa923e --- /dev/null +++ b/src/mc_planning/generator.cpp @@ -0,0 +1,222 @@ +#include +#include +#include +#include + +using namespace mc_planning::motion_interpolator; + +constexpr int X = 0; +constexpr int Y = 1; +constexpr int Z = 2; + +namespace mc_planning +{ + +generator::generator(const CenteredPreviewWindow & preview, double mass, double waist_height) +: preview_(preview), m_ComInterp(std::make_shared>(1.0, preview_.dt() / 2.)), + // XXX Should be only one computation for both to speed up computation + m_ipm_long{ + linear_control_system::LinearTimeVariantInvertedPendulum(preview, 20000), + linear_control_system::LinearTimeVariantInvertedPendulum(preview, 20000), + }, + m_mass(mass), m_waist_height(waist_height) +{ + mc_rtc::log::info("Creating generator with mass={}, waist_height={}", mass, waist_height); + + auto initialize = [this, &waist_height](unsigned axis) { + m_ipm_short[axis].Initialize(); + m_poles[X] << 1.0, 1.0, 150.0; + m_virtual_height[axis].setZero(preview_.size()); + m_ipm_long[axis].initMatrices(waist_height); + }; + initialize(X); + initialize(Y); + + auto previewSize = preview_.size(); + mc_rtc::log::info("Initializing with size {} ", previewSize); + m_cog_height.setZero(previewSize); + m_cog_dot_height.setZero(previewSize); + m_cog_ddot_height.setZero(previewSize); + m_COG_ideal.P << 0.0, 0.0, waist_height; +} + +void generator::setupCOGHeight(unsigned n_current) +{ + // The first time, initialize the whole trajectory + // with constant waist height + // Ignores lateral sway during the trajectory? + if(n_current == 0) + { + m_ComInterp->push_back(0u, m_waist_height); + m_ComInterp->push_back(preview_.index(Time(m_steps.back().t())), m_waist_height); + m_ComInterp->update(); + } + + for(const auto & step : preview_.all()) + { + auto n = step.index(); + m_ComInterp->get(n_current + n, m_cog_height[n], m_cog_dot_height[n], m_cog_ddot_height[n]); + } + + // Interpolated value at current time + unsigned current = preview_.center(); + m_COG_ideal.P(Z) = m_cog_height[current]; + m_COG_ideal.V(Z) = m_cog_dot_height[current]; + m_COG_ideal.Vdot(Z) = m_cog_ddot_height[current]; +} + +void generator::setupTimeTrajectories(unsigned n_current) +{ + auto currentPreview = preview_.all(Index(n_current)); + m_steps.nextWindow(currentPreview.startTime()); + Eigen::VectorXd & px_ref = m_ipm_long[X].p_ref(); + Eigen::VectorXd & py_ref = m_ipm_long[Y].p_ref(); + + for(const auto & current : currentPreview) + { + unsigned i = current.localIndex(); + const double currTime = current.time(); + + m_virtual_height[X](i) = 0.0; + m_virtual_height[Y](i) = 0.0; + + // Move to next step if necessary + m_steps.update(currTime); + const auto & prevStep = m_steps.previous(); + if(!m_steps.isLastStep()) + { + const auto & nextStep = m_steps.next(); + /** + * @brief Interpolates the reference ZMP from one step to the next + * @param axis 0: Step CoM x, 0: Step CoM Y + */ + auto interpolateRefXY = [&](const Eigen::Index axis) { + return prevStep.step()(axis) + + (nextStep.step()(axis) - prevStep.step()(axis)) + * polynomial3((currTime - prevStep.t()) / (nextStep.t() - prevStep.t())); + }; + + px_ref(i) = interpolateRefXY(0); + py_ref(i) = interpolateRefXY(1); + } + else + { /* No previous step provided, start with the ZMP at the next step */ + px_ref(i) = prevStep.step().x(); + py_ref(i) = prevStep.step().y(); + } + + m_ipm_long[X].w2(i) = + (mc_rtc::constants::GRAVITY + m_cog_ddot_height[i]) / (m_cog_height[i] - m_virtual_height[X](i)); + m_ipm_long[Y].w2(i) = + (mc_rtc::constants::GRAVITY + m_cog_ddot_height[i]) / (m_cog_height[i] - m_virtual_height[Y](i)); + } + + unsigned current = preview_.center(); + m_Pcalpha_ideal(Z) = (m_virtual_height[X](current) + m_virtual_height[Y](current)) / 2.0; +} + +void generator::generateTrajectories() +{ + StatePV COG_ideal_pre_next(m_COG_ideal.P, m_COG_ideal.V); + Eigen::Vector3d Pcalpha_ideal_pre_next(m_Pcalpha_ideal); + + /** + * Computes long-term trajectory + */ + auto computeLongTerm = [&](unsigned axis) { + m_ipm_long[axis].update(); + + // Current time + m_ipm_long[axis].getState(0, m_COG_ideal_pre.P(axis), m_COG_ideal_pre.V(axis), m_COG_ideal_pre.Vdot(axis), + m_Pcalpha_ideal_pre(axis), m_Vcalpha_ideal_pre(axis)); + // Next timestep + m_ipm_long[axis].getState(1, m_COG_ideal.P(axis), m_COG_ideal.V(axis), m_COG_ideal.Vdot(axis), + m_Pcalpha_ideal(axis), m_Vcalpha_ideal(axis)); + }; + + /** + * Compute short term trajectory + * Depends on the long term trajectory results + * + * Generates the CoM and ZMP modification to be applied to the ideal long-term so that the short term + * - m_COG_cmp: Modification of the ideal CoG state (position and velocity) + * - m_Pcalpha_cmp: Modifiecation of the ideal ZMP state (position and velocity) + **/ + auto computeShortTerm = [&](unsigned axis) { + unsigned current = preview_.center(); + double omega2 = m_ipm_long[axis].w2(current); + double omega = sqrt(omega2); + m_ipm_short[axis].setSystemMatrices(omega * m_poles[axis](0), omega * m_poles[axis](1), m_poles[axis](2), omega2, + m_mass); + + m_COG_cmp.P(axis) += COG_ideal_pre_next.P(axis) - m_COG_ideal_pre.P(axis); + m_COG_cmp.V(axis) += COG_ideal_pre_next.V(axis) - m_COG_ideal_pre.V(axis); + m_Pcalpha_cmp(axis) += Pcalpha_ideal_pre_next(axis) - m_Pcalpha_ideal(axis); + + m_ipm_short[axis].setStateVariables(m_COG_cmp.P(axis), m_COG_cmp.V(axis), m_Pcalpha_cmp(axis)); + double Fext = 0.0; + m_ipm_short[axis].update(Fext, preview_.dt()); + m_ipm_short[axis].getStateVariables(m_COG_cmp.P(axis), m_COG_cmp.V(axis), m_Pcalpha_cmp(axis), m_Vcalpha_cmp(axis)); + }; + + /** @brief Computes both long-term and short-term trajectories */ + auto computeTrajectories = [&](unsigned axis) { + computeLongTerm(axis); + computeShortTerm(axis); + }; + + /** + * Computes the desired output CoG and ZMP state composed + * of the ideal long-term trajectory output modified by the short-term + * trajectory to ensure continuity + */ + auto applyShortTermCompensation = [this]() { + m_COG_out.P = m_COG_ideal.P + m_COG_cmp.P; + m_COG_out.V = m_COG_ideal.V + m_COG_cmp.V; + m_COG_out.Vdot = m_COG_ideal.Vdot + m_COG_cmp.Vdot; + + m_Pcalpha_out = m_Pcalpha_ideal + m_Pcalpha_cmp; + m_Pcalpha_motion_out = m_Pcalpha_out + m_Vcalpha_ideal * m_omega_valpha * preview_.dt(); + }; + + computeTrajectories(X); + computeTrajectories(Y); + applyShortTermCompensation(); +} + +void generator::generate(unsigned n_time) +{ + if(!m_steps.isValid()) + { + mc_rtc::log::error_and_throw("[generator] Invalid reference provided, call steps()"); + } + setupCOGHeight(n_time); + setupTimeTrajectories(n_time); + generateTrajectories(); +} + +void generator::addToLogger(mc_rtc::Logger & logger) +{ + // clang-format off + // Requested ideal trajectories + logger.addLogEntry("generator_IdealCOGPosition", [this]() { return this->IdealCOGPosition(); }); + logger.addLogEntry("generator_IdealZMPPosition", [this]() { return this->IdealZMPPosition(); }); + // Generated trajectories + logger.addLogEntry("generator_OutputCOGPosition", [this]() { return this->OutputCOGPosition(); }); + logger.addLogEntry("generator_OutputZMPPosition", [this]() { return this->OutputZMPPosition(); }); + logger.addLogEntry("generator_CompensatedCOGPosition", [this]() { return this->CompensatedCOGPosition(); }); + logger.addLogEntry("generator_CompensatedZMPPosition", [this]() { return this->CompensatedZMPPosition(); }); + // clang-format on +} + +void generator::removeFromLogger(mc_rtc::Logger & logger) +{ + logger.removeLogEntry("generator_IdealCOGPosition"); + logger.removeLogEntry("generator_IdealZMPPosition"); + logger.removeLogEntry("generator_OutputCOGPosition"); + logger.removeLogEntry("generator_OutputZMPPosition"); + logger.removeLogEntry("generator_CompensatedCOGPosition"); + logger.removeLogEntry("generator_CompensatedZMPPosition"); +} + +} /* namespace mc_planning */ diff --git a/src/mc_robots/jvrc1.cpp b/src/mc_robots/jvrc1.cpp index 2f238bd6f1..01bb54426d 100644 --- a/src/mc_robots/jvrc1.cpp +++ b/src/mc_robots/jvrc1.cpp @@ -83,9 +83,9 @@ JVRC1RobotModule::JVRC1RobotModule(bool fixed) : RobotModule(std::string(JVRC_VA "L_KNEE", "L_ANKLE_P", "L_ANKLE_R"}; _lipmStabilizerConfig.torsoPitch = 0; _lipmStabilizerConfig.copAdmittance = Eigen::Vector2d{0.01, 0.01}; - _lipmStabilizerConfig.dcmPropGain = 5.0; - _lipmStabilizerConfig.dcmIntegralGain = 10; - _lipmStabilizerConfig.dcmDerivGain = 0.5; + _lipmStabilizerConfig.dcmPropGain = 2.0; + _lipmStabilizerConfig.dcmIntegralGain = 3.0; + _lipmStabilizerConfig.dcmDerivGain = 0.0; _lipmStabilizerConfig.dcmDerivatorTimeConstant = 1; _lipmStabilizerConfig.dcmIntegratorTimeConstant = 10; } diff --git a/src/mc_rtc/gui/StateBuilder.cpp b/src/mc_rtc/gui/StateBuilder.cpp index 9a541e90ad..628660b861 100644 --- a/src/mc_rtc/gui/StateBuilder.cpp +++ b/src/mc_rtc/gui/StateBuilder.cpp @@ -26,6 +26,7 @@ const Color Color::Magenta = Color(1, 0, 1, 1); const Color Color::Yellow = Color(1, 1, 0, 1); const Color Color::Gray = Color(0.6, 0.6, 0.6, 1); const Color Color::LightGray = Color(0.75, 0.75, 0.75, 1); +const Color Color::Purple = Color(0.5, 0, 1, 1); const std::map Color::ColorMap{ {"white", Color::White}, {"black", Color::Black}, {"red", Color::Red}, {"green", Color::Green}, {"blue", Color::Blue}, {"cyan", Color::Cyan}, {"magenta", Color::Magenta}, {"yellow", Color::Yellow}, diff --git a/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp b/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp index 60d9869eec..aa71666a41 100644 --- a/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp +++ b/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp @@ -484,6 +484,7 @@ void StabilizerTask::setContacts(const ContactDescriptionVector & contacts) "requires at least one contact to be set."); } contacts_.clear(); + addContacts_.clear(); // Reset support area boundaries supportMin_ = std::numeric_limits::max() * Eigen::Vector2d::Ones(); @@ -619,6 +620,13 @@ void StabilizerTask::target(const Eigen::Vector3d & com, double comHeight = comTarget_.z() - zmpTarget_.z(); omega_ = std::sqrt(constants::gravity.z() / comHeight); dcmTarget_ = comTarget_ + comdTarget_ / omega_; + + // Only stored for logging purposes + comRef_ = com; + comdRef_ = comd; + comddRef_ = comdd; + zmpRef_ = zmp; + dcmRef_ = dcmTarget_; } void StabilizerTask::setExternalWrenches(const std::vector & surfaceNames, @@ -758,8 +766,8 @@ void StabilizerTask::run() saturateWrench(desiredWrench_, footTasks[ContactState::Right], contacts_.at(ContactState::Right)); footTasks[ContactState::Left]->setZeroTargetWrench(); } - - distribZMP_ = mc_rbdyn::zmp(distribWrench_, zmpFrame_); + // Distributed ZMP + zmpTarget_ = mc_rbdyn::zmp(desiredWrench_, zmpFrame_, c_.safetyThresholds.MIN_NET_TOTAL_FORCE_ZMP); updateCoMTaskZMPCC(); updateFootForceDifferenceControl(); @@ -1102,9 +1110,12 @@ void StabilizerTask::updateCoMTaskZMPCC() { zmpcc_.configure(c_.zmpcc); zmpcc_.enabled(enabled_); - zmpcc_.update(distribZMP_, measuredZMP_, zmpFrame_, dt_); + // Distributed ZMP + zmpcc_.update(zmpTarget_, measuredZMP_, zmpFrame_, dt_); } zmpcc_.apply(comTarget_, comdTarget_, comddTarget_); + // Update the DCM target based on ZMPCC result + dcmTarget_ = comTarget_ + comdTarget_ / omega_; } void StabilizerTask::updateFootForceDifferenceControl() diff --git a/src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp b/src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp index 92943f6bb2..ebae16d980 100644 --- a/src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp +++ b/src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp @@ -371,16 +371,21 @@ void StabilizerTask::addToLogger(mc_rtc::Logger & logger) MC_RTC_LOG_HELPER(name_ + "_left_foot_ratio", leftFootRatio_); // Stabilizer targets - MC_RTC_LOG_HELPER(name_ + "_target_pendulum_com", comTarget_); - MC_RTC_LOG_HELPER(name_ + "_target_pendulum_comd", comdTarget_); - MC_RTC_LOG_HELPER(name_ + "_target_pendulum_comdd", comddTarget_); - MC_RTC_LOG_HELPER(name_ + "_target_pendulum_dcm", dcmTarget_); - MC_RTC_LOG_HELPER(name_ + "_target_pendulum_omega", omega_); - MC_RTC_LOG_HELPER(name_ + "_target_pendulum_zmp", zmpTarget_); - MC_RTC_LOG_HELPER(name_ + "_target_pendulum_zmpd", zmpdTarget_); - MC_RTC_LOG_HELPER(name_ + "_target_stabilizer_zmp", distribZMP_); - - logger.addLogEntry(name_ + "_contactState", this, [this]() -> int { + MC_RTC_LOG_HELPER(name_ + "_reference_com", comRef_); + MC_RTC_LOG_HELPER(name_ + "_reference_comd", comdRef_); + MC_RTC_LOG_HELPER(name_ + "_reference_comdd", comddRef_); + MC_RTC_LOG_HELPER(name_ + "_reference_dcm", dcmRef_); + MC_RTC_LOG_HELPER(name_ + "_reference_omega", omega_); + MC_RTC_LOG_HELPER(name_ + "_reference_zmp", zmpRef_); + + // Stabilizer results + MC_RTC_LOG_HELPER(name_ + "_stabilized_com", comTarget_); + MC_RTC_LOG_HELPER(name_ + "_stabilized_comd", comdTarget_); + MC_RTC_LOG_HELPER(name_ + "_stabilized_comdd", comddTarget_); + MC_RTC_LOG_HELPER(name_ + "_stabilized_dcm", dcmTarget_); + MC_RTC_LOG_HELPER(name_ + "_stabilized_zmp", zmpTarget_); + + logger.addLogEntry(name_ + "_contactState", this, [this]() -> double { if(inDoubleSupport()) return 0; else if(inContact(ContactState::Left)) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 0b94f417c9..92e6acbc6f 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -136,3 +136,19 @@ target_link_libraries(dummyServer PUBLIC mc_control) mc_rtc_test(test_filters mc_filter SpaceVecAlg::SpaceVecAlg) mc_rtc_test(test_interpolation mc_control) + +##################################### +# -- Temporary HMC-related tests -- # +##################################### +mc_rtc_test(test_hmc mc_planning) + +####################################### +# -- Regression tests against logs -- # +####################################### +# Compares excecutable samples against logs of their expected exection +# These tests require network access to download the log files +# Failing of these tests may occur if the methods are changed on purpose. In that case the +# new expected log should be updated. +if(NOT ${DISABLE_LOG_REGRESSION_TESTS}) + add_subdirectory(log_tests) +endif() diff --git a/tests/log_tests/CMakeLists.txt b/tests/log_tests/CMakeLists.txt new file mode 100644 index 0000000000..6cf3ebab8c --- /dev/null +++ b/tests/log_tests/CMakeLists.txt @@ -0,0 +1,34 @@ +# - Dowloads log file from LOG_URL +# - Run the corresponding target EXECUTABLE_TARGET that was used to generate this log file (with optional parameters ARGN). +# This will generates a new log file whose path should be specified with GENERATED_LOG +# - Uses mc_bin_compare tool to compare the entries in both logs +macro(mc_rtc_log_test NAME LOG_URL EXECUTABLE_TARGET GENERATED_LOG) + set(REFERENCE_LOG ${CMAKE_CURRENT_BINARY_DIR}/reference_log.bin) + message(STATUS "Generating ${CMAKE_CURRENT_BINARY_DIR}/test_log_${NAME}.sh") + +file(GENERATE + OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test_log_${NAME}.sh" + CONTENT +" +#!/bin/sh +wget -O ${REFERENCE_LOG} ${LOG_URL} +$ ${ARGN} +$ ${REFERENCE_LOG} ${GENERATED_LOG} +") + +### file(GENERATE...) does not support permissions, and the files are not generated until GENERATE time (e.g after all the cmake scripts have been processed). Thus the only way to set permissions would be at build time, which involves creating extra custom targets + +# Compare logs (will probably only work on linux) + add_test(NAME TestRegressionCoMGeneration + COMMAND "/bin/sh ${CMAKE_CURRENT_BINARY_DIR}/test_log_${NAME}.sh") +endmacro() + +if(TARGET mc_sample_CoMGeneration) + # Test regression of the CoM trajectory generation using the mc_sample_CoMGeneration sample + mc_rtc_log_test( + TestRegressionCoMGeneration + "https://seafile.lirmm.fr/f/75870c0ca45f469591c1/?dl=1" + mc_sample_CoMGeneration + "/tmp/mc_rtc-test-CoMGenerator-latest.bin" + ) +endif() diff --git a/tests/test_hmc.cpp b/tests/test_hmc.cpp new file mode 100644 index 0000000000..c88d51f92d --- /dev/null +++ b/tests/test_hmc.cpp @@ -0,0 +1,285 @@ +/* + * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#include +#include +#include +#include + +#include +#include +#include + +template +bool allclose( + const Eigen::DenseBase & a, + const Eigen::DenseBase & b, + const typename DerivedA::RealScalar & rtol = Eigen::NumTraits::dummy_precision(), + const typename DerivedA::RealScalar & atol = Eigen::NumTraits::epsilon()) +{ + return ((a.derived() - b.derived()).array().abs() <= (atol + rtol * b.derived().array().abs())).all(); +} + +template +void testLookupTable(Fun f, double min, double max, size_t resolution, size_t nbTest, double precision = 1e-6) +{ + using namespace mc_planning; + LookupTable lookupTable(resolution, min, max, f); + + double oldrange = static_cast(nbTest); + double newrange = max - min; + for(size_t i = 0; i < nbTest; ++i) + { + double v = ((static_cast(i) * newrange) / oldrange) + min; + BOOST_REQUIRE_CLOSE(lookupTable(v), f(v), precision); + } +} + +BOOST_AUTO_TEST_CASE(TestLookupTable) +{ + + { + // Test cosh loopup table in a realistic constext. + // + // Creates a lookup table from the value of omega to + // both cosh(w * dt) and sinh(w * dt) + // + // The range is chosen to be similar to that encountered for a humanoid + // robot (from 0.001m to 3m height) + auto omega = [](double h) { return std::sqrt(mc_rtc::constants::GRAVITY / h); }; + double dt = 0.005; + testLookupTable([dt](double x) { return cosh(x * dt); }, omega(2.5), omega(0.01), 100000, 100000); + testLookupTable([dt](double x) { return sinh(x * dt); }, omega(2.5), omega(0.01), 100000, 100000); + } + + { + // Other tests for loopup table + testLookupTable([](double x) { return cosh(x); }, -2., 3., 20000, 500); + + // Test lookuptable for sinh + testLookupTable([](double x) { return sinh(x); }, 0., 3., 20000, 500); + testLookupTable([](double x) { return sinh(x); }, -1.5, 3., 20000, 500); + + testLookupTable([](double x) { return sqrt(x); }, 0., 1000., 2000, 500); + } +} + +BOOST_AUTO_TEST_CASE(TestPreviewSteps) +{ + using PreviewSteps = mc_planning::PreviewSteps; + PreviewSteps steps; + 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}}); + std::cout << "Steps:\n" << mc_rtc::io::to_string(steps.steps(), "\n") << std::endl; + + auto checkPrevious = [&](double t, double expected) { + std::cout << "Previous for t=" << t << ": " << steps.previous(t) << std::endl; + BOOST_REQUIRE_CLOSE(steps.previous(t).t(), expected, 1e-10); + }; + + checkPrevious(-0.5, 0.0); + checkPrevious(0, 0.0); + checkPrevious(1.6, 1.6); + checkPrevious(1.8, 1.6); + checkPrevious(2.9, 2.9); + checkPrevious(2.95, 2.9); + checkPrevious(3.0, 3.0); + checkPrevious(3.5, 3.0); + + auto checkNext = [&](double t, double expected) { + std::cout << "Next for t=" << t << ": " << steps.next(t) << std::endl; + BOOST_REQUIRE_CLOSE(steps.next(t).t(), expected, 1e-10); + }; + + checkNext(-0.5, 0); + checkNext(0.0, 1.5); + checkNext(0.5, 1.5); + checkNext(1.6, 2.9); + checkNext(1.8, 2.9); + checkNext(2.9, 3.0); + checkNext(2.95, 3.0); + checkNext(3.0, 3.0); + checkNext(3.5, 3.0); + + // Test replacing steps after a given value + { + auto steps2 = steps; + PreviewSteps::PreviewStepsSet newSteps; + newSteps.insert({1.6, {0.0, 11.6}}); + newSteps.insert({2.5, {0.0, 12.5}}); + newSteps.insert({3.0, {0.0, 13.0}}); + steps2.replaceAfter(newSteps); + std::cout << "New Steps:\n" << mc_rtc::io::to_string(steps2.steps(), "\n") << std::endl; + BOOST_REQUIRE(steps2.steps().size() == 5); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 0)).step(), Eigen::Vector2d{0.0, 0.0})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 1)).step(), Eigen::Vector2d{0.0, 1.5})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 2)).step(), Eigen::Vector2d{0.0, 11.6})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 3)).step(), Eigen::Vector2d{0.0, 12.5})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 4)).step(), Eigen::Vector2d{0.0, 13.0})); + } + + { + auto steps2 = steps; + PreviewSteps::PreviewStepsSet newSteps; + newSteps.insert({3.5, {0.0, 13.5}}); + steps2.replaceAfter(newSteps); + std::cout << "New Steps:\n" << mc_rtc::io::to_string(steps2.steps(), "\n") << std::endl; + BOOST_REQUIRE(steps2.steps().size() == steps.steps().size() + 1); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 0)).step(), Eigen::Vector2d{0.0, 0.0})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 1)).step(), Eigen::Vector2d{0.0, 1.5})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 2)).step(), Eigen::Vector2d{0.0, 1.6})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 3)).step(), Eigen::Vector2d{0.0, 2.9})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 4)).step(), Eigen::Vector2d{0.0, 3.0})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 5)).step(), Eigen::Vector2d{0.0, 13.5})); + } +} + +BOOST_AUTO_TEST_CASE(TestPreviewStepsSequential) +{ + using PreviewSteps = mc_planning::PreviewSteps; + PreviewSteps steps; + 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.initialize(); + PreviewSteps stepsSeq = steps; + stepsSeq.initialize(); + std::cout << "Steps:\n" << mc_rtc::io::to_string(steps.steps(), "\n") << std::endl; + + double dt = 0.005; + unsigned n_loop = (unsigned)std::lround(5. / dt); + unsigned n_preview = (unsigned)std::lround(1.6 / dt); + for(unsigned loop = 0; loop <= n_loop; loop++) + { + auto previewStart = loop * dt; + // mc_rtc::log::success("Start of new preview window at time: {:.3f}", previewStart); + stepsSeq.nextWindow(previewStart); + + for(unsigned previewWindow = loop; previewWindow < loop + 2 * n_preview + 1; ++previewWindow) + { + double t = previewWindow * dt; + // mc_rtc::log::info("t: {}", t); + stepsSeq.update(t); + + if(t >= steps.back().t()) + { + BOOST_REQUIRE(stepsSeq.isLastStep()); + } + else + { + BOOST_REQUIRE(!stepsSeq.isLastStep()); + } + + if(!stepsSeq.isLastStep()) + { + // mc_rtc::log::info("t: {}", t); + // mc_rtc::log::info("Steps:\n {}", mc_rtc::io::to_string(steps.steps(), "\n")); + // mc_rtc::log::info("prev(t): {:.3f}, prev(sew): {:.3f}", steps.previous(t).t(), stepsSeq.previous().t()); + BOOST_REQUIRE_CLOSE(steps.previous(t).t(), stepsSeq.previous().t(), 1e-10); + BOOST_REQUIRE_CLOSE(steps.next(t).t(), stepsSeq.next().t(), 1e-10); + } + } + } +} + +BOOST_AUTO_TEST_CASE(TestPreviewWindow) +{ + using namespace mc_planning; + + { + CenteredPreviewWindow window(0.015, 0.005); + { + auto view = window.all(Index(0)); // View on the preview window starting at time 0 + BOOST_REQUIRE_EQUAL(window.duration(), 0.03); + BOOST_REQUIRE_EQUAL(window.halfDuration(), 0.015); + BOOST_REQUIRE_EQUAL(window.halfSize(), 3); // n + BOOST_REQUIRE_EQUAL(window.size(), 7); // 2*n+1 + BOOST_REQUIRE_EQUAL(window.last(), 6); // 2*n+1 + BOOST_REQUIRE_EQUAL(view.startIndex(), 0); + BOOST_REQUIRE_EQUAL(view.startTime(), 0); + BOOST_REQUIRE_EQUAL(view.endIndex(), 6); // Array of size 7, last index is 6 + BOOST_REQUIRE_EQUAL(view.endTime(), 0.03); + BOOST_REQUIRE_EQUAL(view.nowIndex(), 3); + BOOST_REQUIRE_EQUAL(view.nowTime(), 0.015); + auto end = *view.end(); + BOOST_REQUIRE_EQUAL(end.localIndex(), 7); // Iterator end is one element after the end + BOOST_REQUIRE_EQUAL(end.localTime(), 0.035); // Iterator end is one element after the end + BOOST_REQUIRE_EQUAL(end.index(), 7); // Iterator end is one element after the end + BOOST_REQUIRE_EQUAL(end.time(), 0.035); // Iterator end is one element after the end + auto start = *view.begin(); + BOOST_REQUIRE_EQUAL(start.localIndex(), 0); + BOOST_REQUIRE_EQUAL(start.localTime(), 0.0); + for(const auto w : view) + { + mc_rtc::log::info("index {}, local time {:.3f}, global index {}, global time {:.3f}", w.localIndex(), + w.localTime(), w.index(), w.time()); + + BOOST_REQUIRE(w.localIndex() <= 6); + BOOST_REQUIRE(w.index() <= 6); + } + } + { + auto past = window.past(Time(10)); + auto pastInclusive = window.pastInclusive(Time(10)); + auto future = window.future(Time(10)); + auto futureInclusive = window.futureInclusive(Time(10)); + BOOST_REQUIRE_EQUAL(past.startTime(), 10); + BOOST_REQUIRE_EQUAL(pastInclusive.startTime(), 10); + BOOST_REQUIRE_EQUAL(past.endTime(), 10.01); + BOOST_REQUIRE_EQUAL(pastInclusive.endTime(), 10.015); + BOOST_REQUIRE_EQUAL(past.nowTime(), 10.015); + BOOST_REQUIRE_EQUAL(pastInclusive.nowTime(), 10.015); + BOOST_REQUIRE_EQUAL(future.startTime(), 10.020); + BOOST_REQUIRE_EQUAL(futureInclusive.startTime(), 10.015); + BOOST_REQUIRE_EQUAL(future.endTime(), 10.03); + BOOST_REQUIRE_EQUAL(futureInclusive.endTime(), 10.03); + BOOST_REQUIRE_EQUAL(future.nowTime(), 10.015); + BOOST_REQUIRE_EQUAL(futureInclusive.nowTime(), 10.015); + } + + { // Test a view starting at time 10 + auto view10 = window.all(Time(10)); + BOOST_REQUIRE_EQUAL(view10.startTime(), 10); + BOOST_REQUIRE_EQUAL(view10.endTime(), 10.03); + auto end = *view10.end(); + BOOST_REQUIRE_EQUAL(end.localIndex(), 7); // Iterator end is one element after the end + BOOST_REQUIRE_EQUAL(end.localTime(), 0.035); // Iterator end is one element after the end + auto start = *view10.begin(); + BOOST_REQUIRE_EQUAL(start.localIndex(), 0); + BOOST_REQUIRE_EQUAL(start.localTime(), 0.0); + for(const auto w : view10) + { + mc_rtc::log::info("local index {}, local time {:.3f}, global index {}, global time {:.3f}", w.localIndex(), + w.localTime(), w.index(), w.time()); + + BOOST_REQUIRE(w.localIndex() <= 6); + } + } + } + + auto test = [&](double preview_time, double dt, double start) { + CenteredPreviewWindow window(preview_time, dt); + unsigned i = window.index(Time(start)); + BOOST_REQUIRE_EQUAL(i, std::lround(start / dt)); + for(const auto & w : window.all(Time(start))) + { + // mc_rtc::log::info("Window element with index: {}, time: {:.3f}", w.localIndex(), w.localTime()); + BOOST_REQUIRE_EQUAL(w.index(), i); + BOOST_REQUIRE_EQUAL(w.time(), i * dt); + BOOST_REQUIRE_EQUAL(window.index(window.time(w.index())), w.index()); + BOOST_REQUIRE_EQUAL(window.time(window.index(w.time())), w.time()); + ++i; + } + }; + + test(1.6, 0.005, 0); + test(1.6, 0.005, 1.5); + test(1.6, 0.002, 0); + test(1.6, 0.002, 1.5); +} diff --git a/tests/test_io_utils.cpp b/tests/test_io_utils.cpp index d6f6b3e99f..b46890d90f 100644 --- a/tests/test_io_utils.cpp +++ b/tests/test_io_utils.cpp @@ -51,3 +51,14 @@ BOOST_AUTO_TEST_CASE(TestIOUtils) BOOST_CHECK(to_string(std::vector{"a", "aa", "aaa"}, lambda, " + ") == "1 + 2 + 3"); } + +BOOST_AUTO_TEST_CASE(TestIOUtilsEigen) +{ + using namespace mc_rtc::io; + // Trajectory of size 2*n_preview+1 + std::vector steps; + steps.push_back(Eigen::Vector3d::Random()); + steps.push_back(Eigen::Vector3d::Random()); + BOOST_REQUIRE(!to_string(steps, [](const Eigen::Vector3d & v) -> Eigen::RowVector3d { return v; }, "\n").empty()); + BOOST_REQUIRE(!to_string(steps).empty()); +} diff --git a/utils/CMakeLists.txt b/utils/CMakeLists.txt index 4089a1f2fc..9949b5bd10 100644 --- a/utils/CMakeLists.txt +++ b/utils/CMakeLists.txt @@ -25,6 +25,9 @@ add_mc_rtc_utils(mc_old_bin_to_flat mc_old_bin_to_flat.cpp) add_mc_rtc_utils(mc_json_to_yaml mc_json_to_yaml.cpp) +add_mc_rtc_utils(mc_bin_compare mc_bin_compare.cpp) +target_link_libraries(mc_bin_compare PUBLIC Boost::program_options Boost::disable_autolinking) + configure_file(mc_bin_utils.in.cpp "${CMAKE_CURRENT_BINARY_DIR}/mc_bin_utils.cpp") set(mc_bin_utils_SRC "${CMAKE_CURRENT_BINARY_DIR}/mc_bin_utils.cpp" diff --git a/utils/mc_bin_compare.cpp b/utils/mc_bin_compare.cpp new file mode 100644 index 0000000000..4909a74ca9 --- /dev/null +++ b/utils/mc_bin_compare.cpp @@ -0,0 +1,284 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +namespace bfs = boost::filesystem; +#include +namespace po = boost::program_options; + +void usage(char * name) +{ + std::cerr << name << " log1.bin log2.bin [entry1 entry2 ...]\n"; +} + +template +bool allclose( + const Eigen::DenseBase & a, + const Eigen::DenseBase & b, + const typename DerivedA::RealScalar & rtol = Eigen::NumTraits::dummy_precision(), + const typename DerivedA::RealScalar & atol = Eigen::NumTraits::epsilon()) +{ + return ((a.derived() - b.derived()).array().abs() <= (atol + rtol * b.derived().array().abs())).all(); +} + +template< + typename T, + typename std::enable_if::value && !mc_rtc::internal::is_eigen_matrix::value + && !std::is_same::value && !std::is_same::value, + int>::type = 0> +bool compare_elem(const T &, const T &, double) +{ + mc_rtc::log::error_and_throw("Comparison for type {} hasn't been implemented", + mc_rtc::type_name()); +} + +template::value && !mc_rtc::internal::is_eigen_matrix::value, + int>::type = 0> +bool compare_elem(const T & e1, const T & e2, double tolerance) +{ + return std::fabs(e1 - e2) < tolerance; +} + +template::value && !std::is_arithmetic::value, + int>::type = 0> +bool compare_elem(const T & e1, const T & e2, double tolerance) +{ + return allclose(e1, e2, tolerance, tolerance); +} + +template<> +bool compare_elem(const Eigen::Quaterniond & e1, const Eigen::Quaterniond & e2, double tolerance) +{ + return allclose(e1.matrix(), e2.matrix(), tolerance, tolerance); +} + +template<> +bool compare_elem(const bool & e1, const bool & e2, double) +{ + return e1 == e2; +} + +template<> +bool compare_elem(const std::string & e1, const std::string & e2, double) +{ + return e1 == e2; +} + +template::value || std::is_same::value, + int>::type = 0> +bool compare_elem(const T & e1, const T & e2, double tolerance) +{ + return allclose(e1.vector(), e2.vector(), tolerance, tolerance); +} + +template<> +bool compare_elem(const sva::PTransformd & e1, const sva::PTransformd & e2, double tolerance) +{ + return sva::transformError(e1, e2).vector().norm() < 6 * tolerance; +} + +template<> +bool compare_elem(const std::vector & e1, const std::vector & e2, double tolerance) +{ + if(e1.size() != e2.size()) + { + return false; + } + for(size_t i = 0; i < e1.size(); ++i) + { + if(!compare_elem(e1[i], e2[i], tolerance)) + { + return false; + } + } + return true; +} + +template +bool compare(const std::string & entry, + const std::vector & v1, + const std::vector & v2, + double tolerance = 1e-10) +{ + if(v1.size() != v2.size()) + { + mc_rtc::log::error("Entry {} requires the same number of elements", entry); + return false; + } + bool res = true; + for(size_t i = 0; i < v1.size(); ++i) + { + const T * elem1 = v1[i]; + const T * elem2 = v2[i]; + if(elem1 == nullptr || elem2 == nullptr) + { + res = res && (elem1 == nullptr && elem2 == nullptr); + } + else + { + bool equal = compare_elem(*elem1, *elem2, tolerance); + if(!equal) + { + // TODO implement fmt::formatter for all types including eigen to + // display the element value generically + // mc_rtc::log::info("Comparation failed for entry {} at element {}: {} != {} (tolerance {})", entry, i, *elem1, + // *elem2, tolerance); + mc_rtc::log::info("Comparaison failed for entry {} at index {} (tolerance {})", entry, i, tolerance); + } + res = res && equal; + } + if(res == false) return false; + } + return res; +} + +bool compare_entry(const mc_rtc::log::FlatLog & log1, const mc_rtc::log::FlatLog & log2, const std::string & entry) +{ + if(!log1.has(entry) || !log2.has(entry)) + { + mc_rtc::log::error("Entry {} must exist in both log files", entry); + return false; + } + auto types1 = log1.type(entry); + auto types2 = log2.type(entry); + if(types1 != types2) + { + mc_rtc::log::error("Types for entry {} must match", entry); + return false; + } + + bool res = false; +#define COMPARE(LOGT) \ + case mc_rtc::log::LogType::LOGT: \ + { \ + using type = mc_rtc::log::GetType::type; \ + res = compare(entry, log1.getRaw(entry), log2.getRaw(entry)); \ + break; \ + } + switch(types1) + { + COMPARE(Quaterniond) + COMPARE(Vector2d) + COMPARE(Vector3d) + COMPARE(Vector6d) + COMPARE(VectorXd) + COMPARE(Int8_t) + COMPARE(Int16_t) + COMPARE(Int32_t) + COMPARE(Int64_t) + COMPARE(Uint8_t) + COMPARE(Uint16_t) + COMPARE(Uint32_t) + COMPARE(Uint64_t) + COMPARE(Float) + COMPARE(Double) + COMPARE(Bool) + COMPARE(String) + COMPARE(MotionVecd) + COMPARE(ForceVecd) + COMPARE(PTransformd) + COMPARE(VectorDouble) + case mc_rtc::log::LogType::None: + default: + mc_rtc::log::error("Comparison of unsupported type {} requested for entry {}", mc_rtc::log::LogTypeName(types1), + entry); + res = false; + break; + } +#undef COMPARE + + if(res == false) + { + mc_rtc::log::error("Comparison for entry {} failed", entry); + } + return res; +} + +int main(int argc, char * argv[]) +{ + po::variables_map vm; + po::options_description tool("mc_bin_compare"); + // clang-format off + tool.add_options() + ("help", "Produce this message") + ("log1", po::value(), "First log file") + ("log2", po::value(), "Second log file") + ("entries", po::value>(), "Entries") + ("tolerance", po::value()->default_value(1e-10), "Tolerance threshold"); + // clang-format on + po::positional_options_description pos; + pos.add("log1", 1); + pos.add("log2", 1); + pos.add("entries", -1); + po::store(po::command_line_parser(argc, argv).options(tool).positional(pos).run(), vm); + po::notify(vm); + if(vm.count("help") || !vm.count("log1") || !vm.count("log2")) + { + std::cout << "Usage: mc_bin_compare [options] log1 log2 [entries...]\n\n"; + std::cout << tool << "\n"; + return 1; + } + std::vector entries; + if(vm.count("entries")) + { + entries = vm["entries"].as>(); + } + auto file1 = vm["log1"].as(); + auto file2 = vm["log2"].as(); + auto tolerance = vm["tolerance"].as(); + + if(!bfs::exists(file1)) + { + mc_rtc::log::error("File {} does not exist", file1); + return 1; + } + if(!bfs::exists(file2)) + { + mc_rtc::log::error("File {} does not exist", file2); + return 1; + } + mc_rtc::log::FlatLog log1(file1); + mc_rtc::log::FlatLog log2(file2); + if(entries.empty()) + { + mc_rtc::log::info("No entry specified, assuming comparison between all entries"); + for(const auto & entry : log1.entries()) + { + entries.push_back(entry); + } + } + mc_rtc::log::info("Comparing:\n" + "- Logs : {} and {} with:\n" + "- Tolerance: {}\n" + "- Entries : {}", + file1, file2, tolerance, mc_rtc::io::to_string(entries)); + + bool success = true; + for(const auto & entry : entries) + { + success = success && compare_entry(log1, log2, entry); + } + + if(success) + { + mc_rtc::log::success("Comparison successful"); + return 0; + } + else + { + mc_rtc::log::error("Comparison failed: one or more log entries differ"); + return 1; + } +} diff --git a/utils/mc_log_gui/mc_log_ui/mc_log_ui.py b/utils/mc_log_gui/mc_log_ui/mc_log_ui.py index 4221964ee7..5a20b8bde2 100644 --- a/utils/mc_log_gui/mc_log_ui/mc_log_ui.py +++ b/utils/mc_log_gui/mc_log_ui/mc_log_ui.py @@ -1034,13 +1034,33 @@ def load_csv(self, fpath, clear = True): self.loaded_files = [] data = read_log(fpath) if not 't' in data: - print("This GUI assumes a time-entry named t is available in the log, failed loading {}".format(fpath)) - return + error = "This GUI assumes a time-entry named t is available in the log, failed loading {}".format(fpath) + if self.isVisible(): + err_diag = QtWidgets.QMessageBox(self) + err_diag.setModal(True) + err_diag.setText(error) + err_diag.exec_() + return + else: + print(error) + self.close() + exit(1) if 't' in self.data and not clear: dt = self.data['t'][1] - self.data['t'][0] ndt = data['t'][1] - data['t'][0] if abs(dt - ndt) > 1e-9: - print("This GUI assumes you are comparing logs with a similar timestep, already loaded dt = {} but attempted to load dt = {} from {}", dt, ndt, fpath) + print() + error = "This GUI assumes you are comparing logs with a similar timestep, already loaded dt = {} but attempted to load dt = {} from {}".format(dt, ndt, fpath) + if self.isVisible(): + err_diag = QtWidgets.QMessageBox(self) + err_diag.setModal(True) + err_diag.setText(error) + err_diag.exec_() + return + else: + print(error) + self.close() + exit(1) return pad_left = int(round((self.data['t'][0] - data['t'][0]) / dt)) pad_right = int(round((data['t'][-1] - self.data['t'][-1]) / dt))