GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/tsid/formulations/inverse-dynamics-formulation-base.hpp Lines: 1 2 50.0 %
Date: 2024-02-02 08:47:34 Branches: 0 0 - %

Line Branch Exec Source
1
//
2
// Copyright (c) 2017 CNRS
3
//
4
// This file is part of tsid
5
// tsid is free software: you can redistribute it
6
// and/or modify it under the terms of the GNU Lesser General Public
7
// License as published by the Free Software Foundation, either version
8
// 3 of the License, or (at your option) any later version.
9
// tsid is distributed in the hope that it will be
10
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12
// General Lesser Public License for more details. You should have
13
// received a copy of the GNU Lesser General Public License along with
14
// tsid If not, see
15
// <http://www.gnu.org/licenses/>.
16
//
17
18
#ifndef __invdyn_inverse_dynamics_formulation_base_hpp__
19
#define __invdyn_inverse_dynamics_formulation_base_hpp__
20
21
#include "tsid/deprecated.hh"
22
#include "tsid/math/fwd.hpp"
23
#include "tsid/robots/robot-wrapper.hpp"
24
#include "tsid/tasks/task-actuation.hpp"
25
#include "tsid/tasks/task-motion.hpp"
26
#include "tsid/tasks/task-contact-force.hpp"
27
#include "tsid/contacts/contact-base.hpp"
28
#include "tsid/contacts/measured-force-base.hpp"
29
#include "tsid/solvers/solver-HQP-base.hpp"
30
31
#include <string>
32
33
namespace tsid {
34
35
struct TaskLevel {
36
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
37
38
  tasks::TaskBase& task;
39
  std::shared_ptr<math::ConstraintBase> constraint;
40
  unsigned int priority;
41
42
  TaskLevel(tasks::TaskBase& task, unsigned int priority);
43
};
44
45
struct TaskLevelForce {
46
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
47
48
  tasks::TaskContactForce& task;
49
  std::shared_ptr<math::ConstraintBase> constraint;
50
  unsigned int priority;
51
52
  TaskLevelForce(tasks::TaskContactForce& task, unsigned int priority);
53
};
54
55
struct MeasuredForceLevel {
56
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
57
58
  contacts::MeasuredForceBase& measuredForce;
59
60
  MeasuredForceLevel(contacts::MeasuredForceBase& measuredForce);
61
};
62
63
///
64
/// \brief Wrapper for a robot based on pinocchio
65
///
66
class InverseDynamicsFormulationBase {
67
 public:
68
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
69
70
  typedef pinocchio::Data Data;
71
  typedef math::Vector Vector;
72
  typedef math::RefVector RefVector;
73
  typedef math::ConstRefVector ConstRefVector;
74
  typedef tasks::TaskMotion TaskMotion;
75
  typedef tasks::TaskContactForce TaskContactForce;
76
  typedef tasks::TaskActuation TaskActuation;
77
  typedef tasks::TaskBase TaskBase;
78
  typedef contacts::MeasuredForceBase MeasuredForceBase;
79
  typedef contacts::ContactBase ContactBase;
80
  typedef solvers::HQPData HQPData;
81
  typedef solvers::HQPOutput HQPOutput;
82
  typedef robots::RobotWrapper RobotWrapper;
83
84
  InverseDynamicsFormulationBase(const std::string& name, RobotWrapper& robot,
85
                                 bool verbose = false);
86
87
2
  virtual ~InverseDynamicsFormulationBase() {}
88
89
  virtual Data& data() = 0;
90
91
  virtual unsigned int nVar() const = 0;
92
  virtual unsigned int nEq() const = 0;
93
  virtual unsigned int nIn() const = 0;
94
95
  virtual bool addMotionTask(TaskMotion& task, double weight,
96
                             unsigned int priorityLevel,
97
                             double transition_duration = 0.0) = 0;
98
99
  virtual bool addForceTask(TaskContactForce& task, double weight,
100
                            unsigned int priorityLevel,
101
                            double transition_duration = 0.0) = 0;
102
103
  virtual bool addActuationTask(TaskActuation& task, double weight,
104
                                unsigned int priorityLevel,
105
                                double transition_duration = 0.0) = 0;
106
107
  virtual bool updateTaskWeight(const std::string& task_name,
108
                                double weight) = 0;
109
110
  /**
111
   * @brief Add a rigid contact constraint to the model, introducing the
112
   * associated reaction forces as problem variables.
113
   * @param contact The contact constraint to add
114
   * @param force_regularization_weight The weight of the force regularization
115
   * task
116
   * @param motion_weight The weight of the motion task (e.g., zero acceleration
117
   * of contact points)
118
   * @param motion_priority_level Priority level of the motion task
119
   * @return True if everything went fine, false otherwise
120
   */
121
  virtual bool addRigidContact(ContactBase& contact,
122
                               double force_regularization_weight,
123
                               double motion_weight = 1.0,
124
                               unsigned int motion_priority_level = 0) = 0;
125
126
  TSID_DEPRECATED virtual bool addRigidContact(ContactBase& contact);
127
128
  /**
129
   * @brief Update the weights associated to the specified contact
130
   * @param contact_name Name of the contact to update
131
   * @param force_regularization_weight Weight of the force regularization task,
132
   * if negative it is not updated
133
   * @param motion_weight Weight of the motion task, if negative it is not
134
   * update
135
   * @return True if everything went fine, false otherwise
136
   */
137
  virtual bool updateRigidContactWeights(const std::string& contact_name,
138
                                         double force_regularization_weight,
139
                                         double motion_weight = -1.0) = 0;
140
141
  virtual bool addMeasuredForce(MeasuredForceBase& measuredForce) = 0;
142
143
  virtual bool removeTask(const std::string& taskName,
144
                          double transition_duration = 0.0) = 0;
145
146
  virtual bool removeRigidContact(const std::string& contactName,
147
                                  double transition_duration = 0.0) = 0;
148
149
  virtual bool removeMeasuredForce(const std::string& measuredForceName) = 0;
150
151
  virtual const HQPData& computeProblemData(double time, ConstRefVector q,
152
                                            ConstRefVector v) = 0;
153
154
  virtual const Vector& getActuatorForces(const HQPOutput& sol) = 0;
155
  virtual const Vector& getAccelerations(const HQPOutput& sol) = 0;
156
  virtual const Vector& getContactForces(const HQPOutput& sol) = 0;
157
  virtual bool getContactForces(const std::string& name, const HQPOutput& sol,
158
                                RefVector f) = 0;
159
160
 protected:
161
  std::string m_name;
162
  RobotWrapper m_robot;
163
  bool m_verbose;
164
};
165
166
}  // namespace tsid
167
168
#endif  // ifndef __invdyn_inverse_dynamics_formulation_base_hpp__