inverse-dynamics-balance-controller.hh 16.3 KB
Newer Older
andreadelprete's avatar
andreadelprete committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
/*
 * Copyright 2017, Andrea Del Prete, LAAS-CNRS
 *
 * This file is part of sot-torque-control.
 * sot-torque-control is free software: you can redistribute it 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.
 * sot-torque-control is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef __sot_torque_control_inverse_dynamics_balance_controller_H__
#define __sot_torque_control_inverse_dynamics_balance_controller_H__

/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#if defined (WIN32)
#  if defined (inverse_dynamics_balance_controller_EXPORTS)
#    define SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT __declspec(dllexport)
#  else
#    define SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT __declspec(dllimport)
#  endif
#else
#  define SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT
#endif


/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#include <sot/torque_control/signal-helper.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
#include <sot/torque_control/utils/logger.hh>
42
#include <sot/torque_control/common.hh>
andreadelprete's avatar
andreadelprete committed
43
44
45
46
47
48
49
#include <map>
#include "boost/assign.hpp"

/* Pinocchio */
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>

50
51
52
53
54
55
56
#include <tsid/robots/robot-wrapper.hpp>
#include <tsid/solvers/solver-HQP-base.hpp>
#include <tsid/contacts/contact-6d.hpp>
#include <tsid/formulations/inverse-dynamics-formulation-acc-force.hpp>
#include <tsid/tasks/task-com-equality.hpp>
#include <tsid/tasks/task-joint-posture.hpp>
#include <tsid/trajectories/trajectory-euclidian.hpp>
andreadelprete's avatar
andreadelprete committed
57
58
59
60
61
62
63
64
65
66
67
68
69
70

namespace dynamicgraph {
  namespace sot {
    namespace torque_control {

      /* --------------------------------------------------------------------- */
      /* --- CLASS ----------------------------------------------------------- */
      /* --------------------------------------------------------------------- */

