Implement the time-varying dcm planner

.github/workflows/ci.yml

@@ -9,12 +9,13 @@ on:
  - cron: '0 2 * * *'
 
 env:
- vcpkg_robotology_TAG: v0.0.3
+ vcpkg_robotology_TAG: v0.4.0
  YCM_TAG: v0.11.0
  YARP_TAG: 964bb26fa4791d83d72882711ea7509306248106
  iDynTree_TAG: v1.1.0
  Catch2_TAG: v2.11.3
  Qhull_TAG: v8.0.0
+ CasADi_TAG: 3.5.1
 
 jobs:
  build:
@@ -53,38 +54,37 @@ jobs:
  # To avoid problems with non-relocatable packages, we unzip the archive exactly in the same C:/robotology/vcpkg
  # that has been used to create the pre-compiled archive
  cd C:/
- md C:/robotology
- md C:/robotology/vcpkg
  wget https://github.com/robotology/robotology-superbuild-dependencies-vcpkg/releases/download/${env:vcpkg_robotology_TAG}/vcpkg-robotology.zip
- unzip vcpkg-robotology.zip -d C:/robotology/vcpkg
+ unzip vcpkg-robotology.zip -d C:/
+ rm vcpkg-robotology.zip
  # Overwrite the VCPKG_INSTALLATION_ROOT env variable defined by GitHub Actions to point to our vcpkg
  echo "::set-env name=VCPKG_INSTALLATION_ROOT::C:/robotology/vcpkg"
 
  # Install Catch2
  cd C:/robotology/vcpkg
- ./vcpkg.exe install --triplet x64-windows catch2
+ ./vcpkg.exe install --overlay-ports=C:/robotology/robotology-vcpkg-binary-ports catch2:x64-windows
 
  - name: Dependencies [macOS]
  if: matrix.os == 'macOS-latest'
  run: |
  brew update
- brew install ace boost eigen swig qt5 orocos-kdl catch2 qhull
+ brew install ace boost eigen swig qt5 orocos-kdl catch2 qhull ipopt pkg-config
 
  - name: Dependencies [Ubuntu]
  if: matrix.os == 'ubuntu-latest'
  run: |
  sudo apt-get update
  sudo apt-get install git build-essential cmake libace-dev coinor-libipopt-dev libboost-system-dev libboost-filesystem-dev \
  libboost-thread-dev liborocos-kdl-dev libeigen3-dev swig qtbase5-dev qtdeclarative5-dev qtmultimedia5-dev \
- libxml2-dev liburdfdom-dev libtinyxml-dev liburdfdom-dev liboctave-dev python-dev valgrind
+ libxml2-dev liburdfdom-dev libtinyxml-dev liburdfdom-dev liboctave-dev python-dev valgrind coinor-libipopt-dev
 
  - name: Cache Source-based Dependencies
  id: cache-source-deps
  uses: actions/cache@v1
  with:
  path: ${{ github.workspace }}/install/deps
  # Including ${{ runner.temp }} is a workaround taken from https://github.com/robotology/whole-body-estimators/pull/62 to fix macos configuration failure on https://github.com/dic-iit/bipedal-locomotion-framework/pull/45
- key: source-deps-${{ runner.os }}-${{runner.temp}}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-ycm-${{ env.YCM_TAG }}-yarp-${{ env.YARP_TAG }}-iDynTree-${{ env.iDynTree_TAG }}-catch2-${{ env.Catch2_TAG }}-qhull-${{ env.Qhull_TAG }}
+ key: source-deps-${{ runner.os }}-${{runner.temp}}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-ycm-${{ env.YCM_TAG }}-yarp-${{ env.YARP_TAG }}-iDynTree-${{ env.iDynTree_TAG }}-catch2-${{ env.Catch2_TAG }}-qhull-${{ env.Qhull_TAG }}-casADi-${{ env.CasADi_TAG }}
 
 
  - name: Source-based Dependencies [Windows]
