GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/tsid/formulations/inverse-dynamics-formulation-acc-force.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, 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__