      class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceController
	:public::dynamicgraph::Entity
      {
        typedef InverseDynamicsBalanceController EntityClassName;
        DYNAMIC_GRAPH_ENTITY_DECL();
Guilhem Saurel's avatar
Guilhem Saurel committed
71
72

      public:
andreadelprete's avatar
andreadelprete committed
73
74
75
76
77
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        /* --- CONSTRUCTOR ---- */
        InverseDynamicsBalanceController( const std::string & name );

Guilhem Saurel's avatar
Guilhem Saurel committed
78
        void init(const double& dt,
79
		  const std::string& robotRef);
80
        void updateComOffset();
81
82
        void removeRightFootContact(const double& transitionTime);
        void removeLeftFootContact(const double& transitionTime);
83
84
        void addRightFootContact(const double& transitionTime);
        void addLeftFootContact(const double& transitionTime);
85
86
87
88
        void addTaskRightHand(/*const double& transitionTime*/);
        void removeTaskRightHand(const double& transitionTime);
        void addTaskLeftHand(/*const double& transitionTime*/);
        void removeTaskLeftHand(const double& transitionTime);
andreadelprete's avatar
andreadelprete committed
89
90

        /* --- SIGNALS --- */
91
92
93
        DECLARE_SIGNAL_IN(com_ref_pos,                dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(com_ref_vel,                dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(com_ref_acc,                dynamicgraph::Vector);
94
95
96
97
98
99
        DECLARE_SIGNAL_IN(rf_ref_pos,                 dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(rf_ref_vel,                 dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(rf_ref_acc,                 dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(lf_ref_pos,                 dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(lf_ref_vel,                 dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(lf_ref_acc,                 dynamicgraph::Vector);
100
101
102
103
104
105
        DECLARE_SIGNAL_IN(rh_ref_pos,                 dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(rh_ref_vel,                 dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(rh_ref_acc,                 dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(lh_ref_pos,                 dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(lh_ref_vel,                 dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(lh_ref_acc,                 dynamicgraph::Vector);
106
107
108
109
110
111
        DECLARE_SIGNAL_IN(posture_ref_pos,            dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(posture_ref_vel,            dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(posture_ref_acc,            dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(base_orientation_ref_pos,   dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(base_orientation_ref_vel,   dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(base_orientation_ref_acc,   dynamicgraph::Vector);
112
113
        DECLARE_SIGNAL_IN(f_ref_right_foot,           dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(f_ref_left_foot,            dynamicgraph::Vector);
114
115
116
117
118
119
120

        DECLARE_SIGNAL_IN(kp_base_orientation,        dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(kd_base_orientation,        dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(kp_constraints,             dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(kd_constraints,             dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(kp_com,                     dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(kd_com,                     dynamicgraph::Vector);
121
122
        DECLARE_SIGNAL_IN(kp_feet,                    dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(kd_feet,                    dynamicgraph::Vector);
123
124
        DECLARE_SIGNAL_IN(kp_hands,                   dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(kd_hands,                   dynamicgraph::Vector);
125
126
127
128
        DECLARE_SIGNAL_IN(kp_posture,                 dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(kd_posture,                 dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(kp_pos,                     dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(kd_pos,                     dynamicgraph::Vector);
andreadelprete's avatar
andreadelprete committed
129
130

        DECLARE_SIGNAL_IN(w_com,                      double);
131
        DECLARE_SIGNAL_IN(w_feet,                     double);
132
        DECLARE_SIGNAL_IN(w_hands,                    double);
andreadelprete's avatar
andreadelprete committed
133
134
135
136
        DECLARE_SIGNAL_IN(w_posture,                  double);
        DECLARE_SIGNAL_IN(w_base_orientation,         double);
        DECLARE_SIGNAL_IN(w_torques,                  double);
        DECLARE_SIGNAL_IN(w_forces,                   double);
137
        DECLARE_SIGNAL_IN(weight_contact_forces,      dynamicgraph::Vector);
andreadelprete's avatar
andreadelprete committed
138
139

        DECLARE_SIGNAL_IN(mu,                         double);
140
141
        DECLARE_SIGNAL_IN(contact_points,             dynamicgraph::Matrix);
        DECLARE_SIGNAL_IN(contact_normal,             dynamicgraph::Vector);
andreadelprete's avatar
andreadelprete committed
142
        DECLARE_SIGNAL_IN(f_min,                      double);
143
144
        DECLARE_SIGNAL_IN(f_max_right_foot,           double);
        DECLARE_SIGNAL_IN(f_max_left_foot,            double);
andreadelprete's avatar
andreadelprete committed
145

146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
        DECLARE_SIGNAL_IN(rotor_inertias,             dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(gear_ratios,                dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(tau_max,                    dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(q_min,                      dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(q_max,                      dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(dq_max,                     dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(ddq_max,                    dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(dt_joint_pos_limits,        double);

        DECLARE_SIGNAL_IN(tau_estimated,              dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(q,                          dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(v,                          dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(wrench_base,                dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(wrench_left_foot,           dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(wrench_right_foot,          dynamicgraph::Vector);

        DECLARE_SIGNAL_IN(active_joints,              dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise
Guilhem Saurel's avatar
Guilhem Saurel committed
163

164
        DECLARE_SIGNAL_OUT(tau_des,                   dynamicgraph::Vector);
165
        DECLARE_SIGNAL_OUT(M,                         dynamicgraph::Matrix);
166
167
168
169
170
171
172
173
        DECLARE_SIGNAL_OUT(dv_des,                    dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(f_des_right_foot,          dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(f_des_left_foot,           dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(zmp_des_right_foot,        dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(zmp_des_left_foot,         dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(zmp_des_right_foot_local,  dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(zmp_des_left_foot_local,   dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(zmp_des,                   dynamicgraph::Vector);
174
        DECLARE_SIGNAL_OUT(zmp_ref,                   dynamicgraph::Vector);
175
176
177
        DECLARE_SIGNAL_OUT(zmp_right_foot,            dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(zmp_left_foot,             dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(zmp,                       dynamicgraph::Vector);
178
        DECLARE_SIGNAL_OUT(com,                       dynamicgraph::Vector);
179
        DECLARE_SIGNAL_OUT(com_vel,                   dynamicgraph::Vector);
180
181
        DECLARE_SIGNAL_OUT(com_acc,                   dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(com_acc_des,               dynamicgraph::Vector);
182
183
184
        DECLARE_SIGNAL_OUT(base_orientation,          dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(right_foot_pos,            dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(left_foot_pos,             dynamicgraph::Vector);
185
186
        DECLARE_SIGNAL_OUT(right_hand_pos,            dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(left_hand_pos,             dynamicgraph::Vector);
187
188
        DECLARE_SIGNAL_OUT(right_foot_vel,            dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(left_foot_vel,             dynamicgraph::Vector);
189
190
        DECLARE_SIGNAL_OUT(right_hand_vel,            dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(left_hand_vel,             dynamicgraph::Vector);
191
192
        DECLARE_SIGNAL_OUT(right_foot_acc,            dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(left_foot_acc,             dynamicgraph::Vector);
193
194
        DECLARE_SIGNAL_OUT(right_hand_acc,            dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(left_hand_acc,             dynamicgraph::Vector);
195
196
        DECLARE_SIGNAL_OUT(right_foot_acc_des,        dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(left_foot_acc_des,         dynamicgraph::Vector);
Guilhem Saurel's avatar
Guilhem Saurel committed
197

andreadelprete's avatar
andreadelprete committed
198
        /// This signal copies active_joints only if it changes from a all false or to an all false value
199
        DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector);
Guilhem Saurel's avatar
Guilhem Saurel committed
200

andreadelprete's avatar
andreadelprete committed
201
202
203
204
205
206
        /* --- COMMANDS --- */
        /* --- ENTITY INHERITANCE --- */
        virtual void display( std::ostream& os ) const;

        void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
        {
207
          getLogger().sendMsg("["+name+"] "+msg, t, file, line);
andreadelprete's avatar
andreadelprete committed
208
        }
209

andreadelprete's avatar
andreadelprete committed
210
      protected:
211

andreadelprete's avatar
andreadelprete committed
212
213
214
215
216
217
        double            m_dt;               /// control loop time period
        double            m_t;
        bool              m_initSucceeded;    /// true if the entity has been successfully initialized
        bool              m_enabled;          /// True if controler is enabled
        bool              m_firstTime;        /// True at the first iteration of the controller

218
219
220
221
        enum ContactState
        {
          DOUBLE_SUPPORT = 0,
          LEFT_SUPPORT = 1,
222
223
224
          LEFT_SUPPORT_TRANSITION = 2,  // transition towards left support
          RIGHT_SUPPORT = 3,
          RIGHT_SUPPORT_TRANSITION = 4
225
226
        };
        ContactState      m_contactState;
227
        double            m_contactTransitionTime;  /// end time of the current contact transition (if any)
228

229
230
        enum RightHandState
        {
Paul Dandignac's avatar
Paul Dandignac committed
231
232
233
          TASK_RIGHT_HAND_ON = 0,
          /*TASK_RIGHT_HAND_TRANSITION = 1,*/
          TASK_RIGHT_HAND_OFF = 1
234
235
236
237
238
        };
        RightHandState         m_rightHandState;

        enum LeftHandState
        {
Paul Dandignac's avatar
Paul Dandignac committed
239
240
241
          TASK_LEFT_HAND_ON = 0,
          /*TASK_LEFT_HAND_TRANSITION = 1,*/
          TASK_LEFT_HAND_OFF = 1
242
243
244
245
        };
        LeftHandState         m_leftHandState;
        /*double            m_handsTransitionTime;*/  /// end time of the current transition (if any)

246
247
248
        int m_frame_id_rf;  /// frame id of right foot
        int m_frame_id_lf;  /// frame id of left foot

249
250
251
        int m_frame_id_rh;  /// frame id of right hand
        int m_frame_id_lh;  /// frame id of left hand

252
253
254
255
256
257
258
259
        /// tsid
        tsid::robots::RobotWrapper *                       m_robot;
        tsid::solvers::SolverHQPBase *           m_hqpSolver;
        tsid::solvers::SolverHQPBase *           m_hqpSolver_60_36_34;
        tsid::solvers::SolverHQPBase *           m_hqpSolver_48_30_17;
        tsid::InverseDynamicsFormulationAccForce * m_invDyn;
        tsid::contacts::Contact6d *                m_contactRF;
        tsid::contacts::Contact6d *                m_contactLF;
260
261
        tsid::contacts::Contact6d *                m_contactRH;
        tsid::contacts::Contact6d *                m_contactLH;
262
263
264
        tsid::tasks::TaskComEquality *             m_taskCom;
        tsid::tasks::TaskSE3Equality *             m_taskRF;
        tsid::tasks::TaskSE3Equality *             m_taskLF;
265
266
        tsid::tasks::TaskSE3Equality *             m_taskRH;
        tsid::tasks::TaskSE3Equality *             m_taskLH;
267
268
269
270
271
272
        tsid::tasks::TaskJointPosture *            m_taskPosture;
        tsid::tasks::TaskJointPosture *            m_taskBlockedJoints;

        tsid::trajectories::TrajectorySample       m_sampleCom;
        tsid::trajectories::TrajectorySample       m_sampleRF;
        tsid::trajectories::TrajectorySample       m_sampleLF;
273
274
        tsid::trajectories::TrajectorySample       m_sampleRH;
        tsid::trajectories::TrajectorySample       m_sampleLH;
275
276
        tsid::trajectories::TrajectorySample       m_samplePosture;

277
278
        double m_w_com;
        double m_w_posture;
279
        double m_w_hands;
280

281
        tsid::math::Vector  m_dv_sot;              /// desired accelerations (sot order)
282
        tsid::math::Vector  m_dv_urdf;             /// desired accelerations (urdf order)
283
284
285
        tsid::math::Vector  m_f;                   /// desired force coefficients (24d)
        tsid::math::Vector6 m_f_RF;                /// desired 6d wrench right foot
        tsid::math::Vector6 m_f_LF;                /// desired 6d wrench left foot
286
        tsid::math::Vector3 m_com_offset;          /// 3d CoM offset
287
288
289
290
291
292
293
294
295
296
297
        tsid::math::Vector3 m_zmp_des_LF;          /// 3d desired zmp left foot
        tsid::math::Vector3 m_zmp_des_RF;          /// 3d desired zmp left foot
        tsid::math::Vector3 m_zmp_des_LF_local;    /// 3d desired zmp left foot expressed in local frame
        tsid::math::Vector3 m_zmp_des_RF_local;    /// 3d desired zmp left foot expressed in local frame
        tsid::math::Vector3 m_zmp_des;             /// 3d desired global zmp
        tsid::math::Vector3 m_zmp_LF;              /// 3d zmp left foot
        tsid::math::Vector3 m_zmp_RF;              /// 3d zmp left foot
        tsid::math::Vector3 m_zmp;                 /// 3d global zmp
        tsid::math::Vector  m_tau_sot;
        tsid::math::Vector  m_q_urdf;
        tsid::math::Vector  m_v_urdf;
andreadelprete's avatar
andreadelprete committed
298

299
300
301
302
303
        typedef se3::Data::Matrix6x Matrix6x;
        Matrix6x m_J_RF;
        Matrix6x m_J_LF;
        Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR;
        Eigen::ColPivHouseholderQR<Matrix6x> m_J_LF_QR;
304
305
        tsid::math::Vector6 m_v_RF_int;
        tsid::math::Vector6 m_v_LF_int;
306

andreadelprete's avatar
andreadelprete committed
307
        unsigned int m_timeLast;
308
        RobotUtil * m_robot_util;
309

andreadelprete's avatar
andreadelprete committed
310
311
312
313
314
315
316
317
      }; // class InverseDynamicsBalanceController
    }    // namespace torque_control
  }      // namespace sot
}        // namespace dynamicgraph



#endif // #ifndef __sot_torque_control_inverse_dynamics_balance_controller_H__