@@ -143,6 +143,27 @@ jobs:
  -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
  cmake --build . --config ${{ matrix.build_type }} --target install
 
+ # CasADi
+ # We override the casadi installation structure to be compliant with the folder structure used for
+ # the other dependencies. Please check:
+ # https://github.com/casadi/casadi/blob/6f122ca22e2a869903628c5738f154c8ac0f7455/CMakeLists.txt#L317
+ cd ${GITHUB_WORKSPACE}
+ git clone https://github.com/casadi/casadi.git -b ${CasADi_TAG} casadi
+ cd casadi
+ mkdir build
+ cd build
+ cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \
+ -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \
+ -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \
+ -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps \
+ -DINCLUDE_PREFIX:PATH="include" \
+ -DCMAKE_PREFIX:PATH="lib/cmake/casadi" \
+ -DLIB_PREFIX:PATH="lib" \
+ -DBIN_PREFIX:PATH="bin" \
+ -DWITH_IPOPT=BOOL:ON ..
+ cmake --build . --config ${{ matrix.build_type }} --target install
+
+
  - name: Source-based Dependencies [Ubuntu/macOS]
  if: steps.cache-source-deps.outputs.cache-hit != 'true' && (matrix.os == 'ubuntu-latest' || matrix.os == 'macOS-latest')
  shell: bash
@@ -177,6 +198,16 @@ jobs:
  -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
  cmake --build . --config ${{ matrix.build_type }} --target install
 
+ # CasADi
+ cd ${GITHUB_WORKSPACE}
+ git clone https://github.com/casadi/casadi.git -b ${CasADi_TAG} casadi
+ cd casadi
+ mkdir build
+ cd build
+ cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps -DWITH_IPOPT=BOOL:ON ..
+ cmake --build . --config ${{ matrix.build_type }} --target install
+
+
  - name: Source-based Dependencies [Ubuntu]
  if: steps.cache-source-deps.outputs.cache-hit != 'true' && matrix.os == 'ubuntu-latest'
  shell: bash
@@ -204,7 +235,6 @@ jobs:
  -DCMAKE_POSITION_INDEPENDENT_CODE=ON ..
  cmake --build . --config ${{ matrix.build_type }} --target install
 
-
  # ===================
  # CMAKE-BASED PROJECT
  # ===================

cmake/AddBipedalLocomotionUnitTest.cmake

@@ -25,6 +25,8 @@ if (FRAMEWORK_RUN_Valgrind_tests)
  set(MEMORYCHECK_COMMAND ${VALGRIND_PROGRAM})
  if (APPLE)
  set(MEMORYCHECK_SUPPRESSIONS "--suppressions=${PROJECT_SOURCE_DIR}/cmake/valgrind-macos.supp")
+ elseif (UNIX AND NOT APPLE)
+ set(MEMORYCHECK_SUPPRESSIONS "--suppressions=${PROJECT_SOURCE_DIR}/cmake/valgrind-linux.supp")
  else ()
  set(MEMORYCHECK_SUPPRESSIONS "")
  endif ()

cmake/BipedalLocomotionFrameworkFindDependencies.cmake

@@ -141,6 +141,9 @@ checkandset_dependency(YARP)
 find_package(Qhull 8.0.0 QUIET)
 checkandset_dependency(Qhull)
 
+find_package(casadi QUIET)
+checkandset_dependency(casadi)
+
 framework_dependent_option(FRAMEWORK_COMPILE_YarpUtilities
  "Compile YarpHelper library?" ON
  "FRAMEWORK_USE_YARP" OFF)
