inverse-dynamics-formulation-acc-force.hpp 5.72 KB
Newer Older
1
//
2
// Copyright (c) 2017 CNRS, NYU, MPI Tübingen
3
//
jcarpent's avatar
jcarpent committed
4
5
// This file is part of tsid
// tsid is free software: you can redistribute it
6
7
8
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
jcarpent's avatar
jcarpent committed
9
// tsid is distributed in the hope that it will be
10
11
12
13
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
jcarpent's avatar
jcarpent committed
14
// tsid If not, see
15
16
17
18
19
20
// <http://www.gnu.org/licenses/>.
//

#ifndef __invdyn_inverse_dynamics_formulation_acc_force_hpp__
#define __invdyn_inverse_dynamics_formulation_acc_force_hpp__

jcarpent's avatar
jcarpent committed
21
22
#include "tsid/formulations/inverse-dynamics-formulation-base.hpp"
#include "tsid/math/constraint-equality.hpp"
23

24
25
#include <vector>

jcarpent's avatar
jcarpent committed
26
namespace tsid
27
28
29
30
31
{

  class TaskLevel
  {
  public:
32
33
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
34
35
    tasks::TaskBase & task;
    math::ConstraintBase * constraint;
36
//    double weight;
37
38
    unsigned int priority;

39
    TaskLevel(tasks::TaskBase & task,
40
41
42
43
44
45
              unsigned int priority);
  };

  class ContactLevel
  {
  public:
46
47
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
48
49
50
51
    contacts::ContactBase & contact;
    math::ConstraintBase * motionConstraint;
    math::ConstraintInequality * forceConstraint;
    math::ConstraintEquality * forceRegTask;
52
53
    unsigned int index; /// index of 1st element of associated force variable in the force vector

54
    ContactLevel(contacts::ContactBase & contact);
55
56
  };

57
58
59
  class ContactTransitionInfo
  {
  public:
60
61
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
62
63
64
65
66
67
68
    double time_start;
    double time_end;
    double fMax_start;  /// max normal force at time time_start
    double fMax_end;    /// max normal force at time time_end
    ContactLevel * contactLevel;
  };

69
70
71
72
73
74
  class InverseDynamicsFormulationAccForce:
      public InverseDynamicsFormulationBase
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

75
    typedef pinocchio::Data Data;
76
77
78
79
80
81
82
    typedef math::Vector Vector;
    typedef math::Matrix Matrix;
    typedef math::ConstRefVector ConstRefVector;
    typedef tasks::TaskBase TaskBase;
    typedef tasks::TaskMotion TaskMotion;
    typedef tasks::TaskContactForce TaskContactForce;
    typedef tasks::TaskActuation TaskActuation;
83
    typedef solvers::HQPOutput HQPOutput;
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105


    InverseDynamicsFormulationAccForce(const std::string & name,
                                       RobotWrapper & robot,
                                       bool verbose=false);

    const Data & data() const;

    unsigned int nVar() const;
    unsigned int nEq() const;
    unsigned int nIn() const;

    bool addMotionTask(TaskMotion & task,
                       double weight,
                       unsigned int priorityLevel,
                       double transition_duration=0.0);

    bool addForceTask(TaskContactForce & task,
                      double weight,
                      unsigned int priorityLevel,
                      double transition_duration=0.0);

106
107
108
109
    bool addActuationTask(TaskActuation & task,
                          double weight,
                          unsigned int priorityLevel,
                          double transition_duration=0.0);
110

111
112
113
    bool updateTaskWeight(const std::string & task_name,
                          double weight);

114
115
116
117
    bool addRigidContact(ContactBase & contact, double force_regularization_weight,
                         double motion_weight=1.0, unsigned int motion_priority_level=0);

    DEPRECATED bool addRigidContact(ContactBase & contact);
118

119
120
121
122
    bool updateRigidContactWeights(const std::string & contact_name,
                                   double force_regularization_weight,
                                   double motion_weight=-1.0);

123
124
125
126
127
128
    bool removeTask(const std::string & taskName,
                    double transition_duration=0.0);

    bool removeRigidContact(const std::string & contactName,
                            double transition_duration=0.0);

129
    const HQPData & computeProblemData(double time,
130
131
132
                                       ConstRefVector q,
                                       ConstRefVector v);

133
134
135
    const Vector & getActuatorForces(const HQPOutput & sol);
    const Vector & getAccelerations(const HQPOutput & sol);
    const Vector & getContactForces(const HQPOutput & sol);
136
    Vector getContactForces(const std::string & name, const HQPOutput & sol);
137
    bool getContactForces(const std::string & name,
138
                          const HQPOutput & sol,
139
                          RefVector f);
140

141
142
143
144
145
146
147
148
  public:

    void addTask(TaskLevel* task,
                 double weight,
                 unsigned int priorityLevel);

    void resizeHqpData();

149
150
    bool removeFromHqpData(const std::string & name);

151
    bool decodeSolution(const HQPOutput & sol);
152

153
    Data m_data;
154
    HQPData m_hqpData;
155
156
157
158
159
160
161
    std::vector<TaskLevel*>     m_taskMotions;
    std::vector<TaskLevel*>     m_taskContactForces;
    std::vector<TaskLevel*>     m_taskActuations;
    std::vector<ContactLevel*>   m_contacts;
    double m_t;         /// time
    unsigned int m_k;   /// number of contact-force variables
    unsigned int m_v;   /// number of acceleration variables
162
    unsigned int m_u;   /// number of unactuated DoFs
163
164
165
    unsigned int m_eq;  /// number of equality constraints
    unsigned int m_in;  /// number of inequality constraints
    Matrix m_Jc;        /// contact force Jacobian
166
    math::ConstraintEquality m_baseDynamics;
167

168
    bool m_solutionDecoded;
169
170
171
    Vector m_dv;
    Vector m_f;
    Vector m_tau;
172
173

    std::vector<ContactTransitionInfo*> m_contactTransitions;
174
175
176
177
178
  };

}

#endif // ifndef __invdyn_inverse_dynamics_formulation_acc_force_hpp__