Skip to content

Commit

Permalink
Implement GlobalCoPEvaluator in contact Component (#745)
Browse files Browse the repository at this point in the history
  • Loading branch information
GiulioRomualdi authored Oct 30, 2023
1 parent 85f8928 commit c4ec6d6
Show file tree
Hide file tree
Showing 13 changed files with 552 additions and 4 deletions.
2 changes: 2 additions & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@ All notable changes to this project are documented in this file.
## [Unreleased]
- Added the ``DistanceTask`` and ``GravityTask`` to the IK (https://github.com/ami-iit/bipedal-locomotion-framework/pull/717)
- Add the possibility to control a subset of linear coordinates in `TSID::SE3Task` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/738)
- Implement `GlobalCoPEvaluator` in `Contacts` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/745)
- Implement `Wrench::getLocalCoP()` method (https://github.com/ami-iit/bipedal-locomotion-framework/pull/745)

### Added
- Add the possibility to control a subset of coordinates in `TSID::CoMTask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/724, https://github.com/ami-iit/bipedal-locomotion-framework/pull/727)
Expand Down
4 changes: 2 additions & 2 deletions bindings/python/Contacts/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -8,8 +8,8 @@ if(TARGET BipedalLocomotion::Contacts AND TARGET BipedalLocomotion::ContactDetec

add_bipedal_locomotion_python_module(
NAME ContactsBindings
SOURCES src/Contacts.cpp src/ContactDetectors.cpp src/Module.cpp
HEADERS ${H_PREFIX}/Contacts.h ${H_PREFIX}/ContactDetectors.h ${H_PREFIX}/Module.h
SOURCES src/Contacts.cpp src/ContactDetectors.cpp src/GlobalCoPEvaluator.cpp src/Module.cpp
HEADERS ${H_PREFIX}/Contacts.h ${H_PREFIX}/ContactDetectors.h ${H_PREFIX}/GlobalCoPEvaluator.h ${H_PREFIX}/Module.h
LINK_LIBRARIES BipedalLocomotion::Contacts BipedalLocomotion::ContactDetectors
TESTS tests/test_contact.py tests/test_fixed_foot_detector.py tests/test_schmitt_trigger_detector.py)

Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
/**
* @file GlobaCoPEvaluator.h
* @authors Giulio Romualdi
* @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* distributed under the terms of the BSD-3-Clause license.
*/

#ifndef BIPEDAL_LOCOMOTION_BINDINGS_CONTACTS_GLOBAL_COP_EVALUATOR_H
#define BIPEDAL_LOCOMOTION_BINDINGS_CONTACTS_GLOBAL_COP_EVALUATOR_H

#include <pybind11/pybind11.h>

namespace BipedalLocomotion
{
namespace bindings
{
namespace Contacts
{

void CreateGlobalCoPEvaluator(pybind11::module& module);

} // namespace Contacts
} // namespace bindings
} // namespace BipedalLocomotion

#endif // BIPEDAL_LOCOMOTION_BINDINGS_CONTACTS_GLOBAL_COP_EVALUATOR_H
41 changes: 41 additions & 0 deletions bindings/python/Contacts/src/GlobalCoPEvaluator.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,41 @@
/**
* @file GlobalCoPEvaluator.cpp
* @authors Giulio Romualdi
* @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* distributed under the terms of the BSD-3-Clause license.
*/

#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include <BipedalLocomotion/Contacts/GlobalCoPEvaluator.h>

#include <BipedalLocomotion/bindings/Contacts/GlobalCoPEvaluator.h>
#include <BipedalLocomotion/bindings/System/Advanceable.h>

namespace BipedalLocomotion
{
namespace bindings
{
namespace Contacts
{

void CreateGlobalCoPEvaluator(pybind11::module& module)
{
namespace py = ::pybind11;
namespace Contacts = ::BipedalLocomotion::Contacts;
namespace System = ::BipedalLocomotion::System;

BipedalLocomotion::bindings::System::CreateAdvanceable<std::vector<Contacts::ContactWrench>, //
Eigen::Vector3d>(module,
"GlobalCoPEvaluator");
py::class_<Contacts::GlobalCoPEvaluator,
System::Advanceable<std::vector<Contacts::ContactWrench>, //
Eigen::Vector3d>>(module, "GlobalCoPEvaluator")
.def(py::init());
}

} // namespace Contacts
} // namespace bindings
} // namespace BipedalLocomotion
3 changes: 3 additions & 0 deletions bindings/python/Contacts/src/Module.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@

#include <BipedalLocomotion/bindings/Contacts/ContactDetectors.h>
#include <BipedalLocomotion/bindings/Contacts/Contacts.h>
#include <BipedalLocomotion/bindings/Contacts/GlobalCoPEvaluator.h>
#include <BipedalLocomotion/bindings/Contacts/Module.h>

namespace BipedalLocomotion
Expand All @@ -30,6 +31,8 @@ void CreateModule(pybind11::module& module)
CreateContactDetector(module);
CreateSchmittTriggerDetector(module);
CreateFixedFootDetector(module);

CreateGlobalCoPEvaluator(module);
}
} // namespace Contacts
} // namespace bindings
Expand Down
2 changes: 2 additions & 0 deletions src/Contacts/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,9 @@ if (FRAMEWORK_COMPILE_Contact)
NAME Contacts
SUBDIRECTORIES tests/Contacts
PUBLIC_HEADERS ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h ${H_PREFIX}/ContactPhaseList.h ${H_PREFIX}/ContactListJsonParser.h
${H_PREFIX}/GlobalCoPEvaluator.h
SOURCES src/Contact.cpp src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp src/ContactListJsonParser.cpp
src/GlobalCoPEvaluator.cpp
PUBLIC_LINK_LIBRARIES MANIF::manif BipedalLocomotion::Math BipedalLocomotion::TextLogging
PRIVATE_LINK_LIBRARIES nlohmann_json::nlohmann_json
INSTALLATION_FOLDER Contacts)
Expand Down
2 changes: 1 addition & 1 deletion src/Contacts/include/BipedalLocomotion/Contacts/Contact.h
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,6 @@ struct PlannedContact : ContactBase
*/
struct EstimatedContact : ContactBase
{

/**
* Instant at which the contact state was toggled.
*/
Expand All @@ -130,6 +129,7 @@ struct EstimatedContact : ContactBase
*/
struct ContactWrench : public ContactBase
{
/**< Wrench acting on the contact */
BipedalLocomotion::Math::Wrenchd wrench{BipedalLocomotion::Math::Wrenchd::Zero()};
};

Expand Down
137 changes: 137 additions & 0 deletions src/Contacts/include/BipedalLocomotion/Contacts/GlobalCoPEvaluator.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,137 @@
/**
* @file GlobalCoPEvaluator.h
* @authors Giulio Romualdi
* @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* distributed under the terms of the BSD-3-Clause license.
*/

#ifndef BIPEDAL_LOCOMOTION_CONTACTS_GLOBAL_COP_EVALUATOR_H
#define BIPEDAL_LOCOMOTION_CONTACTS_GLOBAL_COP_EVALUATOR_H

#include <initializer_list>
#include <vector>

#include <Eigen/Dense>

#include <BipedalLocomotion/Contacts/Contact.h>
#include <BipedalLocomotion/System/Advanceable.h>

namespace BipedalLocomotion
{
namespace Contacts
{

/**
* GlobalCoPEvaluator is a class that computes the global CoP given a set of contact wrenches.
* Following P. Sardain and G. Bessonnet, "Forces acting on a biped robot. Center of
* pressure-zero moment point," in IEEE Transactions on Systems, Man, and Cybernetics,
* we defined the global CoP as the weighted average of the CoP of each contact, with the weight
* determined by the normal force of the contact.
* In detail, given \f$n\f$ contacts, with \f$n > 0\f$, the global CoP is defined as
* \f[
* {}^{I} x_{\text{CoP}}^{\text{global}} = \frac{1}{\sum_{i=1}^{n} f_{z,i}} \sum_{i=1}^{n} ({}^I f_{z,i} \; \; x_{\text{CoP},i})
* \f]
* where \f${}^I x_{\text{CoP}}^{\text{global}}\f$ is the global CoP expressed in the inertial frame
* \f$I\f$, \f${}^I f_{z,i}\f$ is the z component of the contact force expressed in the inertial
* frame \f$I\f$, \f${}^I x_{\text{CoP},i}\f$ is the CoP of the \f$i\f$-th contact expressed in the
* inertial frame \f$I\f$.
* The local CoP \f${}^I x_{\text{CoP},i}\f$ can be computed as follows
* \f[
* {}^I x_{\text{CoP},i} = {}^I x_{\text{C},i} + {}^I R_{\text{C},i} \; {}^C x_{\text{CoP},i}
* \f]
* where \f${}^I x_{\text{C},i}\f$ is the contact position in the inertial frame \f$I\f$,
* \f${}^I R_{\text{C},i}\f$ is the rotation matrix from the inertial frame \f$I\f$ to the contact
* frame \f$C\f$, and \f${}^C x_{\text{CoP},i}\f$ is the CoP of the \f$i\f$-th contact expressed in
* the contact frame \f$C\f$ that can be computed as follows
* \f[
* {}^C x_{\text{CoP},i} = \frac{1}{f_{z,i}} \begin{bmatrix} -\mu_{y,i} \\ \mu_{x,i} \\ 0 \end{bmatrix}
* \f]
* with \f$\mu_{x,i}\f$ and \f$\mu_{y,i}\f$ being the x and y component of the moment of the contact
* wrench expressed in the contact frame \f$C\f$.
* @note This class assumes that the contact wrenches stored in Contacts::ContactWrench list are
* expressed in the body frame (left trivialized).
* @note This class assumes that all the contacts are coplanar, that the normal of the common
* plane is the z axis, and that the contact force on the z axis is positive.
* @note In addition to evaluating the CoP, this class checks that at least one contact is active
* and that the CoP is not constant for a given number of iterations.
*/
class GlobalCoPEvaluator
: public BipedalLocomotion::System::Advanceable<std::vector<Contacts::ContactWrench>,
Eigen::Vector3d>
{
public:
// clang-format off
/**
* Initialize the class
* @param handler pointer to a parameter handler
* @note the following parameters are required by the class
* | Parameter Name | Type | Description | Mandatory |
* |:--------------------------:|:----------------:|:------------------------------------------------------------------------------------------------------------:|:---------:|
* | `minimum_normal_force` | `double` | Minimum normal force required to consider a contact active (in N) | Yes |
* | `cop_admissible_limits` | `vector<double>` | 2D vector defining the admissible CoP region, comparing the local CoP x and y to the 1st and 2nd elements | Yes |
* | `constant_cop_tolerance` | `double` | Radius (in m) of a sphere used to considered if the global CoP is constant | Yes |
* | `constant_cop_max_counter` | `int` | Maximum number of samples after which a constant CoP generates an error. If negative the check is disabled. | Yes |
* @return true in case of success, false otherwise.
*/
bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) override;
// clang-format on

/**
* Set the input to the class.
* @param input list containing the contact wrench
* @return true in case of success and false otherwise
* @note This class assumes that the contact wrenches stored in Contacts::ContactWrench list are
* expressed in the body frame (left trivialized).
*/
bool setInput(const std::initializer_list<Contacts::ContactWrench>& input);

/**
* Set the input to the class.
* @param input list containing the contact wrench
* @return true in case of success and false otherwise
* @note This class assumes that the contact wrenches stored in Contacts::ContactWrench list are
* expressed in the body frame (left trivialized).
*/
bool setInput(const std::vector<Contacts::ContactWrench>& input) override;

/**
* Compute the global CoP.
* @return true in case of success and false otherwise
*/
bool advance() override;

/**
* Check if the CoP evaluated by the class is valid.
* @return true if valid, false otherwise.
*/
bool isOutputValid() const override;

/**
* Get the global CoP
* @return a 3D vector containing the position of the global CoP expressed respect to the global
* (inertial) frame.
*/
const Eigen::Vector3d& getOutput() const override;

private:
Eigen::Vector3d m_cop{Eigen::Vector3d::Zero()}; /**< Global CoP position in the inertial frame
*/
std::vector<Contacts::ContactWrench> m_contacts; /**< Vector containing the contacts */
bool m_isInitialized{false}; /**< True if the object is initialized */
bool m_isOutputValid{false}; /**< True if the output is valid */

int m_constantCoPCounter{0}; /**< Counter used to store the number of constant CoP over time */

Eigen::Vector2d m_CoPAdmissibleLimits; /**< Vector containing the local admissible limits for
the CoP */
int m_constantCoPMaxCounter{-1}; /**< Maximum number of samples after which a constant CoP
generates an error */
double m_minimumNormalForce{0.0}; /**< Minimum required contact force */
double m_constantCoPTolerance{0.0}; /**< Radius (in m) of a sphere used to considered if the
global CoP is constant */
};

} // namespace Contacts
} // namespace BipedalLocomotion

#endif // BIPEDAL_LOCOMOTION_CONTACTS_GLOBAL_COP_EVALUATOR_H
Loading

0 comments on commit c4ec6d6

Please sign in to comment.