@@ -155,7 +158,7 @@ framework_dependent_option(FRAMEWORK_COMPILE_Estimators
 
 framework_dependent_option(FRAMEWORK_COMPILE_Planners
  "Compile Planners libraries?" ON
- "FRAMEWORK_USE_Qhull" OFF)
+ "FRAMEWORK_USE_Qhull;FRAMEWORK_USE_casadi" OFF)
 
 framework_dependent_option(FRAMEWORK_COMPILE_ContactModels
  "Compile ContactModels library?" ON

cmake/valgrind-linux.supp

@@ -0,0 +1,18 @@
+{
+ <ubuntu-18.04-casadi-ipopt>
+ Memcheck:Leak
+ match-leak-kinds: definite
+ fun:malloc
+ fun:get_num_procs
+ fun:blas_get_cpu_number
+ fun:gotoblas_init
+ fun:call_init
+ fun:_dl_init
+ fun:dl_open_worker
+ fun:_dl_catch_exception
+ fun:_dl_open
+ fun:dlopen_doit
+ fun:_dl_catch_exception
+ fun:_dl_catch_error
+ fun:_dlerror_run
+}

src/Planners/CMakeLists.txt

@@ -16,10 +16,10 @@ if (FRAMEWORK_COMPILE_Planners)
 
  add_bipedal_locomotion_library(
  NAME Planners
- PUBLIC_HEADERS ${H_PREFIX}/ConvexHullHelper.h
- SOURCES src/ConvexHullHelper.cpp
- PUBLIC_LINK_LIBRARIES Eigen3::Eigen
- PRIVATE_LINK_LIBRARIES Qhull::qhullcpp Qhull::qhullstatic_r
+ PUBLIC_HEADERS ${H_PREFIX}/ConvexHullHelper.h ${H_PREFIX}/DCMPlanner.h ${H_PREFIX}/TimeVaryingDCMPlanner.h
+ SOURCES src/ConvexHullHelper.cpp src/DCMPlanner.cpp src/TimeVaryingDCMPlanner.cpp
+ PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System BipedalLocomotion::Contact
+ PRIVATE_LINK_LIBRARIES Qhull::qhullcpp Qhull::qhullstatic_r casadi
  INSTALLATION_FOLDER Planners)
 
  add_subdirectory(tests)

src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h

@@ -0,0 +1,88 @@
+/**
+ * @file DCMPlanner.h
+ * @authors Giulio Romualdi
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_PLANNERS_DCM_PLANNER_H
+#define BIPEDAL_LOCOMOTION_PLANNERS_DCM_PLANNER_H
+
+#include <memory>
+
+#include <Eigen/Dense>
+
+#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
+#include <BipedalLocomotion/Planners/ContactPhaseList.h>
+#include <BipedalLocomotion/System/Advanceable.h>
+
+namespace BipedalLocomotion
+{
+namespace Planners
+{
+
+/**
+ * DCMPlannerState describes the state of the Divergent Component of Motion (DCM) planner.
+ */
+struct DCMPlannerState
+{
+ Eigen::Vector3d dcmPosition; /**< Position of the DCM expressed w.r.t. the inertial frame */
+ Eigen::Vector3d dcmVelocity; /**< Velocity of the DCM expressed w.r.t. the inertial frame */
+ Eigen::Vector3d vrpPosition; /**< Position of the virtual repellent point (VRP) expressed
+ w.r.t. the inertial frame */
+ double omega;/**< Value of the parameter omega */
+};
+
+/**
+ * DCMPlanner defines a trajectory generator for the variable height Divergent
+ * component of motion (DCM). Please inherit publicly from this class in order to define your
+ * planner.
+ */
+class DCMPlanner : public BipedalLocomotion::System::Advanceable<DCMPlannerState>
+{
+protected:
+ std::shared_ptr<const ContactPhaseList> m_contactPhaseList; /**< Pointer containing the contact
+ phases. */
+
+ DCMPlannerState m_initialState; /**< Initial state of the planner */
+
+public:
+ /**
+ * Initialize the planner.
+ * @param handler pointer to the parameter handler.
+ * @return true in case of success/false otherwise.
+ */
+ virtual bool initialize(std::shared_ptr<ParametersHandler::IParametersHandler> handler);
+
+ /**
+ * Set the initial state of the planner
+ * @param initialState the initial state of the planner
+ */
+ void setInitialState(const DCMPlannerState& initialState);
+
+ /**
+ * Set the contact phase list
+ * @note the contactPhaseList pointer should point to an already initialized ContactPhaseList.
+ * Please be sure that the memory pointed is reachable for the entire life time of the
+ * DCMPlanner class.
+ * @param contactPhaseList pointer containing the list of the contact phases
+ * @return true in case of success, false otherwise.
+ */
+ bool setContactPhaseList(std::weak_ptr<const ContactPhaseList> contactPhaseList);
+
+ /**
+ * Compute the DCM trajectory.
+ * @warning Please implement the function in your custom planner.
+ * @return true in case of success, false otherwise.
+ */
+ virtual bool computeTrajectory() = 0;
+
+ /**
+ * Destructor.
+ */
+ virtual ~DCMPlanner() = default;
+};
+} // namespace Planners
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_PLANNERS_DCM_PLANNER_H

