GCC Code Coverage Report

Directory: ./
File: include/tsid/formulations/inverse-dynamics-formulation-base.hpp
Date: 2025-05-10 01:12:46
Exec Total Coverage
Lines: 1 1 100.0%
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 4 virtual ~InverseDynamicsFormulationBase() = default;
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__
169