inverse-dynamics-formulation-base.hpp 5.29 KB
Newer Older
1
2
3
//
// Copyright (c) 2017 CNRS
//
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_base_hpp__
#define __invdyn_inverse_dynamics_formulation_base_hpp__

21
#include "tsid/deprecation.hpp"
jcarpent's avatar
jcarpent committed
22
23
24
25
26
27
28
#include "tsid/math/fwd.hpp"
#include "tsid/robots/robot-wrapper.hpp"
#include "tsid/tasks/task-actuation.hpp"
#include "tsid/tasks/task-motion.hpp"
#include "tsid/tasks/task-contact-force.hpp"
#include "tsid/contacts/contact-base.hpp"
#include "tsid/solvers/solver-HQP-base.hpp"
29
30
31

#include <string>

jcarpent's avatar
jcarpent committed
32
namespace tsid
33
34
35
36
37
38
39
40
41
42
{

  ///
  /// \brief Wrapper for a robot based on pinocchio
  ///
  class InverseDynamicsFormulationBase
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

43
    typedef pinocchio::Data Data;
44
45
46
47
48
49
    typedef math::Vector Vector;
    typedef math::RefVector RefVector;
    typedef math::ConstRefVector ConstRefVector;
    typedef tasks::TaskMotion TaskMotion;
    typedef tasks::TaskContactForce TaskContactForce;
    typedef tasks::TaskActuation TaskActuation;
50
    typedef tasks::TaskBase TaskBase;
51
    typedef contacts::ContactBase ContactBase;
52
53
    typedef solvers::HQPData HQPData;
    typedef solvers::HQPOutput HQPOutput;
54
    typedef robots::RobotWrapper RobotWrapper;
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76


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

    virtual const Data & data() const = 0;

    virtual unsigned int nVar() const = 0;
    virtual unsigned int nEq() const = 0;
    virtual unsigned int nIn() const = 0;

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

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

77
78
79
80
    virtual bool addActuationTask(TaskActuation & task,
                                  double weight,
                                  unsigned int priorityLevel,
                                  double transition_duration=0.0) = 0;
81

82
83
84
    virtual bool updateTaskWeight(const std::string & task_name,
                                  double weight) = 0;

85
86
87
88
89
90
91
92
93
94
95
96
97
    /**
     * @brief Add a rigid contact constraint to the model, introducing the associated reaction forces
     *        as problem variables.
     * @param contact The contact constraint to add
     * @param force_regularization_weight The weight of the force regularization task
     * @param motion_weight The weight of the motion task (e.g., zero acceleration of contact points)
     * @param motion_priority_level Priority level of the motion task
     * @return True if everything went fine, false otherwise
     */
    virtual bool addRigidContact(ContactBase & contact, double force_regularization_weight,
                                 double motion_weight=1.0, unsigned int motion_priority_level=0) = 0;

    DEPRECATED virtual bool addRigidContact(ContactBase & contact);
98

99
100
101
102
103
104
105
106
107
108
109
    /**
     * @brief Update the weights associated to the specified contact
     * @param contact_name Name of the contact to update
     * @param force_regularization_weight Weight of the force regularization task, if negative it is not updated
     * @param motion_weight Weight of the motion task, if negative it is not update
     * @return True if everything went fine, false otherwise
     */
    virtual bool updateRigidContactWeights(const std::string & contact_name,
                                           double force_regularization_weight,
                                           double motion_weight=-1.0) = 0;

110
111
112
113
114
115
    virtual bool removeTask(const std::string & taskName,
                            double transition_duration=0.0) = 0;

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

116
    virtual const HQPData & computeProblemData(double time,
117
118
119
                                               ConstRefVector q,
                                               ConstRefVector v) = 0;

120
121
122
    virtual const Vector & getActuatorForces(const HQPOutput & sol) = 0;
    virtual const Vector & getAccelerations(const HQPOutput & sol) = 0;
    virtual const Vector & getContactForces(const HQPOutput & sol) = 0;
123
    virtual bool getContactForces(const std::string & name,
124
                                  const HQPOutput & sol,
125
126
                                  RefVector f) = 0;

127
128
129
130
131
132
133
134
135
  protected:
    std::string m_name;
    RobotWrapper m_robot;
    bool m_verbose;
  };

}

#endif // ifndef __invdyn_inverse_dynamics_formulation_base_hpp__