-
Notifications
You must be signed in to change notification settings - Fork 33
/
ContactModel.h
112 lines (93 loc) · 4.26 KB
/
ContactModel.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
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 <memory>
#include <string>
#include <unordered_map>
#include <iDynTree/Core/MatrixFixSize.h>
#include <iDynTree/Core/Transform.h>
#include <iDynTree/Core/Twist.h>
#include <iDynTree/Core/VectorFixSize.h>
#include <iDynTree/Core/Wrench.h>
#include <BipedalLocomotionControllers/ParametersHandler/IParametersHandler.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 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;
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 */
/**
* 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;
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::initialize
* @return true/false in case of success/failure
*/
virtual bool initialize(std::weak_ptr<ParametersHandler::IParametersHandler> handler) = 0;
/**
* 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.
* @note the meaning of the parameters may depend on the particular implementation. An example
* can be found in BipedalLocomotionControllers::ContactModel::ContinuousContactmodel::setState
* @retun true/false in case of success/failure
*/
virtual void setState(const iDynTree::Twist& twist,
const iDynTree::Transform& transform,
const iDynTree::Transform& nullForceTransform) = 0;
};
} // namespace ContactModels
} // namespace BipedalLocomotionControllers
#endif // BIPEDAL_LOCOMOTION_CONTROLLERS_CONTACT_MODELS_CONTACT_MODEL_H