GCC Code Coverage Report

Directory: ./
File: include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp
Date: 2024-08-26 20:29:39
Exec Total Coverage
Lines: 1 1 100.0%
Branches: 0 0 -%
Line Branch Exec Source
1 //
2 // Copyright (c) 2017 CNRS, NYU, MPI Tübingen, UNITN
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_acc_force_hpp__
19 #define __invdyn_inverse_dynamics_formulation_acc_force_hpp__
20
21 #include <vector>
22
23 #include "tsid/formulations/contact-level.hpp"
24 #include "tsid/formulations/inverse-dynamics-formulation-base.hpp"
25 #include "tsid/math/constraint-equality.hpp"
26
27 namespace tsid {
28
29 class ContactTransitionInfo {
30 public:
31 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
32
33 double time_start;
34 double time_end;
35 double fMax_start; /// max normal force at time time_start
36 double fMax_end; /// max normal force at time time_end
37 std::shared_ptr<ContactLevel> contactLevel;
38 };
39
40 class InverseDynamicsFormulationAccForce
41 : public InverseDynamicsFormulationBase {
42 public:
43 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
44
45 typedef pinocchio::Data Data;
46 typedef math::Vector Vector;
47 typedef math::Matrix Matrix;
48 typedef math::ConstRefVector ConstRefVector;
49 typedef tasks::TaskBase TaskBase;
50 typedef tasks::TaskMotion TaskMotion;
51 typedef tasks::TaskContactForce TaskContactForce;
52 typedef tasks::TaskActuation TaskActuation;
53 typedef contacts::MeasuredForceBase MeasuredForceBase;
54 typedef solvers::HQPOutput HQPOutput;
55
56 InverseDynamicsFormulationAccForce(const std::string& name,
57 RobotWrapper& robot, bool verbose = false);
58
59 2 virtual ~InverseDynamicsFormulationAccForce() {}
60
61 Data& data();
62
63 unsigned int nVar() const;
64 unsigned int nEq() const;
65 unsigned int nIn() const;
66
67 bool addMotionTask(TaskMotion& task, double weight,
68 unsigned int priorityLevel,
69 double transition_duration = 0.0);
70
71 bool addForceTask(TaskContactForce& task, double weight,
72 unsigned int priorityLevel,
73 double transition_duration = 0.0);
74
75 bool addActuationTask(TaskActuation& task, double weight,
76 unsigned int priorityLevel,
77 double transition_duration = 0.0);
78
79 bool updateTaskWeight(const std::string& task_name, double weight);
80
81 bool addRigidContact(ContactBase& contact, double force_regularization_weight,
82 double motion_weight = 1.0,
83 unsigned int motion_priority_level = 0);
84
85 TSID_DEPRECATED bool addRigidContact(ContactBase& contact);
86
87 bool updateRigidContactWeights(const std::string& contact_name,
88 double force_regularization_weight,
89 double motion_weight = -1.0);
90
91 bool addMeasuredForce(MeasuredForceBase& measuredForce);
92
93 bool removeTask(const std::string& taskName,
94 double transition_duration = 0.0);
95
96 bool removeRigidContact(const std::string& contactName,
97 double transition_duration = 0.0);
98
99 bool removeMeasuredForce(const std::string& measuredForceName);
100
101 const HQPData& computeProblemData(double time, ConstRefVector q,
102 ConstRefVector v);
103
104 const Vector& getActuatorForces(const HQPOutput& sol);
105 const Vector& getAccelerations(const HQPOutput& sol);
106 const Vector& getContactForces(const HQPOutput& sol);
107 Vector getContactForces(const std::string& name, const HQPOutput& sol);
108 bool getContactForces(const std::string& name, const HQPOutput& sol,
109 RefVector f);
110
111 public:
112 template <class TaskLevelPointer>
113 void addTask(TaskLevelPointer task, double weight,
114 unsigned int priorityLevel);
115
116 void resizeHqpData();
117
118 bool removeFromHqpData(const std::string& name);
119
120 bool decodeSolution(const HQPOutput& sol);
121
122 Data m_data;
123 HQPData m_hqpData;
124 std::vector<std::shared_ptr<TaskLevel>> m_taskMotions;
125 std::vector<std::shared_ptr<TaskLevelForce>> m_taskContactForces;
126 std::vector<std::shared_ptr<TaskLevel>> m_taskActuations;
127 std::vector<std::shared_ptr<ContactLevel>> m_contacts;
128 std::vector<std::shared_ptr<MeasuredForceLevel>> m_measuredForces;
129 double m_t; /// time
130 unsigned int m_k; /// number of contact-force variables
131 unsigned int m_v; /// number of acceleration variables
132 unsigned int m_u; /// number of unactuated DoFs
133 unsigned int m_eq; /// number of equality constraints
134 unsigned int m_in; /// number of inequality constraints
135 Matrix m_Jc; /// contact force Jacobian
136 std::shared_ptr<math::ConstraintEquality> m_baseDynamics;
137
138 bool m_solutionDecoded;
139 Vector m_dv;
140 Vector m_f;
141 Vector m_tau;
142
143 Vector h_fext; /// sum of external measured forces
144
145 std::vector<std::shared_ptr<ContactTransitionInfo>> m_contactTransitions;
146 };
147 } // namespace tsid
148 #endif // ifndef __invdyn_inverse_dynamics_formulation_acc_force_hpp__
149