diff --git a/CMakeLists.txt b/CMakeLists.txt index 3859043f3c..fafa569461 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,6 @@ # GNU Lesser General Public License v2.1 or any later version. cmake_minimum_required(VERSION 3.5) -set(CMAKE_CXX_STANDARD 14) ## MAIN project project(BipedalLocomotionFramework diff --git a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake index df896da1d2..c9ad923f19 100644 --- a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake @@ -143,3 +143,7 @@ framework_dependent_option(FRAMEWORK_COMPILE_Estimators framework_dependent_option(FRAMEWORK_COMPILE_Planners "Compile Planners libraries?" ON "FRAMEWORK_HAS_Eigen3" OFF) + +framework_dependent_option(FRAMEWORK_COMPILE_ContactModels + "Compile ContactModels library?" ON + FRAMEWORK_HAS_Eigen3 OFF) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 967a5fa195..5718fdba1d 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -8,3 +8,4 @@ add_subdirectory(ParametersHandler) add_subdirectory(Estimators) add_subdirectory(System) add_subdirectory(Planners) +add_subdirectory(ContactModels) diff --git a/src/ContactModels/CMakeLists.txt b/src/ContactModels/CMakeLists.txt new file mode 100644 index 0000000000..c22dea8379 --- /dev/null +++ b/src/ContactModels/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. + +# set target name +if(FRAMEWORK_COMPILE_ContactModels) + + add_bipedal_locomotion_library( + NAME ContactModels + SOURCES src/ContactModel.cpp src/ContinuousContactModel.cpp + PUBLIC_HEADERS include/BipedalLocomotion/ContactModels/ContactModel.h include/BipedalLocomotion/ContactModels/ContinuousContactModel.h + PUBLIC_LINK_LIBRARIES iDynTree::idyntree-core BipedalLocomotion::ParametersHandler + DEPENDS_ON_EIGEN_PRIVATE + SUBDIRECTORIES tests) + +endif() diff --git a/src/ContactModels/include/BipedalLocomotion/ContactModels/ContactModel.h b/src/ContactModels/include/BipedalLocomotion/ContactModels/ContactModel.h new file mode 100644 index 0000000000..12c4690a7d --- /dev/null +++ b/src/ContactModels/include/BipedalLocomotion/ContactModels/ContactModel.h @@ -0,0 +1,150 @@ +/** + * @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_CONTACT_MODELS_CONTACT_MODEL_H +#define BIPEDAL_LOCOMOTION_CONTACT_MODELS_CONTACT_MODEL_H + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace BipedalLocomotion +{ +namespace ContactModels +{ +/** + * ContactModel is a generic implementation of a contact model. It computes the contact wrench + * between the robot and the environments. + */ +class ContactModel +{ + bool m_isContactWrenchComputed; /**< If true the contact wrench has been already computed */ + bool m_isAutonomousDynamicsComputed; /**< If true the autonomous dynamics has been already + computed */ + bool m_isControlMatrixComputed; /**< If true the controllers matrix has been already computed */ + + bool m_isRegressorComputed; /**< If true the regressor matrix has been already computed */ + +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 dynamics is \a f */ + iDynTree::Vector6 m_autonomousDynamics; + + /** Control matrix of the contact model rate of change (i.e. given a non linear system + * \f$\dot{x} = f + g u\f$ the control matrix is \a g */ + iDynTree::Matrix6x6 m_controlMatrix; + + /** Contains the regressor of the contact model. \f$f = A \theta\f$, where \f$f\f$ is the + * contact wrench, \f$A\f$ the regressor and \f$\theta\f$ the parameters */ + iDynTree::MatrixDynSize m_regressor; + + /** + * 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; + + /** + * Evaluate the regressor matrix + */ + virtual void computeRegressor() = 0; + + /** + * Initialization of the class. + * @param handler std::weak_ptr to a parameter container. This class does not have the ownership + * of the container. + * @note the required parameters may depends on the particular implementation. An example + * can be found in BipedalLocomotionControllers::ContactModel::ContinuousContactmodel::initializePrivate + * @return true/false in case of success/failure + */ + virtual bool initializePrivate(std::weak_ptr handler) = 0; + + /** + * Set the internal state of the model. + */ + virtual void setStatePrivate(const iDynTree::Twist& twist, + const iDynTree::Transform& transform) = 0; + + /** + * Set the null force transform. + */ + virtual void setNullForceTransformPrivate(const iDynTree::Transform& transform) = 0; + +public: + /** + * Initialization of the class. Please call this method before evaluating any other function + * @param handler std::weak_ptr to a parameter container. This class does not have the ownership + * of the container. + * @warning std::weak_ptr models temporary ownership: when the handler is accessed only if it + * exists, the std::weak_ptr is converted in a std::shared_ptr. + * @note the required parameters may depends on the particular implementation. An example + * can be found in BipedalLocomotionControllers::ContactModel::ContinuousContactmodel::initializePrivate + * @return true/false in case of success/failure + */ + bool initialize(std::weak_ptr handler); + + /** + * 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(); + + /** + * Get and compute (only if it is necessary) the regressor + * @return the regressor at a given state + */ + const iDynTree::MatrixDynSize& getRegressor(); + + /** + * Set the internal state of the model. + * @note the meaning of the parameters may depend on the particular implementation. An example + * can be found in BipedalLocomotionControllers::ContactModel::ContinuousContactmodel::setState + */ + void setState(const iDynTree::Twist& twist, + const iDynTree::Transform& transform); + + void setNullForceTransform(const iDynTree::Transform& transform); + +}; +} // namespace ContactModels +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_CONTACT_MODELS_CONTACT_MODEL_H diff --git a/src/ContactModels/include/BipedalLocomotion/ContactModels/ContinuousContactModel.h b/src/ContactModels/include/BipedalLocomotion/ContactModels/ContinuousContactModel.h new file mode 100644 index 0000000000..107b444c2d --- /dev/null +++ b/src/ContactModels/include/BipedalLocomotion/ContactModels/ContinuousContactModel.h @@ -0,0 +1,147 @@ +/** + * @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_CONTACT_MODELS_CONTINUOUS_CONTACT_MODEL_H +#define BIPEDAL_LOCOMOTION_CONTACT_MODELS_CONTINUOUS_CONTACT_MODEL_H + +#include +#include +#include + +#include +#include + +namespace BipedalLocomotion +{ +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 rectangular. 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). We define \a length the length + * of the edge parallel to the \x coordinate and width the length of the edge parallel to the \a y + * coordinate. + * @warning This model assumes infinite friction. Please make sure that the friction force between + * the contact surface and the environment is feasible (i.e. by enforcing a constraint in the + * controller). + */ +class ContinuousContactModel final : public ContactModel +{ + /** Homogeneous transformation of the frame placed in the center of the contact surface */ + iDynTree::Transform m_frameTransform{iDynTree::Transform::Identity()}; + + /** Homogeneous transformation corresponding to a null contact wrench */ + iDynTree::Transform m_nullForceTransform{iDynTree::Transform::Identity()}; + + /** Twist expressed in mixed representation of the frame placed in the center of the contact + * surface */ + iDynTree::Twist m_twist{iDynTree::Twist::Zero()}; + + double m_springCoeff{0.0}; /**< Spring coefficient associated to the model */ + double m_damperCoeff{0.0}; /**< Damper coefficient associated to the model */ + + double m_length{0.0}; /**< Length of the rectangular contact surface */ + double m_width{0.0}; /**< 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; + + /** + * Evaluate the regressor matrix + */ + void computeRegressor() final; + + /** + * Initialization of the class. Please call this method before evaluating any other function + * @param handler std::weak_ptr to a parameter container. This class does not have the ownership + * of the container. + * @note The following parameters are required. + * - \a length (double): length in meters associated to the model + * - \a width (double): width in meters associated to the model + * - \a spring_coeff (double): spring coefficient associated to the model + * - \a damper_coeff (double): damper coefficient associated to the model + * @return true/false in case of success/failure + */ + bool initializePrivate(std::weak_ptr handler) final; + + /** + * Set the internal state of the model. + * @param twist spatial velocity (expressed in mixed representation) of the link + * @param transform transformation between the link and the inertial frame + */ + void setStatePrivate(const iDynTree::Twist& twist, + const iDynTree::Transform& transform) final; + + /** + * Set the null force transform of the model. + * @param transform transformation corresponding to a null force expressed w.r.t. the inertial + * frame + */ + void setNullForceTransformPrivate(const iDynTree::Transform& transform) final; + + +public: + + /** + * Constructor + */ + ContinuousContactModel(); + + /** + * Compute the contact force applied by the environment on the system in a particular point of + * 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 applied by the environment on the system 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); + + const double& springCoeff() const; + + double& springCoeff(); + + const double& damperCoeff() const; + + double& damperCoeff(); +}; +} // namespace ContactModels +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_CONTACT_MODELS_CONTINUOUS_CONTACT_MODEL_H diff --git a/src/ContactModels/src/ContactModel.cpp b/src/ContactModels/src/ContactModel.cpp new file mode 100644 index 0000000000..3be37d8225 --- /dev/null +++ b/src/ContactModels/src/ContactModel.cpp @@ -0,0 +1,92 @@ +/** + * @file ContactModel.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 + +using namespace BipedalLocomotion::ContactModels; + +bool ContactModel::initialize(std::weak_ptr handler) +{ + // the parameters has been update the previous quantities has to be evaluated again + m_isContactWrenchComputed = false; + m_isControlMatrixComputed = false; + m_isAutonomousDynamicsComputed = false; + m_isRegressorComputed = false; + + return initializePrivate(handler); +} + +void ContactModel::setNullForceTransform(const iDynTree::Transform& nullForceTransform) +{ + // the state has been update the previous quantities has to be evaluated again + m_isContactWrenchComputed = false; + m_isControlMatrixComputed = false; + m_isAutonomousDynamicsComputed = false; + m_isRegressorComputed = false; + + + setNullForceTransformPrivate(nullForceTransform); +} + +void ContactModel::setState(const iDynTree::Twist& twist, + const iDynTree::Transform& transform) + +{ + // the state has been update the previous quantities has to be evaluated again + m_isContactWrenchComputed = false; + m_isControlMatrixComputed = false; + m_isAutonomousDynamicsComputed = false; + m_isRegressorComputed = false; + + setStatePrivate(twist, transform); + + return; +} + +const iDynTree::Wrench& ContactModel::getContactWrench() +{ + if (!m_isContactWrenchComputed) + { + computeContactWrench(); + m_isContactWrenchComputed = true; + } + + return m_contactWrench; +} + +const iDynTree::Vector6& ContactModel::getAutonomousDynamics() +{ + if (!m_isAutonomousDynamicsComputed) + { + computeAutonomousDynamics(); + m_isAutonomousDynamicsComputed = true; + } + + return m_autonomousDynamics; +} + +const iDynTree::Matrix6x6& ContactModel::getControlMatrix() +{ + if (!m_isControlMatrixComputed) + { + computeControlMatrix(); + m_isControlMatrixComputed = true; + } + + return m_controlMatrix; +} + +const iDynTree::MatrixDynSize& ContactModel::getRegressor() +{ + if (!m_isRegressorComputed) + { + computeRegressor(); + m_isRegressorComputed = true; + } + + return m_regressor; +} diff --git a/src/ContactModels/src/ContinuousContactModel.cpp b/src/ContactModels/src/ContinuousContactModel.cpp new file mode 100644 index 0000000000..f418831f56 --- /dev/null +++ b/src/ContactModels/src/ContinuousContactModel.cpp @@ -0,0 +1,274 @@ +/** + * @file ContinuousContactModel.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 + +#include +#include + +using namespace BipedalLocomotion::ContactModels; +using namespace BipedalLocomotion::ParametersHandler; + +ContinuousContactModel::ContinuousContactModel() +{ + m_controlMatrix.zero(); + m_autonomousDynamics.zero(); + m_regressor.resize(6, 2); + m_regressor.zero(); +} + +bool ContinuousContactModel::initializePrivate(std::weak_ptr weakHandler) +{ + auto handler = weakHandler.lock(); + if (handler == nullptr) + { + std::cerr << "[ContinuousContactModel::initialize] The parameter handler is corrupted. " + "Please make sure that the handler exists." + << std::endl; + return false; + } + + if (!handler->getParameter("length", m_length)) + { + std::cerr << "[ContinuousContactModel::initialize] Unable to get the variable named length." + << std::endl; + return false; + } + + if (!handler->getParameter("width", m_width)) + { + std::cerr << "[ContinuousContactModel::initialize] Unable to get the variable named width." + << std::endl; + return false; + } + + if (!handler->getParameter("spring_coeff", m_springCoeff)) + { + std::cerr << "[ContinuousContactModel::initialize] Unable to get the variable named " + "spring_coeff." + << std::endl; + return false; + } + + if (!handler->getParameter("damper_coeff", m_damperCoeff)) + { + std::cerr << "[ContinuousContactModel::initialize] Unable to get the variable named " + "damper_coeff." + << std::endl; + return false; + } + return true; +} + +void ContinuousContactModel::setNullForceTransformPrivate(const iDynTree::Transform& transform) +{ + m_nullForceTransform = transform; +} + +void ContinuousContactModel::setStatePrivate(const iDynTree::Twist& twist, + const iDynTree::Transform& transform) +{ + m_twist = twist; + m_frameTransform = transform; +} + +void ContinuousContactModel::computeContactWrench() +{ + double area = m_length * m_width; + + auto force(iDynTree::toEigen(m_contactWrench.getLinearVec3())); + auto torque(iDynTree::toEigen(m_contactWrench.getAngularVec3())); + + auto position(iDynTree::toEigen(m_frameTransform.getPosition())); + auto rotation(iDynTree::toEigen(m_frameTransform.getRotation())); + + auto linearVelocity(iDynTree::toEigen(m_twist.getLinearVec3())); + auto angularVelocity(iDynTree::toEigen(m_twist.getAngularVec3())); + + auto nullForcePosition(iDynTree::toEigen(m_nullForceTransform.getPosition())); + auto nullForceRotation(iDynTree::toEigen(m_nullForceTransform.getRotation())); + + // compute the force + force = std::abs(rotation(2, 2)) * area * (m_springCoeff * (nullForcePosition - position) + - m_damperCoeff * linearVelocity); + + // compute the torque + auto skewRe1 = iDynTree::skew(rotation.col(0)); + auto skewRe2 = iDynTree::skew(rotation.col(1)); + torque = std::abs(rotation(2, 2)) * area / 12 * (m_length * m_length + * (m_damperCoeff * skewRe1 * skewRe1 * angularVelocity + + m_springCoeff * skewRe1 * nullForceRotation.col(0)) + + m_width * m_width + * (m_damperCoeff * skewRe2 * skewRe2 * angularVelocity + + m_springCoeff * skewRe2 * nullForceRotation.col(1))); +} + +void ContinuousContactModel::computeAutonomousDynamics() +{ + double area = m_length * m_width; + + auto autonomousDynamics(iDynTree::toEigen(m_autonomousDynamics)); + + auto position(iDynTree::toEigen(m_frameTransform.getPosition())); + auto rotation(iDynTree::toEigen(m_frameTransform.getRotation())); + + auto linearVelocity(iDynTree::toEigen(m_twist.getLinearVec3())); + auto angularVelocity(iDynTree::toEigen(m_twist.getAngularVec3())); + + auto nullForcePosition(iDynTree::toEigen(m_nullForceTransform.getPosition())); + auto nullForceRotation(iDynTree::toEigen(m_nullForceTransform.getRotation())); + + Eigen::Matrix3d rotationRateOfChange = iDynTree::skew(angularVelocity) * rotation; + + autonomousDynamics.head(3) = area * (rotationRateOfChange(2,2) * (m_springCoeff * (nullForcePosition - position) + - m_damperCoeff * linearVelocity) + - rotation(2,2) * m_springCoeff * linearVelocity); + + auto skewRe1 = iDynTree::skew(rotation.col(0)); + auto skewRe2 = iDynTree::skew(rotation.col(1)); + auto skewDotRe1 = iDynTree::skew(rotationRateOfChange.col(0)); + auto skewDotRe2 = iDynTree::skew(rotationRateOfChange.col(1)); + + autonomousDynamics.tail(3) + = area / 12 * (rotationRateOfChange(2, 2) * (m_length * m_length * (m_damperCoeff * skewRe1 * skewRe1 * angularVelocity + + m_springCoeff * skewRe1 * nullForceRotation.col(0)) + + m_width * m_width * (m_damperCoeff * skewRe2 * skewRe2 * angularVelocity + + m_springCoeff * skewRe2 * nullForceRotation.col(1))) + + rotation(2, 2) * (m_length * m_length * (m_springCoeff * skewDotRe1 * nullForceRotation.col(0) + + m_damperCoeff * (skewDotRe1 * skewRe1 + skewRe1 * skewDotRe1) * angularVelocity) + + (m_width * m_width * (m_springCoeff * skewDotRe2 * nullForceRotation.col(1) + + m_damperCoeff * (skewDotRe2 * skewRe2 + skewRe2 * skewDotRe2) * angularVelocity)))); + +} + +void ContinuousContactModel::computeControlMatrix() +{ + double area = m_length * m_width; + + auto controlMatrix(iDynTree::toEigen(m_controlMatrix)); + + auto position(iDynTree::toEigen(m_frameTransform.getPosition())); + auto rotation(iDynTree::toEigen(m_frameTransform.getRotation())); + + auto linearVelocity(iDynTree::toEigen(m_twist.getLinearVec3())); + auto angularVelocity(iDynTree::toEigen(m_twist.getAngularVec3())); + + auto nullForcePosition(iDynTree::toEigen(m_nullForceTransform.getPosition())); + auto nullForceRotation(iDynTree::toEigen(m_nullForceTransform.getRotation())); + + Eigen::Matrix3d rotationRateOfChange = iDynTree::skew(angularVelocity) * rotation; + + controlMatrix.topLeftCorner(3, 3).diagonal().array() = - area * m_damperCoeff * rotation(2,2); + + auto skewRe1 = iDynTree::skew(rotation.col(0)); + auto skewRe2 = iDynTree::skew(rotation.col(1)); + controlMatrix.bottomRightCorner(3, 3) = area / 12 * rotation(2,2) * m_damperCoeff * + (m_length * m_length * skewRe1 * skewRe1 + m_width * m_width * skewRe2 * skewRe2); +} + +iDynTree::Force ContinuousContactModel::getForceAtPoint(const double& x, const double& y) +{ + auto position(iDynTree::toEigen(m_frameTransform.getPosition())); + auto rotation(iDynTree::toEigen(m_frameTransform.getRotation())); + + auto linearVelocity(iDynTree::toEigen(m_twist.getLinearVec3())); + auto angularVelocity(iDynTree::toEigen(m_twist.getAngularVec3())); + + auto nullForcePosition(iDynTree::toEigen(m_nullForceTransform.getPosition())); + auto nullForceRotation(iDynTree::toEigen(m_nullForceTransform.getRotation())); + + iDynTree::Force force; + if (std::abs(x) > m_length / 2 || std::abs(y) > m_width / 2) + { + force.zero(); + return force; + } + + iDynTree::Vector3 pointPosition; + pointPosition(0) = x; + pointPosition(1) = y; + pointPosition(2) = 0; + + iDynTree::toEigen(force) = + m_springCoeff * ((nullForcePosition - position) + + (nullForceRotation - rotation) * iDynTree::toEigen(pointPosition)) + - m_damperCoeff * (linearVelocity + iDynTree::skew(angularVelocity) * rotation * iDynTree::toEigen(pointPosition)); + + return force; +} + +iDynTree::Torque ContinuousContactModel::getTorqueGeneratedAtPoint(const double& x, const double& y) +{ + iDynTree::Torque torque; + if (std::abs(x) > m_length / 2 || std::abs(y) > m_width / 2) + { + torque.zero(); + return torque; + } + + auto rotation(iDynTree::toEigen(m_frameTransform.getRotation())); + iDynTree::Vector3 pointPosition; + pointPosition(0) = x; + pointPosition(1) = y; + pointPosition(2) = 0; + + iDynTree::toEigen(torque) = (rotation * iDynTree::toEigen(pointPosition)).cross(iDynTree::toEigen(getForceAtPoint(x, y))); + return torque; +} + +void ContinuousContactModel::computeRegressor() +{ + double area = m_length * m_width; + auto position(iDynTree::toEigen(m_frameTransform.getPosition())); + auto rotation(iDynTree::toEigen(m_frameTransform.getRotation())); + + auto linearVelocity(iDynTree::toEigen(m_twist.getLinearVec3())); + auto angularVelocity(iDynTree::toEigen(m_twist.getAngularVec3())); + + auto nullForcePosition(iDynTree::toEigen(m_nullForceTransform.getPosition())); + auto nullForceRotation(iDynTree::toEigen(m_nullForceTransform.getRotation())); + + auto skewRe1 = iDynTree::skew(rotation.col(0)); + auto skewRe2 = iDynTree::skew(rotation.col(1)); + + auto regressor(iDynTree::toEigen(m_regressor)); + + regressor.topLeftCorner<3, 1>() + = std::abs(rotation(2, 2)) * area * (nullForcePosition - position); + + regressor.topRightCorner<3, 1>() = -std::abs(rotation(2, 2)) * area * linearVelocity; + + regressor.bottomLeftCorner<3, 1>() + = area / 12.0 * std::abs(rotation(2, 2)) + * (m_length * m_length * skewRe1 * nullForceRotation.col(0) + + m_width * m_width * skewRe2 * nullForceRotation.col(1)); + + regressor.bottomRightCorner<3, 1>() + = area / 12.0 * std::abs(rotation(2, 2)) + * (m_length * m_length * skewRe1 * skewRe1 + m_width * m_width * skewRe2 * skewRe2) + * angularVelocity; +} + +const double& ContinuousContactModel::springCoeff() const +{ + return m_springCoeff; +} + +double& ContinuousContactModel::springCoeff() +{ + return m_springCoeff; +} + +const double& ContinuousContactModel::damperCoeff() const +{ + return m_damperCoeff; +} + +double& ContinuousContactModel::damperCoeff() +{ + return m_damperCoeff; +} diff --git a/src/ContactModels/tests/CMakeLists.txt b/src/ContactModels/tests/CMakeLists.txt new file mode 100644 index 0000000000..0e91ce3495 --- /dev/null +++ b/src/ContactModels/tests/CMakeLists.txt @@ -0,0 +1,8 @@ +# Copyright (C) 2019 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 ContinousContactModel + SOURCES ContinousContactModelTest.cpp + LINKS BipedalLocomotion::ContactModels BipedalLocomotion::ParametersHandlerYarpImplementation) diff --git a/src/ContactModels/tests/ContinousContactModelTest.cpp b/src/ContactModels/tests/ContinousContactModelTest.cpp new file mode 100644 index 0000000000..0f8db058ec --- /dev/null +++ b/src/ContactModels/tests/ContinousContactModelTest.cpp @@ -0,0 +1,213 @@ +/** + * @file ContinousContactModelTest.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 + +// Catch2 +#include + +#include +#include + +#include +#include + +using namespace iDynTree; +using namespace BipedalLocomotion::ContactModels; +using namespace BipedalLocomotion::ParametersHandler; + +template +void checkVectorAreEqual(const T& vector1, const U& vector2, double tol = 0) +{ + REQUIRE(vector1.size() == vector2.size()); + + for (unsigned int i = 0; i < vector1.size(); i++) + REQUIRE(std::abs(vector1[i] - vector2[i]) <= tol); +} + +TEST_CASE("Continuous Contact") +{ + Transform world_T_link{Transform::Identity()}; + world_T_link.setRotation(Rotation::RPY(-0.15, 0.2, 0.1)); + world_T_link.setPosition(Position(-0.02, 0.01, 0.005)); + + Transform nullForceTransform{Transform::Identity()}; + Twist linkVelocity{Twist::Zero()}; + toEigen(linkVelocity.getLinearVec3()).setRandom(); + toEigen(linkVelocity.getAngularVec3()).setRandom(); + + double springCoeff = 2000.0; + double damperCoeff = 100.0; + + double length = 0.12; + double width = 0.09; + + std::shared_ptr handler = std::make_shared(); + handler->setParameter("spring_coeff", springCoeff); + handler->setParameter("damper_coeff", damperCoeff); + handler->setParameter("length", length); + handler->setParameter("width", width); + + ContinuousContactModel model; + REQUIRE(model.initialize(handler)); + model.setState(linkVelocity, world_T_link); + model.setNullForceTransform(nullForceTransform); + + SECTION("Test contact wrench") + { + // Compute numerical integral using Montecarlo method + + // Instantiate the two uniform distributions + std::default_random_engine generator; + generator.seed(42); + std::uniform_real_distribution xAxis(-length / 2, length / 2); + std::uniform_real_distribution yAxis(-width / 2, width / 2); + + double area = length * width; + unsigned int samples = 1e7; + + Wrench numericalWrench; + numericalWrench.zero(); + + for (unsigned int i = 0; i < samples; i++) + { + double x = xAxis(generator); + double y = yAxis(generator); + + // force + toEigen(numericalWrench.getLinearVec3()) += toEigen(model.getForceAtPoint(x, y)); + + // torque + toEigen(numericalWrench.getAngularVec3()) + += toEigen(model.getTorqueGeneratedAtPoint(x, y)); + } + + toEigen(numericalWrench.getLinearVec3()) = toEigen(numericalWrench.getLinearVec3()) + / samples * area + * std::abs(world_T_link.getRotation()(2, 2)); + toEigen(numericalWrench.getAngularVec3()) = toEigen(numericalWrench.getAngularVec3()) + / samples * area + * std::abs(world_T_link.getRotation()(2, 2)); + + double tollerance = 1e-4; + + checkVectorAreEqual(numericalWrench.getLinearVec3(), + model.getContactWrench().getLinearVec3(), + tollerance); + checkVectorAreEqual(numericalWrench.getAngularVec3(), + model.getContactWrench().getAngularVec3(), + tollerance); + } + + SECTION("Test regressor") + { + MatrixDynSize regressor = model.getRegressor(); + Wrench wrenchComputed; + Vector2 contactParams; + contactParams(0) = springCoeff; + contactParams(1) = damperCoeff; + toEigen(wrenchComputed.getLinearVec3()) = toEigen(regressor).topRows<3>() * toEigen(contactParams); + toEigen(wrenchComputed.getAngularVec3()) = toEigen(regressor).bottomRows<3>() * toEigen(contactParams); + + double tollerance = 1e-7; + checkVectorAreEqual(wrenchComputed.getLinearVec3(), + model.getContactWrench().getLinearVec3(), + tollerance); + checkVectorAreEqual(wrenchComputed.getAngularVec3(), + model.getContactWrench().getAngularVec3(), + tollerance); + } + + SECTION("Test contact dynamics") + { + // Instantiate the acceleration + SpatialAcc acceleration; + for (unsigned int i = 0; i < acceleration.size(); i++) + acceleration(i) = 1; + + // define numerical step + double numericalDerivStep = 1e-6; + + // compute the numerical derivative + Transform world_T_link_prev, world_T_link_next; + Twist linkVelocity_prev, linkVelocity_next; + + Position world_T_link_nextPos, world_T_link_prevPos; + Rotation world_T_link_nextRotation, world_T_link_prevRotation; + + // the propagation of the transformation is based under the assumption of twists written in + // mixed representation + + // Position propagation + toEigen(world_T_link_prevPos) + = toEigen(world_T_link.getPosition()) + - toEigen(linkVelocity.getLinearVec3()) * numericalDerivStep; + + toEigen(world_T_link_nextPos) + = toEigen(world_T_link.getPosition()) + + toEigen(linkVelocity.getLinearVec3()) * numericalDerivStep; + + AngularMotionVector3 linkUnitRotation; + + // Rotation propagation + // R(t - dt) = exp(-skew(omega) dt) R(t) (this is valid for MIXED only and CONSTANT angular + // velocity) + toEigen(linkUnitRotation) = -toEigen(linkVelocity.getAngularVec3()) * numericalDerivStep; + world_T_link_prevRotation = linkUnitRotation.exp() * world_T_link.getRotation(); + + // R(t + dt) = exp(skew(omega) dt) R(t) (this is valid for MIXED only and CONSTANT angular + // velocity) + toEigen(linkUnitRotation) = toEigen(linkVelocity.getAngularVec3()) * numericalDerivStep; + world_T_link_nextRotation = linkUnitRotation.exp() * world_T_link.getRotation(); + + // update the transformations + world_T_link_prev.setPosition(world_T_link_prevPos); + world_T_link_prev.setRotation(world_T_link_prevRotation); + + world_T_link_next.setPosition(world_T_link_nextPos); + world_T_link_next.setRotation(world_T_link_nextRotation); + + // Velocity (linear and angular) propagation + toEigen(linkVelocity_prev.getLinearVec3()) + = toEigen(linkVelocity.getLinearVec3()) + - toEigen(acceleration.getLinearVec3()) * numericalDerivStep; + toEigen(linkVelocity_prev.getAngularVec3()) + = toEigen(linkVelocity.getAngularVec3()) + - toEigen(acceleration.getAngularVec3()) * numericalDerivStep; + + toEigen(linkVelocity_next.getLinearVec3()) + = toEigen(linkVelocity.getLinearVec3()) + + toEigen(acceleration.getLinearVec3()) * numericalDerivStep; + toEigen(linkVelocity_next.getAngularVec3()) + = toEigen(linkVelocity.getAngularVec3()) + + toEigen(acceleration.getAngularVec3()) * numericalDerivStep; + + Vector6 contactWrenchRate; + toEigen(contactWrenchRate) = toEigen(model.getAutonomousDynamics()) + + toEigen(model.getControlMatrix()) * toEigen(acceleration); + + model.setState(linkVelocity_prev, world_T_link_prev); + model.setNullForceTransform(nullForceTransform); + + Wrench contactWrench_prev = model.getContactWrench(); + + model.setState(linkVelocity_next, world_T_link_next); + model.setNullForceTransform(nullForceTransform); + + Wrench contactWrench_next = model.getContactWrench(); + + + // Evaluate the numerical rate of change + Vector6 contactWrenchRateNumerical; + toEigen(contactWrenchRateNumerical) + = (toEigen(contactWrench_next) - toEigen(contactWrench_prev)) + / (2 * numericalDerivStep); + + double tollerance = 1e-4; + checkVectorAreEqual(contactWrenchRateNumerical, contactWrenchRate, tollerance); + } +}