Implement contact models library

CMakeLists.txt

@@ -3,14 +3,17 @@
 # GNU Lesser General Public License v2.1 or any later version.
 
 cmake_minimum_required(VERSION 3.5)
-set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD 17)
 
 ## MAIN project
 project(BipedalLocomotionControllers
  VERSION 0.0.1)
 
 list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
 
+option(BUILD_TESTING "Create tests using CMake" OFF)
+include(CTest)
+
 # Check BipedalLocomotionControllers dependencies, find necessary libraries.
 include(BipedalLocomotionControllersFindDependencies)
 

README.md

@@ -1,4 +1,4 @@
-# bipedal-locomotion-controllers <a href="https://isocpp.org"><img src="https://img.shields.io/badge/standard-C++14-blue.svg?style=flat&logo=c%2B%2B" alt="C++ Standard" /></a> </a><a href="./LICENSE"><img src="https://img.shields.io/badge/license-LGPL-19c2d8.svg" alt="Size" /></a>
+# bipedal-locomotion-controllers <a href="https://isocpp.org"><img src="https://img.shields.io/badge/standard-C++17-blue.svg?style=flat&logo=c%2B%2B" alt="C++ Standard" /></a> </a><a href="./LICENSE"><img src="https://img.shields.io/badge/license-LGPL-19c2d8.svg" alt="Size" /></a>
 
 The **bipedal-locomotion-controllers** project is a _suite_ of libraries for achieving bipedal locomotion on humanoid robots.
 

cmake/BipedalLocomotionControllersFindDependencies.cmake

@@ -117,3 +117,16 @@ endmacro()
 ################################################################################
 # Find all packages
 
+find_package(iDynTree 0.11.105 QUIET)
+checkandset_dependency(iDynTree)
+
+find_package(Catch2 QUIET)
+checkandset_dependency(Catch2)
+
+BIPEDAL_LOCOMOTION_CONTROLLERS_dependent_option(BIPEDAL_LOCOMOTION_CONTROLLERS_COMPILE_ContactModels
+ "Compile ContactModels library?" ON
+ BIPEDAL_LOCOMOTION_CONTROLLERS_HAS_iDynTree OFF)
+
+bipedal_locomotion_controllers_dependent_option(BIPEDAL_LOCOMOTION_CONTROLLERS_COMPILE_tests
+ "Compile tests?" ON
+ "BIPEDAL_LOCOMOTION_CONTROLLERS_HAS_Catch2;BUILD_TESTING" OFF)

src/CMakeLists.txt

@@ -2,3 +2,4 @@
 # This software may be modified and distributed under the terms of the
 # GNU Lesser General Public License v2.1 or any later version.
 
+add_subdirectory(ContactModels)

src/ContactModels/CMakeLists.txt

