inverse-dynamics-formulation-acc-force.hpp 5.26 KB
Newer Older
1
//
2
// Copyright (c) 2017 CNRS, NYU, MPI Tübingen, UNITN
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
#include "tsid/formulations/inverse-dynamics-formulation-base.hpp"
22
#include "tsid/formulations/contact-level.hpp"
jcarpent's avatar
jcarpent committed
23
#include "tsid/math/constraint-equality.hpp"
24

25
26
#include <vector>

jcarpent's avatar
jcarpent committed
27
namespace tsid
28
29
{

30
31
32
  class ContactTransitionInfo
  {
  public:
33
34
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
35
36
37
38
    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
39
    std::shared_ptr<ContactLevel> contactLevel;
40
41
  };

42
43
44
45
46
47
  class InverseDynamicsFormulationAccForce:
      public InverseDynamicsFormulationBase
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

48
    typedef pinocchio::Data Data;
49
50
51
52
53
54
55
    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;
56
    typedef solvers::HQPOutput HQPOutput;
57
58
59
60
61
62


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

63
    Data & data() ;
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78

    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);

79
80
81
82
    bool addActuationTask(TaskActuation & task,
                          double weight,
                          unsigned int priorityLevel,
                          double transition_duration=0.0);
83

84
85
86
    bool updateTaskWeight(const std::string & task_name,
                          double weight);

87
88
89
90
    bool addRigidContact(ContactBase & contact, double force_regularization_weight,
                         double motion_weight=1.0, unsigned int motion_priority_level=0);

    DEPRECATED bool addRigidContact(ContactBase & contact);
91

92
93
94
95
    bool updateRigidContactWeights(const std::string & contact_name,
                                   double force_regularization_weight,
                                   double motion_weight=-1.0);

96
97
98
99
100
101
    bool removeTask(const std::string & taskName,
                    double transition_duration=0.0);

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

102
    const HQPData & computeProblemData(double time,
103
104
105
                                       ConstRefVector q,
                                       ConstRefVector v);

106
107
108
    const Vector & getActuatorForces(const HQPOutput & sol);
    const Vector & getAccelerations(const HQPOutput & sol);
    const Vector & getContactForces(const HQPOutput & sol);
109
    Vector getContactForces(const std::string & name, const HQPOutput & sol);
110
    bool getContactForces(const std::string & name,
111
                          const HQPOutput & sol,
112
                          RefVector f);
113

114
115
  public:

116
117
    template<class TaskLevelPointer>
    void addTask(TaskLevelPointer task,
118
119
120
121
122
                 double weight,
                 unsigned int priorityLevel);

    void resizeHqpData();

123
124
    bool removeFromHqpData(const std::string & name);

125
    bool decodeSolution(const HQPOutput & sol);
126

127
    Data m_data;
128
    HQPData m_hqpData;
129
130
131
132
    std::vector<std::shared_ptr<TaskLevel> >        m_taskMotions;
    std::vector<std::shared_ptr<TaskLevelForce> >   m_taskContactForces;
    std::vector<std::shared_ptr<TaskLevel> >        m_taskActuations;
    std::vector<std::shared_ptr<ContactLevel> >     m_contacts;
133
134
135
    double m_t;         /// time
    unsigned int m_k;   /// number of contact-force variables
    unsigned int m_v;   /// number of acceleration variables
136
    unsigned int m_u;   /// number of unactuated DoFs
137
138
139
    unsigned int m_eq;  /// number of equality constraints
    unsigned int m_in;  /// number of inequality constraints
    Matrix m_Jc;        /// contact force Jacobian
140
    std::shared_ptr<math::ConstraintEquality> m_baseDynamics;
141

142
    bool m_solutionDecoded;
143
144
145
    Vector m_dv;
    Vector m_f;
    Vector m_tau;
146

147
    std::vector<std::shared_ptr<ContactTransitionInfo> > m_contactTransitions;
148
149
150
151
152
  };

}

#endif // ifndef __invdyn_inverse_dynamics_formulation_acc_force_hpp__