src/Planners/include/BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h

@@ -0,0 +1,79 @@
+/**
+ * @file TimeVaryingDCMPlanner.h
+ * @authors Giulio Romualdi
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_PLANNERS_TIME_VARYING_DCM_PLANNER_H
+#define BIPEDAL_LOCOMOTION_PLANNERS_TIME_VARYING_DCM_PLANNER_H
+
+#include <memory>
+
+#include <BipedalLocomotion/Planners/DCMPlanner.h>
+
+namespace BipedalLocomotion
+{
+namespace Planners
+{
+
+/**
+ * DCMPlanner defines a trajectory generator for the variable height Divergent component of motion
+ * (DCM).
+ */
+class TimeVaryingDCMPlanner : public DCMPlanner
+{
+ /**
+ * Private implementation
+ */
+ struct Impl;
+
+ std::unique_ptr<Impl> m_pimpl; /**< Pointer to private implementation */
+
+public:
+ /**
+ * Constructor.
+ */
+ TimeVaryingDCMPlanner();
+
+ /**
+ * Destructor.
+ */
+ ~TimeVaryingDCMPlanner();
+
+ /**
+ * Initialize the planner.
+ * @param handler pointer to the parameter handler.
+ * @return true in case of success/false otherwise.
+ */
+ bool initialize(std::shared_ptr<ParametersHandler::IParametersHandler> handler) override;
+
+ /**
+ * Compute the DCM trajectory.
+ * @return true in case of success, false otherwise.
+ */
+ bool computeTrajectory() final;
+
+ /**
+ * @brief Get the object.
+ * @return a const reference of the requested object.
+ */
+ const DCMPlannerState& get() const final;
+
+ /**
+ * @brief Determines the validity of the object retrieved with get()
+ * @return True if the object is valid, false otherwise.
+ */
+ bool isValid() const final;
+
+ /**
+ * @brief Advance the internal state. This may change the value retrievable from get().
+ * @return True if the advance is successfull.
+ */
+ bool advance() final;
+
+};
+} // namespace Planners
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_PLANNERS_TIME_VARYING_DCM_PLANNER_H

src/Planners/src/DCMPlanner.cpp

@@ -0,0 +1,34 @@
+/**
+ * @file DCMPlanner.cpp
+ * @authors Giulio Romualdi
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#include <BipedalLocomotion/Planners/DCMPlanner.h>
+
+using namespace BipedalLocomotion::Planners;
+
+bool DCMPlanner::initialize(std::shared_ptr<ParametersHandler::IParametersHandler> handler)
+{
+ return true;
+}
+
+bool DCMPlanner::setContactPhaseList(std::weak_ptr<const ContactPhaseList> contactPhaseList)
+{
+ if(contactPhaseList.expired())
+ {
+ std::cerr << "[DCMPlanner::setContactPhaseList] The contactPhaseList pointer is expired. "
+ "Please pass a valid pointer."
+ << std::endl;
+ return false;
+ }
+
+ m_contactPhaseList = contactPhaseList.lock();
+ return true;
+}
+
+void DCMPlanner::setInitialState(const DCMPlannerState& initialState)
+{
+ m_initialState = initialState;
+}