@@ -0,0 +1,59 @@
+# 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.
+
+# set target name
+if(BIPEDAL_LOCOMOTION_CONTROLLERS_COMPILE_ContactModels)
+
+ set(LIBRARY_TARGET_NAME ContactModels)
+
+ # set cpp files
+ set(${LIBRARY_TARGET_NAME}_SRC
+ src/ContactModel.cpp
+ src/ContinuousContactModel.cpp
+ )
+
+ # set hpp files
+ set(${LIBRARY_TARGET_NAME}_HDR
+ include/BipedalLocomotionControllers/ContactModels/ContactModel.h
+ include/BipedalLocomotionControllers/ContactModels/ContactModel.tpp
+ include/BipedalLocomotionControllers/ContactModels/ContinuousContactModel.h
+ )
+
+ # add an executable to the project using the specified source files.
+ add_library(${LIBRARY_TARGET_NAME} SHARED ${${LIBRARY_TARGET_NAME}_SRC} ${${LIBRARY_TARGET_NAME}_HDR})
+
+ set_target_properties(${LIBRARY_TARGET_NAME} PROPERTIES OUTPUT_NAME "${PROJECT_NAME}${LIBRARY_TARGET_NAME}")
+
+ target_link_libraries(${LIBRARY_TARGET_NAME} PUBLIC ${iDynTree_LIBRARIES})
+
+ add_library(BipedalLocomotionControllers::${LIBRARY_TARGET_NAME} ALIAS ${LIBRARY_TARGET_NAME})
+
+ set_target_properties(${LIBRARY_TARGET_NAME} PROPERTIES VERSION ${BipedalLocomotionControllers_VERSION}
+ PUBLIC_HEADER "${${LIBRARY_TARGET_NAME}_HDR}")
+
+ target_compile_features(${LIBRARY_TARGET_NAME} PUBLIC cxx_std_17)
+
+ # Specify include directories for both compilation and installation process.
+ # The $<INSTALL_PREFIX> generator expression is useful to ensure to create
+ # relocatable configuration files, see https://cmake.org/cmake/help/latest/manual/cmake-packages.7.html#creating-relocatable-packages
+ target_include_directories(${LIBRARY_TARGET_NAME} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+ "$<INSTALL_INTERFACE:$<INSTALL_PREFIX>/${CMAKE_INSTALL_INCLUDEDIR}>")
+
+ # Specify installation targets, typology and destination folders.
+ install(TARGETS ${LIBRARY_TARGET_NAME}
+ EXPORT ${PROJECT_NAME}
+ COMPONENT runtime
+ LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib
+ ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib
+ RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin
+ PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/BipedalLocomotionControllers/${LIBRARY_TARGET_NAME}" COMPONENT dev)
+
+ set_property(GLOBAL APPEND PROPERTY BipedalLocomotionControllers_TARGETS ${LIBRARY_TARGET_NAME})
+ set_property(GLOBAL APPEND PROPERTY BipedalLocomotionControllers_INCLUDES ${${LIBRARY_TARGET_NAME}_HDR})
+
+ add_subdirectory(tests)
+
+ message(STATUS "Created target ${LIBRARY_TARGET_NAME} for export ${PROJECT_NAME}.")
+
+endif()

src/ContactModels/include/BipedalLocomotionControllers/ContactModels/ContactModel.h

@@ -0,0 +1,112 @@
+/**
+ * @file ContactModel.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_CONTROLLERS_CONTACT_MODELS_CONTACT_MODEL_H
+#define BIPEDAL_LOCOMOTION_CONTROLLERS_CONTACT_MODELS_CONTACT_MODEL_H
+
+#include <any>
+#include <unordered_map>
+#include <string>
+
+#include <iDynTree/Core/MatrixFixSize.h>
+#include <iDynTree/Core/VectorFixSize.h>
+#include <iDynTree/Core/Wrench.h>
+
+namespace BipedalLocomotionControllers
+{
+namespace ContactModels
+{
+/**
+ * ContactModel is a generic implementation of a contact model. It computes the contact wrench
+ * between the robot and the environments
+ */
+class ContactModel
+{
+protected:
+ iDynTree::Wrench m_contactWrench; /**< Contact wrench between the robot and the environment
+ expressed in mixed representation */
+
+ /** Autonomous dynamics of the contact model rate of change (i.e. given a non linear system\f$
+ * \dot{x} = f + g u\f$ the autonomous is \a f */
+ iDynTree::Vector6 m_autonomousDynamics;
+
+ /** Autonomous dynamics of the contact model rate of change (i.e. given a non linear system\f$
+ * \dot{x} = f + g u\f$ the autonomous is \a g */
+ iDynTree::Matrix6x6 m_controlMatrix;
+
+ bool m_isContactWrenchComputed; /**< If true the contact wrench has been already computed */
+ bool m_isAutonomusDynamicsComputed; /**< If true the autonomous dynamics has been already computed */
+ bool m_isControlMatrixComputed; /**< If true the controllers matrix has been already computed */
+
+ /**
+ * Evaluate the contact wrench given a specific contact model
+ */
+ virtual void computeContactWrench() = 0;
+
+ /**
+ * Evaluate the Autonomous System Dynamics of the derivative of a specific contact model
+ */
+ virtual void computeAutonomousDynamics() = 0;
+
+ /**
+ * Evaluate the Control Matrix of the derivative of a specific contact model
+ */
+ virtual void computeControlMatrix() = 0;
+
+ /**
+ * Get variable from a set of variables
+ * @param variables map containing variables
+ * @param variableName name of the variable
+ * @param variable variable
+ * @return true/false in case of success/failure
+ */
+ template <class T>
+ bool getVariable(const std::unordered_map<std::string, std::any>& variables,
+ const std::string& variableName,
+ T& variable);
+
+ /**
+ * Set the immutable parameters
+ */
+ virtual void setImmutableParameters(const std::unordered_map<std::string, std::any>& parameters) = 0;
+
+public:
+ /**
+ * Get and compute (only if it is necessary) the contact wrench
+ * @return the contact wrench expressed in mixed representation
+ */
+ const iDynTree::Wrench& getContactWrench();
+
+ /**
+ * Get and compute (only if it is necessary) the autonomous system dynamics
+ * @return the autonomous system dynamics at a given state
+ */
+ const iDynTree::Vector6& getAutonomousDynamics();
+
+ /**
+ * Get and compute (only if it is necessary) the control matrix
+ * @return the control matrix at a given state
+ */
+ const iDynTree::Matrix6x6& getControlMatrix();
+
+ /**
+ * Set the internal state of the model
+ */
+ virtual void setState(const std::unordered_map<std::string, std::any>& state) = 0;
+
+ /**
+ * Set the mutable parameters
+ */
+ virtual void setMutableParameters(const std::unordered_map<std::string, std::any>& parameters) = 0;
+};
+} // namespace ContactModels
+} // namespace BipedalLocomotionControllers
+
+
+#include "ContactModel.tpp"
+
+#endif // BIPEDAL_LOCOMOTION_CONTROLLERS_CONTACT_MODELS_CONTACT_MODEL_H

