sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
inverse-dynamics-balance-controller.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2017, Andrea Del Prete, LAAS-CNRS
3  *
4  */
5 
6 #ifndef __sot_torque_control_inverse_dynamics_balance_controller_H__
7 #define __sot_torque_control_inverse_dynamics_balance_controller_H__
8 
9 /* --------------------------------------------------------------------- */
10 /* --- API ------------------------------------------------------------- */
11 /* --------------------------------------------------------------------- */
12 
13 #if defined(WIN32)
14 #if defined(inverse_dynamics_balance_controller_EXPORTS)
15 #define SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT __declspec(dllexport)
16 #else
17 #define SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT __declspec(dllimport)
18 #endif
19 #else
20 #define SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT
21 #endif
22 
23 /* --------------------------------------------------------------------- */
24 /* --- INCLUDE --------------------------------------------------------- */
25 /* --------------------------------------------------------------------- */
26 
27 #include <map>
28 #include "boost/assign.hpp"
29 
30 /* Pinocchio */
31 #include <pinocchio/multibody/model.hpp>
32 #include <pinocchio/parsers/urdf.hpp>
33 
34 #include <tsid/robots/robot-wrapper.hpp>
35 #include <tsid/solvers/solver-HQP-base.hpp>
36 #include <tsid/contacts/contact-6d.hpp>
37 #include <tsid/formulations/inverse-dynamics-formulation-acc-force.hpp>
38 #include <tsid/tasks/task-com-equality.hpp>
39 #include <tsid/tasks/task-joint-posture.hpp>
40 #include <tsid/trajectories/trajectory-euclidian.hpp>
41 
42 /* HELPER */
43 #include <dynamic-graph/signal-helper.h>
44 #include <sot/core/matrix-geometry.hh>
45 #include <sot/core/robot-utils.hh>
46 
48 
49 namespace dynamicgraph {
50 namespace sot {
51 namespace torque_control {
52 
53 /* --------------------------------------------------------------------- */
54 /* --- CLASS ----------------------------------------------------------- */
55 /* --------------------------------------------------------------------- */
56 
59  DYNAMIC_GRAPH_ENTITY_DECL();
60 
61  public:
62  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63 
64  /* --- CONSTRUCTOR ---- */
65  InverseDynamicsBalanceController(const std::string& name);
66 
67  void init(const double& dt, const std::string& robotRef);
68  void updateComOffset();
69  void removeRightFootContact(const double& transitionTime);
70  void removeLeftFootContact(const double& transitionTime);
71  void addRightFootContact(const double& transitionTime);
72  void addLeftFootContact(const double& transitionTime);
73  void addTaskRightHand(/*const double& transitionTime*/);
74  void removeTaskRightHand(const double& transitionTime);
75  void addTaskLeftHand(/*const double& transitionTime*/);
76  void removeTaskLeftHand(const double& transitionTime);
77 
78  /* --- SIGNALS --- */
79  DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector);
80  DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector);
81  DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector);
82  DECLARE_SIGNAL_IN(rf_ref_pos, dynamicgraph::Vector);
83  DECLARE_SIGNAL_IN(rf_ref_vel, dynamicgraph::Vector);
84  DECLARE_SIGNAL_IN(rf_ref_acc, dynamicgraph::Vector);
85  DECLARE_SIGNAL_IN(lf_ref_pos, dynamicgraph::Vector);
86  DECLARE_SIGNAL_IN(lf_ref_vel, dynamicgraph::Vector);
87  DECLARE_SIGNAL_IN(lf_ref_acc, dynamicgraph::Vector);
88  DECLARE_SIGNAL_IN(rh_ref_pos, dynamicgraph::Vector);
89  DECLARE_SIGNAL_IN(rh_ref_vel, dynamicgraph::Vector);
90  DECLARE_SIGNAL_IN(rh_ref_acc, dynamicgraph::Vector);
91  DECLARE_SIGNAL_IN(lh_ref_pos, dynamicgraph::Vector);
92  DECLARE_SIGNAL_IN(lh_ref_vel, dynamicgraph::Vector);
93  DECLARE_SIGNAL_IN(lh_ref_acc, dynamicgraph::Vector);
94  DECLARE_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector);
95  DECLARE_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector);
96  DECLARE_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector);
97  DECLARE_SIGNAL_IN(base_orientation_ref_pos, dynamicgraph::Vector);
98  DECLARE_SIGNAL_IN(base_orientation_ref_vel, dynamicgraph::Vector);
99  DECLARE_SIGNAL_IN(base_orientation_ref_acc, dynamicgraph::Vector);
100  DECLARE_SIGNAL_IN(f_ref_right_foot, dynamicgraph::Vector);
101  DECLARE_SIGNAL_IN(f_ref_left_foot, dynamicgraph::Vector);
102 
103  DECLARE_SIGNAL_IN(kp_base_orientation, dynamicgraph::Vector);
104  DECLARE_SIGNAL_IN(kd_base_orientation, dynamicgraph::Vector);
105  DECLARE_SIGNAL_IN(kp_constraints, dynamicgraph::Vector);
106  DECLARE_SIGNAL_IN(kd_constraints, dynamicgraph::Vector);
107  DECLARE_SIGNAL_IN(kp_com, dynamicgraph::Vector);
108  DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector);
109  DECLARE_SIGNAL_IN(kp_feet, dynamicgraph::Vector);
110  DECLARE_SIGNAL_IN(kd_feet, dynamicgraph::Vector);
111  DECLARE_SIGNAL_IN(kp_hands, dynamicgraph::Vector);
112  DECLARE_SIGNAL_IN(kd_hands, dynamicgraph::Vector);
113  DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector);
114  DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector);
115  DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector);
116  DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector);
117 
118  DECLARE_SIGNAL_IN(w_com, double);
119  DECLARE_SIGNAL_IN(w_feet, double);
120  DECLARE_SIGNAL_IN(w_hands, double);
121  DECLARE_SIGNAL_IN(w_posture, double);
122  DECLARE_SIGNAL_IN(w_base_orientation, double);
123  DECLARE_SIGNAL_IN(w_torques, double);
124  DECLARE_SIGNAL_IN(w_forces, double);
125  DECLARE_SIGNAL_IN(weight_contact_forces, dynamicgraph::Vector);
126 
127  DECLARE_SIGNAL_IN(mu, double);
128  DECLARE_SIGNAL_IN(contact_points, dynamicgraph::Matrix);
129  DECLARE_SIGNAL_IN(contact_normal, dynamicgraph::Vector);
130  DECLARE_SIGNAL_IN(f_min, double);
131  DECLARE_SIGNAL_IN(f_max_right_foot, double);
132  DECLARE_SIGNAL_IN(f_max_left_foot, double);
133 
134  DECLARE_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector);
135  DECLARE_SIGNAL_IN(gear_ratios, dynamicgraph::Vector);
136  DECLARE_SIGNAL_IN(tau_max, dynamicgraph::Vector);
137  DECLARE_SIGNAL_IN(q_min, dynamicgraph::Vector);
138  DECLARE_SIGNAL_IN(q_max, dynamicgraph::Vector);
139  DECLARE_SIGNAL_IN(dq_max, dynamicgraph::Vector);
140  DECLARE_SIGNAL_IN(ddq_max, dynamicgraph::Vector);
141  DECLARE_SIGNAL_IN(dt_joint_pos_limits, double);
142 
143  DECLARE_SIGNAL_IN(tau_estimated, dynamicgraph::Vector);
144  DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
145  DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
146  DECLARE_SIGNAL_IN(wrench_base, dynamicgraph::Vector);
147  DECLARE_SIGNAL_IN(wrench_left_foot, dynamicgraph::Vector);
148  DECLARE_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector);
149 
150  DECLARE_SIGNAL_IN(active_joints, dynamicgraph::Vector);
151 
152  DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector);
153  DECLARE_SIGNAL_OUT(M, dynamicgraph::Matrix);
154  DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector);
155  DECLARE_SIGNAL_OUT(f_des_right_foot, dynamicgraph::Vector);
156  DECLARE_SIGNAL_OUT(f_des_left_foot, dynamicgraph::Vector);
157  DECLARE_SIGNAL_OUT(zmp_des_right_foot, dynamicgraph::Vector);
158  DECLARE_SIGNAL_OUT(zmp_des_left_foot, dynamicgraph::Vector);
159  DECLARE_SIGNAL_OUT(zmp_des_right_foot_local, dynamicgraph::Vector);
160  DECLARE_SIGNAL_OUT(zmp_des_left_foot_local, dynamicgraph::Vector);
161  DECLARE_SIGNAL_OUT(zmp_des, dynamicgraph::Vector);
162  DECLARE_SIGNAL_OUT(zmp_ref, dynamicgraph::Vector);
163  DECLARE_SIGNAL_OUT(zmp_right_foot, dynamicgraph::Vector);
164  DECLARE_SIGNAL_OUT(zmp_left_foot, dynamicgraph::Vector);
165  DECLARE_SIGNAL_OUT(zmp, dynamicgraph::Vector);
166  DECLARE_SIGNAL_OUT(com, dynamicgraph::Vector);
167  DECLARE_SIGNAL_OUT(com_vel, dynamicgraph::Vector);
168  DECLARE_SIGNAL_OUT(com_acc, dynamicgraph::Vector);
169  DECLARE_SIGNAL_OUT(com_acc_des, dynamicgraph::Vector);
170  DECLARE_SIGNAL_OUT(base_orientation, dynamicgraph::Vector);
171  DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector);
172  DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector);
173  DECLARE_SIGNAL_OUT(right_hand_pos, dynamicgraph::Vector);
174  DECLARE_SIGNAL_OUT(left_hand_pos, dynamicgraph::Vector);
175  DECLARE_SIGNAL_OUT(right_foot_vel, dynamicgraph::Vector);
176  DECLARE_SIGNAL_OUT(left_foot_vel, dynamicgraph::Vector);
177  DECLARE_SIGNAL_OUT(right_hand_vel, dynamicgraph::Vector);
178  DECLARE_SIGNAL_OUT(left_hand_vel, dynamicgraph::Vector);
179  DECLARE_SIGNAL_OUT(right_foot_acc, dynamicgraph::Vector);
180  DECLARE_SIGNAL_OUT(left_foot_acc, dynamicgraph::Vector);
181  DECLARE_SIGNAL_OUT(right_hand_acc, dynamicgraph::Vector);
182  DECLARE_SIGNAL_OUT(left_hand_acc, dynamicgraph::Vector);
183  DECLARE_SIGNAL_OUT(right_foot_acc_des, dynamicgraph::Vector);
184  DECLARE_SIGNAL_OUT(left_foot_acc_des, dynamicgraph::Vector);
185 
187  DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector);
188 
189  /* --- COMMANDS --- */
190  /* --- ENTITY INHERITANCE --- */
191  virtual void display(std::ostream& os) const;
192 
193  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
194  logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
195  }
196 
197  protected:
198  double m_dt;
199  double m_t;
201  bool m_enabled;
202  bool m_firstTime;
203 
205  DOUBLE_SUPPORT = 0,
206  LEFT_SUPPORT = 1,
207  LEFT_SUPPORT_TRANSITION = 2, // transition towards left support
208  RIGHT_SUPPORT = 3,
209  RIGHT_SUPPORT_TRANSITION = 4
210  };
213 
215  TASK_RIGHT_HAND_ON = 0,
216  /*TASK_RIGHT_HAND_TRANSITION = 1,*/
217  TASK_RIGHT_HAND_OFF = 1
218  };
220 
222  TASK_LEFT_HAND_ON = 0,
223  /*TASK_LEFT_HAND_TRANSITION = 1,*/
224  TASK_LEFT_HAND_OFF = 1
225  };
227  /*double m_handsTransitionTime;*/
228 
231 
234 
236  tsid::robots::RobotWrapper* m_robot;
237  tsid::solvers::SolverHQPBase* m_hqpSolver;
238  tsid::solvers::SolverHQPBase* m_hqpSolver_60_36_34;
239  tsid::solvers::SolverHQPBase* m_hqpSolver_48_30_17;
240  tsid::InverseDynamicsFormulationAccForce* m_invDyn;
241  tsid::contacts::Contact6d* m_contactRF;
242  tsid::contacts::Contact6d* m_contactLF;
243  tsid::contacts::Contact6d* m_contactRH;
244  tsid::contacts::Contact6d* m_contactLH;
245  tsid::tasks::TaskComEquality* m_taskCom;
246  tsid::tasks::TaskSE3Equality* m_taskRF;
247  tsid::tasks::TaskSE3Equality* m_taskLF;
248  tsid::tasks::TaskSE3Equality* m_taskRH;
249  tsid::tasks::TaskSE3Equality* m_taskLH;
250  tsid::tasks::TaskJointPosture* m_taskPosture;
251  tsid::tasks::TaskJointPosture* m_taskBlockedJoints;
252 
253  tsid::trajectories::TrajectorySample m_sampleCom;
254  tsid::trajectories::TrajectorySample m_sampleRF;
255  tsid::trajectories::TrajectorySample m_sampleLF;
256  tsid::trajectories::TrajectorySample m_sampleRH;
257  tsid::trajectories::TrajectorySample m_sampleLH;
258  tsid::trajectories::TrajectorySample m_samplePosture;
259 
260  double m_w_com;
261  double m_w_posture;
262  double m_w_hands;
263 
264  tsid::math::Vector m_dv_sot;
265  tsid::math::Vector m_dv_urdf;
266  tsid::math::Vector m_f;
278  tsid::math::Vector m_tau_sot;
279  tsid::math::Vector m_q_urdf;
280  tsid::math::Vector m_v_urdf;
281 
282  typedef pinocchio::Data::Matrix6x Matrix6x;
285  Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR;
286  Eigen::ColPivHouseholderQR<Matrix6x> m_J_LF_QR;
289 
290  unsigned int m_timeLast;
291  RobotUtilShrPtr m_robot_util;
292 
293 }; // class InverseDynamicsBalanceController
294 } // namespace torque_control
295 } // namespace sot
296 } // namespace dynamicgraph
297 
298 #endif // #ifndef __sot_torque_control_inverse_dynamics_balance_controller_H__
Eigen::Matrix< double, 3, 1 > Vector3
tsid::math::Vector3 m_zmp_des
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
Eigen::Matrix< double, 6, 1 > Vector6
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
#define SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT
to read text file
Definition: treeview.dox:22