Merge pull request #387 from dic-iit/feature/CoMZMPController

Implement the CoMZMP controller

CHANGELOG.md

@@ -13,6 +13,7 @@ All notable changes to this project are documented in this file.
 - Implement python bindings for QPInverseKinematics class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/303)
 - Implement `ControlTask` in for System component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/373).
 - Allow changing the log verbosity (https://github.com/dic-iit/bipedal-locomotion-framework/pull/385)
+- Implement the CoMZMP controller (https://github.com/dic-iit/bipedal-locomotion-framework/pull/387)
 
 ### Changed
 - Add common Python files to gitignore (https://github.com/dic-iit/bipedal-locomotion-framework/pull/338)

cmake/BipedalLocomotionFrameworkDependencies.cmake

@@ -160,6 +160,10 @@ framework_dependent_option(FRAMEWORK_COMPILE_IK
  "Compile IK library?" ON
  "FRAMEWORK_COMPILE_System;FRAMEWORK_USE_LieGroupControllers;FRAMEWORK_COMPILE_ManifConversions;FRAMEWORK_USE_manif;FRAMEWORK_USE_OsqpEigen" OFF)
 
+framework_dependent_option(FRAMEWORK_COMPILE_SimplifiedModelControllers
+ "Compile SimplifiedModelControllers library?" ON
+ "FRAMEWORK_USE_manif" OFF)
+
 framework_dependent_option(FRAMEWORK_COMPILE_JointPositionTrackingApplication
  "Compile joint-position-tracking application?" ON
  "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_Planners;FRAMEWORK_COMPILE_RobotInterface" OFF)

src/CMakeLists.txt

@@ -19,3 +19,4 @@ add_subdirectory(Math)
 add_subdirectory(TSID)
 add_subdirectory(Perception)
 add_subdirectory(IK)
+add_subdirectory(SimplifiedModelControllers)

src/SimplifiedModelControllers/CMakeLists.txt

@@ -0,0 +1,16 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+if(FRAMEWORK_COMPILE_SimplifiedModelControllers)
+
+ set(H_PREFIX include/BipedalLocomotion/SimplifiedModelControllers)
+
+ add_bipedal_locomotion_library(
+ NAME SimplifiedModelControllers
+ PUBLIC_HEADERS ${H_PREFIX}/CoMZMPController.h
+ SOURCES src/CoMZMPController.cpp
+ PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler MANIF::manif BipedalLocomotion::System
+ PRIVATE_LINK_LIBRARIES BipedalLocomotion::TextLogging
+ SUBDIRECTORIES tests)
+endif()

src/SimplifiedModelControllers/include/BipedalLocomotion/SimplifiedModelControllers/CoMZMPController.h

@@ -0,0 +1,157 @@
+/**
+ * @file CoMZMPController.h
+ * @authors Giulio Romualdi
+ * @copyright 2021 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_SIMPLIFIED_MODEL_CONTROLLERS_COM_ZMP_CONTROLLER_H
+#define BIPEDAL_LOCOMOTION_SIMPLIFIED_MODEL_CONTROLLERS_COM_ZMP_CONTROLLER_H
+
+#include <memory>
+
+#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
+#include <BipedalLocomotion/System/Advanceable.h>
+
+#include <manif/SO2.h>
+
+namespace BipedalLocomotion
+{
+namespace SimplifiedModelControllers
+{
+
+struct CoMZMPControllerInput
+{
+ Eigen::Vector2d desiredCoMVelocity; /**< Desired CoM velocity */
+ Eigen::Vector2d desiredCoMPosition; /**< Desired CoM position */
+ Eigen::Vector2d desiredZMPPosition; /**< Desired ZMP position */
+
+ Eigen::Vector2d CoMPosition; /**< CoM position */
+ Eigen::Vector2d ZMPPosition; /**< ZMP position */
+
+ /** the yaw angle represents the I_R_B rotation matrix. The rotation brings a vector expressed
+ * in the frame rigidly attached to the CoM (B) to the inertial frame (I). The yaw angle allows
+ * the user to have different gains on the forward and lateral walking direction. */
+ double angle;
+};
+
+using CoMZMPControllerOutput = Eigen::Vector2d;
+
+/**
+ * CoMZMPController implements the following control law
+ * \f[
+ * \dot{x} = \dot{x}^* - K _ {zmp} (r^*_{zmp} - r_{zmp}) + K _ {com} (x^* - x)
+ * \f]
+ * where \f$x\f$ is the CoM position and \f$r_{zmp}\f$ the ZMP position.
+ * The gains \f$K_{zmp}\f$ and \f$K_{com}\f$ depends on the orientation of a frame rigidly attached
+ * to the CoM (B) w.r.t. the inertial frame (I).
+ * The rotation matrix \f${}^I R _ B\f$ depends only on the yaw angle.
+ * <img
+ * src="https://user-images.githubusercontent.com/16744101/128683430-f63c3c34-404a-42d9-b864-2437445abe38.png"
+ * alt="CoMZMPController">
+ * @note Since the controller is based on the assumption of the Linear Inverted Pendulum Model
+ * (LIPM), the CoM height velocity is always equal to zero.
+ * @note The yaw angle allows the user to have different gains on the forward and
+ * lateral walking direction.
+ * @note The design of the controller is taken from [Y. Choi, D. Kim, Y. Oh, and B.-j. J. You, _On
+ * the Walking Control for Humanoid Robot Based on Kinematic Resolution of CoM Jacobian With
+ * Embedded Motion_](https://doi.org/10.1109/ROBOT.2006.1642102).
+ */
+class CoMZMPController : public System::Advanceable<CoMZMPControllerInput, CoMZMPControllerOutput>
+{
+public:
+ /**
+ * Initialize the controller.
+ * @param handler pointer to the parameter handler.
+ * @note the following parameters are required by the class
+ * | Parameter Name | Type | Description | Mandatory |
+ * |:----------------:|:----------------:|:-----------------------------------------------------------------------------------------------------:|:---------:|
+ * | `com_gain` | `vector<double>` | 2D-vector containing the gains of the CoM written in a frame rigidly attached to the simplified model | Yes |
+ * | `zmp_gain` | `vector<double>` | 2D-vector containing the gains of the ZMP written in a frame rigidly attached to the simplified model | Yes |
+ * @return true in case of success/false otherwise.
+ */
+ bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) override;
+
+ /**
+ * Get the the controller output.
+ * @return The velocity of the CoM.
+ */
+ const Output& getOutput() const final;
+
+ /**
+ * Determines the validity of the object retrieved with getOutput()
+ * @return True if the object is valid, false otherwise.
+ */
+ bool isOutputValid() const final;
+
+ /**
+ * Compute the control law.
+ * @return True if the advance is successfull.
+ */
+ bool advance() final;
+
+ /**
+ * Set the input of the advanceable block.
+ * @param input the CoMZMPControllerInput struct
+ * @return true in case of success and false otherwise.
+ */
+ bool setInput(const Input& input) final;
+
+ /**
+ * Set the desired set-point.
+ * @param CoMVelocity a 2d-vector containing the x and y coordinate of the CoM velocity.
+ * @param CoMPosition a 2d-vector containing the x and y coordinate of the CoM position.
+ * @param ZMPPosition a 2d-vector containing the x and y coordinate of the ZMP position.
+ */
+ void setSetPoint(Eigen::Ref<const Eigen::Vector2d> CoMVelocity,
+ Eigen::Ref<const Eigen::Vector2d> CoMPosition,
+ Eigen::Ref<const Eigen::Vector2d> ZMPPosition);
+
+ /**
+ * Set the state feedback
+ * @param CoMPosition a 2d-vector containing the x and y coordinate of the CoM position.
+ * @param ZMPPosition a 2d-vector containing the x and y coordinate of the ZMP position.
+ * @param I_R_B rotation matrix that brings a vector expressed in the frame rigidly attached to
+ * the CoM (B) to the inertial frame (I).
+ * @note Since the controller is based on the LIPM assumption, the rotation matrix contains
+ * only a rotation along the z axis. The yaw angle allows the user to have different gains on
+ * the forward and lateral walking direction.
+ */
+ void setFeedback(Eigen::Ref<const Eigen::Vector2d> CoMPosition,
+ Eigen::Ref<const Eigen::Vector2d> ZMPPosition,
+ const manif::SO2d& I_R_B);
+
+ /**
+ * Set the state feedback
+ * @param CoMPosition a 2d-vector containing the x and y coordinate of the CoM position.
+ * @param ZMPPosition a 2d-vector containing the x and y coordinate of the ZMP position.
+ * @param angle the yaw angle (in radians) represents the \f${}^I R_B\f$ rotation matrix. The
+ * rotation brings a vector expressed in the frame rigidly attached to the CoM (B) to the
+ * inertial frame (I). The yaw angle allows the user to have different gains on the forward and
+ * lateral walking direction.
+ */
+ void setFeedback(Eigen::Ref<const Eigen::Vector2d> CoMPosition,
+ Eigen::Ref<const Eigen::Vector2d> ZMPPosition,
+ const double angle);
+
+ private:
+ manif::SO2d m_I_R_B{manif::SO2d::Identity()};
+ Eigen::Vector2d m_CoMGain{Eigen::Vector2d::Zero()};
+ Eigen::Vector2d m_ZMPGain{Eigen::Vector2d::Zero()};
+
+ Eigen::Vector2d m_controllerOutput{Eigen::Vector2d::Zero()};
+
+ Eigen::Vector2d m_desiredCoMVelocity{Eigen::Vector2d::Zero()};
+ Eigen::Vector2d m_CoMPosition{Eigen::Vector2d::Zero()};
+ Eigen::Vector2d m_desiredCoMPosition{Eigen::Vector2d::Zero()};
+ Eigen::Vector2d m_ZMPPosition{Eigen::Vector2d::Zero()};
+ Eigen::Vector2d m_desiredZMPPosition{Eigen::Vector2d::Zero()};
+
+ bool m_isOutputValid{false};
+ bool m_isInitalized{false};
+};
+
+} // namespace SimplifiedModelControllers
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_SIMPLIFIED_MODEL_CONTROLLERS_COM_ZMP_CONTROLLER_H