src/ContactModels/include/BipedalLocomotionControllers/ContactModels/ContactModel.tpp

@@ -0,0 +1,33 @@
+/**
+ * @file ContactModel.tpp
+ * @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 <iostream>
+
+namespace BipedalLocomotionControllers
+{
+namespace ContactModels
+{
+template <class T>
+bool ContactModel::getVariable(const std::unordered_map<std::string, std::any>& variables,
+ const std::string& variableName, T& variable)
+{
+ const auto& var = variables.find(variableName);
+
+ if (var == variables.end())
+ {
+ std::cerr << "[ContactModel::getVariable] The variables named " << variableName
+ << " cannot be found." << std::endl;
+ return false;
+ }
+
+ variable = std::any_cast<T>(var->second);
+
+ return true;
+}
+
+} // namespace ContactModels
+} // namespace BipedalLocomotionControllers

src/ContactModels/include/BipedalLocomotionControllers/ContactModels/ContinuousContactModel.h

@@ -0,0 +1,141 @@
+/**
+ * @file ContinuousContactModel.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_CONTROLLERS_CONTACT_MODELS_CONTINUOUS_CONTACT_MODEL_H
+#define BIPEDAL_LOCOMOTION_CONTROLLERS_CONTACT_MODELS_CONTINUOUS_CONTACT_MODEL_H
+
+#include <iDynTree/Core/Transform.h>
+#include <iDynTree/Core/Twist.h>
+#include <iDynTree/Core/Wrench.h>
+
+#include <BipedalLocomotionControllers/ContactModels/ContactModel.h>
+
+namespace BipedalLocomotionControllers
+{
+namespace ContactModels
+{
+/**
+ * ContinuousContactModel is a model that describe the contact using a continuous representation. It
+ * is an extension of the brush model used for describing the contact between the tire and the
+ * ground. Each point in the contact surface is subjected to an infinitesimal force \f$ f \f$ given
+ * by \f$ f=k(x_0 - x)- b x\f$, where \f$k\f$ is the spring coefficient and \f$b\f$ is the damper
+ * coefficient. \f$x\f$ and \f$x_0\f$ are, respectively, the position of the point placed in the
+ * contact surface and the point con corresponding to a null contact force written in the inertial
+ * frame. Furthermore, during the contact scenario, we assume the link acts as a rigid body. While
+ * the environment will deform. The ground characteristics are isotropic, and it can be approximated
+ * as a continuum of springs and dampers.
+ * The contact surface is supposed to be <b>rectangular</b> and the frame placed on the contact
+ * surfaced is centered in the middle of the surface, with the \a z-axis pointing upwards and the \a
+ * x-axis pointing forward (parallel to one edge of the rectangle).
+ */
+class ContinuousContactModel final : public ContactModel
+{
+ iDynTree::Transform m_frameTransform; /**< Homogeneous transformation of the frame placed in the
+ center of the contact surface */
+ iDynTree::Transform m_nullForceTransform; /**< Homogeneous transformation corresponding to a
+ null contact wrench */
+ iDynTree::Twist m_twist; /**< Twist expressed in mixed representation of the frame placed in the
+ center of the contact surface */
+
+ double m_springCoeff; /**< Spring coefficient associated to the model */
+ double m_damperCoeff; /**< Damper coefficient associated to the model */
+
+ double m_length; /**< Length of the rectangular contact surface */
+ double m_width; /**< Width of the rectangular contact surface */
+
+ /**
+ * Evaluate the contact wrench given a specific contact model
+ */
+ void computeContactWrench() final;
+
+ /**
+ * Evaluate the Autonomous System Dynamics of the derivative of a specific contact model
+ */
+ void computeAutonomousDynamics() final;
+
+ /**
+ * Evaluate the Control Matrix of the derivative of a specific contact model
+ */
+ void computeControlMatrix() final;
+
+ /**
+ * Set the parameters cannot change
+ * @param state std::unordered_map containing the mutable parameters of the system. To compute the contact
+ * wrench between two system (i.e. a link of a robot and the environment) the following data has
+ * to be available. If not an std::runtime_error is thrown.
+ * - \a length (double): length in meters associated to the model
+ * - \a width (double): width in meters associated to the model
+ */
+ void setImmutableParameters(const std::unordered_map<std::string, std::any>& parameters) final;
+
+public:
+
+ /**
+ * Constructor. It instantiate the value of the mutable parameters and the immutable parameters.
+ * @param immutableParameters std::unordered_map containing the immutable parameters of the
+ * system. The list of the required parameters could be found in @ref
+ * BipedalLocomotionController::ContactModels::setImmutableparameters
+ * @param mutableParameters std::unordered_map containing the mutable parameters of the
+ * system. The list of the required parameters could be found in @ref
+ * BipedalLocomotionController::ContactModels::setMutableparameters
+ */
+ ContinuousContactModel(const std::unordered_map<std::string, std::any>& immutableParameters,
+ const std::unordered_map<std::string, std::any>& mutableParameters);
+
+ /**
+ * Constructor. It instantiate the value of the mutable parameters and the immutable parameters.
+ * @param parameters std::unordered_map containing the immutable and mutable parameters of the
+ * system. The list of the required parameters could be found in @ref
+ * BipedalLocomotionController::ContactModels::setMutableparameters
+ */
+ ContinuousContactModel(const std::unordered_map<std::string, std::any>& parameters);
+
+
+ /**
+ * Set the internal state of the model.
+ * @param state std::unordered_map containing the state of the system. To compute the contact
+ * wrench between two system (i.e. a link of a robot and the environment) the following data has
+ * to be available. If not an std::runtime_error is thrown.
+ * - \a frame_transform (iDynTree::Transform): transformation between the link and the inertial frame;
+ * - \a null_force_transform (iDynTree::Transform): transformation corresponding to a null force;
+ * - \a twist (iDynTree::Twist): twist (expressed in mixed representation) of the link
+ */
+ void setState(const std::unordered_map<std::string, std::any>& state) final;
+
+ /**
+ * Set the parameters may depends on time/state (i.e. they are not considered constant)
+ * @param state std::unordered_map containing the mutable parameters of the system. To compute the contact
+ * wrench between two system (i.e. a link of a robot and the environment) the following data has
+ * to be available. If not an std::runtime_error is thrown.
+ * - \a spring_coeff (double): spring coefficient associated to the model
+ * - \a damper_coeff (double): damper coefficient associated to the model
+ */
+ void setMutableParameters(const std::unordered_map<std::string, std::any>& parameters) final;
+
+ /**
+ * Compute the force in a particular point in the contact surface.
+ * @param x x-coordinate of the point expressed in a frame placed in the contact surface whose
+ * origin is the center of the surface [in meters]
+ * @param y y-coordinate of the point expressed in a frame placed in the contact surface whose
+ * origin is the center of the surface [in meters]
+ */
+ iDynTree::Force getForceAtPoint(const double& x, const double& y);
+
+ /**
+ * Compute the torque about the origin of the frame place in the contact surface whose
+ * origin is the center of the surface generated by the force acting on the point (x, y)
+ * @param x x-coordinate of the point expressed in a frame placed in the contact surface whose
+ * origin is the center of the surface [in meters]
+ * @param y y-coordinate of the point expressed in a frame placed in the contact surface whose
+ * origin is the center of the surface [in meters]
+ */
+ iDynTree::Torque getTorqueGeneratedAtPoint(const double& x, const double& y);
+};
+} // namespace ContactModels
+} // namespace BipedalLocomotionControllers
+
+#endif // BIPEDAL_LOCOMOTION_CONTROLLERS_CONTACT_MODELS_CONTINUOUS_CONTACT_MODEL_H