src/SimplifiedModelControllers/src/CoMZMPController.cpp

@@ -0,0 +1,119 @@
+/**
+ * @file CoMZMPController.cpp
+ * @authors Giulio Romualdi
+ * @copyright 2021 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/ParametersHandler/IParametersHandler.h>
+#include <BipedalLocomotion/SimplifiedModelControllers/CoMZMPController.h>
+#include <BipedalLocomotion/TextLogging/Logger.h>
+
+#include <memory>
+
+using namespace BipedalLocomotion::SimplifiedModelControllers;
+using namespace BipedalLocomotion;
+
+bool CoMZMPController::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler)
+{
+ constexpr auto logPrefix = "[CoMZMPController::initialize]";
+
+ m_isOutputValid = false;
+
+ auto ptr = handler.lock();
+ if (ptr == nullptr)
+ {
+ log()->error("{} The handler has to point to an already initialized IParametershandler.",
+ logPrefix);
+ return false;
+ }
+
+ bool ok = ptr->getParameter("com_gain", m_CoMGain);
+ ok = ok && ptr->getParameter("zmp_gain", m_ZMPGain);
+
+ if (!ok)
+ {
+ log()->error("{} Unable to load the controller gains.", logPrefix);
+ return false;
+ }
+
+ m_isInitalized = true;
+
+ return true;
+}
+
+const CoMZMPController::Output& CoMZMPController::getOutput() const
+{
+ return m_controllerOutput;
+}
+
+bool CoMZMPController::isOutputValid() const
+{
+ return m_isOutputValid;
+}
+
+bool CoMZMPController::advance()
+{
+ if (!m_isInitalized)
+ {
+ log()->error("[CoMZMPController::advance] The controller is not initialized. Please call "
+ "the 'initialize()' method");
+ return false;
+ }
+
+ if (m_isOutputValid)
+ return true;
+
+ // feed forward
+ m_controllerOutput = m_desiredCoMVelocity;
+
+ // CoM Controller
+ m_controllerOutput.noalias() += m_I_R_B.act(
+ m_CoMGain.asDiagonal() * m_I_R_B.inverse().act(m_desiredCoMPosition - m_CoMPosition));
+
+ // ZMP Controller
+ m_controllerOutput.noalias() += m_I_R_B.act(
+ m_ZMPGain.asDiagonal() * m_I_R_B.inverse().act(m_ZMPPosition - m_desiredZMPPosition));
+
+ m_isOutputValid = true;
+
+ return true;
+}
+
+bool CoMZMPController::setInput(const Input& input)
+{
+ this->setFeedback(input.CoMPosition, input.ZMPPosition, input.angle);
+ this->setSetPoint(input.desiredCoMVelocity, input.desiredCoMPosition, input.desiredZMPPosition);
+
+ return true;
+}
+
+void CoMZMPController::setSetPoint(Eigen::Ref<const Eigen::Vector2d> CoMVelocity,
+ Eigen::Ref<const Eigen::Vector2d> CoMPosition,
+ Eigen::Ref<const Eigen::Vector2d> ZMPPosition)
+{
+
+ m_desiredCoMVelocity = CoMVelocity;
+ m_desiredCoMPosition = CoMPosition;
+ m_desiredZMPPosition = ZMPPosition;
+
+ m_isOutputValid = false;
+}
+
+void CoMZMPController::setFeedback(Eigen::Ref<const Eigen::Vector2d> CoMPosition,
+ Eigen::Ref<const Eigen::Vector2d> ZMPPosition,
+ const manif::SO2d& I_R_B)
+{
+ m_CoMPosition = CoMPosition;
+ m_ZMPPosition = ZMPPosition;
+ m_I_R_B = I_R_B;
+
+ m_isOutputValid = false;
+}
+
+void CoMZMPController::setFeedback(Eigen::Ref<const Eigen::Vector2d> CoMPosition,
+ Eigen::Ref<const Eigen::Vector2d> ZMPPosition,
+ const double angle)
+{
+ this->setFeedback(CoMPosition, ZMPPosition, manif::SO2d(angle));
+}

src/SimplifiedModelControllers/tests/CMakeLists.txt

@@ -0,0 +1,8 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+add_bipedal_test(
+ NAME CoMZMPController
+ SOURCES CoMZMPControllerTest.cpp
+ LINKS BipedalLocomotion::SimplifiedModelControllers)

src/SimplifiedModelControllers/tests/CoMZMPControllerTest.cpp

@@ -0,0 +1,66 @@
+/**
+ * @file CoMZMPControllerTest.cpp
+ * @authors Giulio Romualdi
+ * @copyright 2021 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 <array>
+
+// Catch2
+#include <catch2/catch.hpp>
+
+#include <BipedalLocomotion/ParametersHandler/StdImplementation.h>
+#include <BipedalLocomotion/SimplifiedModelControllers/CoMZMPController.h>
+
+using namespace BipedalLocomotion::SimplifiedModelControllers;
+using namespace BipedalLocomotion::ParametersHandler;
+
+Eigen::Map<const Eigen::Vector2d> toEigen(const std::array<double, 2>& v)
+{
+ return Eigen::Map<const Eigen::Vector2d>(v.data(), v.size());
+};
+
+TEST_CASE("Controller")
+{
+ // the controller law is
+ // u = dx* + k_com (x* - x) - k_zmp (zmp* - zmp)
+ // here we chose
+ // k_com = diag([1, 2])
+ // k_zmp = diag([3, 4])
+ // x* = [0.01, 0]
+ // zmp* = [0.01, 0]
+ // x = [-0.02, 0.03]
+ // zmp = [0.04, -0.01]
+
+ std::array<double, 2> k_zmp{3, 4};
+ std::array<double, 2> k_com{1, 2};
+
+ CoMZMPController::Input input;
+ input.desiredCoMVelocity.setZero();
+ input.desiredCoMPosition << 0.01, 0;
+ input.desiredZMPPosition << 0.01, 0;
+ input.CoMPosition << -0.02, 0.03;
+ input.ZMPPosition << 0.04, -0.01;
+ input.angle = 0;
+
+ auto handler = std::make_shared<StdImplementation>();
+ handler->setParameter("zmp_gain", k_zmp);
+ handler->setParameter("com_gain", k_com);
+
+ CoMZMPController controller;
+ REQUIRE_FALSE(controller.isOutputValid());
+ REQUIRE(controller.initialize(handler));
+
+ controller.setInput(input);
+ REQUIRE(controller.advance());
+ REQUIRE(controller.isOutputValid());
+
+ Eigen::Vector2d expectedOutput = input.desiredCoMVelocity;
+ expectedOutput.noalias()
+ += toEigen(k_com).asDiagonal() * (input.desiredCoMPosition - input.CoMPosition);
+ expectedOutput.noalias()
+ += toEigen(k_zmp).asDiagonal() * (input.ZMPPosition - input.desiredZMPPosition);
+
+ expectedOutput.isApprox(controller.getOutput());
+}