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 |
|
|
|
29 |
|
|
#include "boost/assign.hpp" |
30 |
|
|
|
31 |
|
|
/* Pinocchio */ |
32 |
|
|
#include <pinocchio/multibody/model.hpp> |
33 |
|
|
#include <pinocchio/parsers/urdf.hpp> |
34 |
|
|
#include <tsid/contacts/contact-6d.hpp> |
35 |
|
|
#include <tsid/formulations/inverse-dynamics-formulation-acc-force.hpp> |
36 |
|
|
#include <tsid/robots/robot-wrapper.hpp> |
37 |
|
|
#include <tsid/solvers/solver-HQP-base.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 |
|
|
|
45 |
|
|
#include <sot/core/matrix-geometry.hh> |
46 |
|
|
#include <sot/core/robot-utils.hh> |
47 |
|
|
#include <sot/torque_control/utils/vector-conversions.hh> |
48 |
|
|
|
49 |
|
|
namespace dynamicgraph { |
50 |
|
|
namespace sot { |
51 |
|
|
namespace torque_control { |
52 |
|
|
|
53 |
|
|
/* --------------------------------------------------------------------- */ |
54 |
|
|
/* --- CLASS ----------------------------------------------------------- */ |
55 |
|
|
/* --------------------------------------------------------------------- */ |
56 |
|
|
|
57 |
|
|
class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT |
58 |
|
|
InverseDynamicsBalanceController : public ::dynamicgraph::Entity { |
59 |
|
|
typedef InverseDynamicsBalanceController EntityClassName; |
60 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
61 |
|
|
|
62 |
|
|
public: |
63 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
64 |
|
|
|
65 |
|
|
/* --- CONSTRUCTOR ---- */ |
66 |
|
|
InverseDynamicsBalanceController(const std::string& name); |
67 |
|
|
|
68 |
|
|
void init(const double& dt, const std::string& robotRef); |
69 |
|
|
void updateComOffset(); |
70 |
|
|
void removeRightFootContact(const double& transitionTime); |
71 |
|
|
void removeLeftFootContact(const double& transitionTime); |
72 |
|
|
void addRightFootContact(const double& transitionTime); |
73 |
|
|
void addLeftFootContact(const double& transitionTime); |
74 |
|
|
void addTaskRightHand(/*const double& transitionTime*/); |
75 |
|
|
void removeTaskRightHand(const double& transitionTime); |
76 |
|
|
void addTaskLeftHand(/*const double& transitionTime*/); |
77 |
|
|
void removeTaskLeftHand(const double& transitionTime); |
78 |
|
|
|
79 |
|
|
/* --- SIGNALS --- */ |
80 |
|
|
DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector); |
81 |
|
|
DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector); |
82 |
|
|
DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector); |
83 |
|
|
DECLARE_SIGNAL_IN(rf_ref_pos, dynamicgraph::Vector); |
84 |
|
|
DECLARE_SIGNAL_IN(rf_ref_vel, dynamicgraph::Vector); |
85 |
|
|
DECLARE_SIGNAL_IN(rf_ref_acc, dynamicgraph::Vector); |
86 |
|
|
DECLARE_SIGNAL_IN(lf_ref_pos, dynamicgraph::Vector); |
87 |
|
|
DECLARE_SIGNAL_IN(lf_ref_vel, dynamicgraph::Vector); |
88 |
|
|
DECLARE_SIGNAL_IN(lf_ref_acc, dynamicgraph::Vector); |
89 |
|
|
DECLARE_SIGNAL_IN(rh_ref_pos, dynamicgraph::Vector); |
90 |
|
|
DECLARE_SIGNAL_IN(rh_ref_vel, dynamicgraph::Vector); |
91 |
|
|
DECLARE_SIGNAL_IN(rh_ref_acc, dynamicgraph::Vector); |
92 |
|
|
DECLARE_SIGNAL_IN(lh_ref_pos, dynamicgraph::Vector); |
93 |
|
|
DECLARE_SIGNAL_IN(lh_ref_vel, dynamicgraph::Vector); |
94 |
|
|
DECLARE_SIGNAL_IN(lh_ref_acc, dynamicgraph::Vector); |
95 |
|
|
DECLARE_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector); |
96 |
|
|
DECLARE_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector); |
97 |
|
|
DECLARE_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector); |
98 |
|
|
DECLARE_SIGNAL_IN(base_orientation_ref_pos, dynamicgraph::Vector); |
99 |
|
|
DECLARE_SIGNAL_IN(base_orientation_ref_vel, dynamicgraph::Vector); |
100 |
|
|
DECLARE_SIGNAL_IN(base_orientation_ref_acc, dynamicgraph::Vector); |
101 |
|
|
DECLARE_SIGNAL_IN(f_ref_right_foot, dynamicgraph::Vector); |
102 |
|
|
DECLARE_SIGNAL_IN(f_ref_left_foot, dynamicgraph::Vector); |
103 |
|
|
|
104 |
|
|
DECLARE_SIGNAL_IN(kp_base_orientation, dynamicgraph::Vector); |
105 |
|
|
DECLARE_SIGNAL_IN(kd_base_orientation, dynamicgraph::Vector); |
106 |
|
|
DECLARE_SIGNAL_IN(kp_constraints, dynamicgraph::Vector); |
107 |
|
|
DECLARE_SIGNAL_IN(kd_constraints, dynamicgraph::Vector); |
108 |
|
|
DECLARE_SIGNAL_IN(kp_com, dynamicgraph::Vector); |
109 |
|
|
DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector); |
110 |
|
|
DECLARE_SIGNAL_IN(kp_feet, dynamicgraph::Vector); |
111 |
|
|
DECLARE_SIGNAL_IN(kd_feet, dynamicgraph::Vector); |
112 |
|
|
DECLARE_SIGNAL_IN(kp_hands, dynamicgraph::Vector); |
113 |
|
|
DECLARE_SIGNAL_IN(kd_hands, dynamicgraph::Vector); |
114 |
|
|
DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector); |
115 |
|
|
DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector); |
116 |
|
|
DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector); |
117 |
|
|
DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector); |
118 |
|
|
|
119 |
|
|
DECLARE_SIGNAL_IN(w_com, double); |
120 |
|
|
DECLARE_SIGNAL_IN(w_feet, double); |
121 |
|
|
DECLARE_SIGNAL_IN(w_hands, double); |
122 |
|
|
DECLARE_SIGNAL_IN(w_posture, double); |
123 |
|
|
DECLARE_SIGNAL_IN(w_base_orientation, double); |
124 |
|
|
DECLARE_SIGNAL_IN(w_torques, double); |
125 |
|
|
DECLARE_SIGNAL_IN(w_forces, double); |
126 |
|
|
DECLARE_SIGNAL_IN(weight_contact_forces, dynamicgraph::Vector); |
127 |
|
|
|
128 |
|
|
DECLARE_SIGNAL_IN(mu, double); |
129 |
|
|
DECLARE_SIGNAL_IN(contact_points, dynamicgraph::Matrix); |
130 |
|
|
DECLARE_SIGNAL_IN(contact_normal, dynamicgraph::Vector); |
131 |
|
|
DECLARE_SIGNAL_IN(f_min, double); |
132 |
|
|
DECLARE_SIGNAL_IN(f_max_right_foot, double); |
133 |
|
|
DECLARE_SIGNAL_IN(f_max_left_foot, double); |
134 |
|
|
|
135 |
|
|
DECLARE_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector); |
136 |
|
|
DECLARE_SIGNAL_IN(gear_ratios, dynamicgraph::Vector); |
137 |
|
|
DECLARE_SIGNAL_IN(tau_max, dynamicgraph::Vector); |
138 |
|
|
DECLARE_SIGNAL_IN(q_min, dynamicgraph::Vector); |
139 |
|
|
DECLARE_SIGNAL_IN(q_max, dynamicgraph::Vector); |
140 |
|
|
DECLARE_SIGNAL_IN(dq_max, dynamicgraph::Vector); |
141 |
|
|
DECLARE_SIGNAL_IN(ddq_max, dynamicgraph::Vector); |
142 |
|
|
DECLARE_SIGNAL_IN(dt_joint_pos_limits, double); |
143 |
|
|
|
144 |
|
|
DECLARE_SIGNAL_IN(tau_estimated, dynamicgraph::Vector); |
145 |
|
|
DECLARE_SIGNAL_IN(q, dynamicgraph::Vector); |
146 |
|
|
DECLARE_SIGNAL_IN(v, dynamicgraph::Vector); |
147 |
|
|
DECLARE_SIGNAL_IN(wrench_base, dynamicgraph::Vector); |
148 |
|
|
DECLARE_SIGNAL_IN(wrench_left_foot, dynamicgraph::Vector); |
149 |
|
|
DECLARE_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector); |
150 |
|
|
|
151 |
|
|
DECLARE_SIGNAL_IN( |
152 |
|
|
active_joints, |
153 |
|
|
dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise |
154 |
|
|
|
155 |
|
|
DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector); |
156 |
|
|
DECLARE_SIGNAL_OUT(M, dynamicgraph::Matrix); |
157 |
|
|
DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector); |
158 |
|
|
DECLARE_SIGNAL_OUT(f_des_right_foot, dynamicgraph::Vector); |
159 |
|
|
DECLARE_SIGNAL_OUT(f_des_left_foot, dynamicgraph::Vector); |
160 |
|
|
DECLARE_SIGNAL_OUT(zmp_des_right_foot, dynamicgraph::Vector); |
161 |
|
|
DECLARE_SIGNAL_OUT(zmp_des_left_foot, dynamicgraph::Vector); |
162 |
|
|
DECLARE_SIGNAL_OUT(zmp_des_right_foot_local, dynamicgraph::Vector); |
163 |
|
|
DECLARE_SIGNAL_OUT(zmp_des_left_foot_local, dynamicgraph::Vector); |
164 |
|
|
DECLARE_SIGNAL_OUT(zmp_des, dynamicgraph::Vector); |
165 |
|
|
DECLARE_SIGNAL_OUT(zmp_ref, dynamicgraph::Vector); |
166 |
|
|
DECLARE_SIGNAL_OUT(zmp_right_foot, dynamicgraph::Vector); |
167 |
|
|
DECLARE_SIGNAL_OUT(zmp_left_foot, dynamicgraph::Vector); |
168 |
|
|
DECLARE_SIGNAL_OUT(zmp, dynamicgraph::Vector); |
169 |
|
|
DECLARE_SIGNAL_OUT(com, dynamicgraph::Vector); |
170 |
|
|
DECLARE_SIGNAL_OUT(com_vel, dynamicgraph::Vector); |
171 |
|
|
DECLARE_SIGNAL_OUT(com_acc, dynamicgraph::Vector); |
172 |
|
|
DECLARE_SIGNAL_OUT(com_acc_des, dynamicgraph::Vector); |
173 |
|
|
DECLARE_SIGNAL_OUT(base_orientation, dynamicgraph::Vector); |
174 |
|
|
DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector); |
175 |
|
|
DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector); |
176 |
|
|
DECLARE_SIGNAL_OUT(right_hand_pos, dynamicgraph::Vector); |
177 |
|
|
DECLARE_SIGNAL_OUT(left_hand_pos, dynamicgraph::Vector); |
178 |
|
|
DECLARE_SIGNAL_OUT(right_foot_vel, dynamicgraph::Vector); |
179 |
|
|
DECLARE_SIGNAL_OUT(left_foot_vel, dynamicgraph::Vector); |
180 |
|
|
DECLARE_SIGNAL_OUT(right_hand_vel, dynamicgraph::Vector); |
181 |
|
|
DECLARE_SIGNAL_OUT(left_hand_vel, dynamicgraph::Vector); |
182 |
|
|
DECLARE_SIGNAL_OUT(right_foot_acc, dynamicgraph::Vector); |
183 |
|
|
DECLARE_SIGNAL_OUT(left_foot_acc, dynamicgraph::Vector); |
184 |
|
|
DECLARE_SIGNAL_OUT(right_hand_acc, dynamicgraph::Vector); |
185 |
|
|
DECLARE_SIGNAL_OUT(left_hand_acc, dynamicgraph::Vector); |
186 |
|
|
DECLARE_SIGNAL_OUT(right_foot_acc_des, dynamicgraph::Vector); |
187 |
|
|
DECLARE_SIGNAL_OUT(left_foot_acc_des, dynamicgraph::Vector); |
188 |
|
|
|
189 |
|
|
/// This signal copies active_joints only if it changes from a all false or to |
190 |
|
|
/// an all false value |
191 |
|
|
DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector); |
192 |
|
|
|
193 |
|
|
/* --- COMMANDS --- */ |
194 |
|
|
/* --- ENTITY INHERITANCE --- */ |
195 |
|
|
virtual void display(std::ostream& os) const; |
196 |
|
|
|
197 |
|
|
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, |
198 |
|
|
const char* = "", int = 0) { |
199 |
|
|
logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; |
200 |
|
|
} |
201 |
|
|
|
202 |
|
|
protected: |
203 |
|
|
double m_dt; /// control loop time period |
204 |
|
|
double m_t; |
205 |
|
|
bool |
206 |
|
|
m_initSucceeded; /// true if the entity has been successfully initialized |
207 |
|
|
bool m_enabled; /// True if controler is enabled |
208 |
|
|
bool m_firstTime; /// True at the first iteration of the controller |
209 |
|
|
|
210 |
|
|
enum ContactState { |
211 |
|
|
DOUBLE_SUPPORT = 0, |
212 |
|
|
LEFT_SUPPORT = 1, |
213 |
|
|
LEFT_SUPPORT_TRANSITION = 2, // transition towards left support |
214 |
|
|
RIGHT_SUPPORT = 3, |
215 |
|
|
RIGHT_SUPPORT_TRANSITION = 4 |
216 |
|
|
}; |
217 |
|
|
ContactState m_contactState; |
218 |
|
|
double m_contactTransitionTime; /// end time of the current contact |
219 |
|
|
/// transition (if any) |
220 |
|
|
|
221 |
|
|
enum RightHandState { |
222 |
|
|
TASK_RIGHT_HAND_ON = 0, |
223 |
|
|
/*TASK_RIGHT_HAND_TRANSITION = 1,*/ |
224 |
|
|
TASK_RIGHT_HAND_OFF = 1 |
225 |
|
|
}; |
226 |
|
|
RightHandState m_rightHandState; |
227 |
|
|
|
228 |
|
|
enum LeftHandState { |
229 |
|
|
TASK_LEFT_HAND_ON = 0, |
230 |
|
|
/*TASK_LEFT_HAND_TRANSITION = 1,*/ |
231 |
|
|
TASK_LEFT_HAND_OFF = 1 |
232 |
|
|
}; |
233 |
|
|
LeftHandState m_leftHandState; |
234 |
|
|
/*double m_handsTransitionTime;*/ /// end time of the current |
235 |
|
|
/// transition (if any) |
236 |
|
|
|
237 |
|
|
int m_frame_id_rf; /// frame id of right foot |
238 |
|
|
int m_frame_id_lf; /// frame id of left foot |
239 |
|
|
|
240 |
|
|
int m_frame_id_rh; /// frame id of right hand |
241 |
|
|
int m_frame_id_lh; /// frame id of left hand |
242 |
|
|
|
243 |
|
|
/// tsid |
244 |
|
|
tsid::robots::RobotWrapper* m_robot; |
245 |
|
|
tsid::solvers::SolverHQPBase* m_hqpSolver; |
246 |
|
|
tsid::solvers::SolverHQPBase* m_hqpSolver_60_36_34; |
247 |
|
|
tsid::solvers::SolverHQPBase* m_hqpSolver_48_30_17; |
248 |
|
|
tsid::InverseDynamicsFormulationAccForce* m_invDyn; |
249 |
|
|
tsid::contacts::Contact6d* m_contactRF; |
250 |
|
|
tsid::contacts::Contact6d* m_contactLF; |
251 |
|
|
tsid::contacts::Contact6d* m_contactRH; |
252 |
|
|
tsid::contacts::Contact6d* m_contactLH; |
253 |
|
|
tsid::tasks::TaskComEquality* m_taskCom; |
254 |
|
|
tsid::tasks::TaskSE3Equality* m_taskRF; |
255 |
|
|
tsid::tasks::TaskSE3Equality* m_taskLF; |
256 |
|
|
tsid::tasks::TaskSE3Equality* m_taskRH; |
257 |
|
|
tsid::tasks::TaskSE3Equality* m_taskLH; |
258 |
|
|
tsid::tasks::TaskJointPosture* m_taskPosture; |
259 |
|
|
tsid::tasks::TaskJointPosture* m_taskBlockedJoints; |
260 |
|
|
|
261 |
|
|
tsid::trajectories::TrajectorySample m_sampleCom; |
262 |
|
|
tsid::trajectories::TrajectorySample m_sampleRF; |
263 |
|
|
tsid::trajectories::TrajectorySample m_sampleLF; |
264 |
|
|
tsid::trajectories::TrajectorySample m_sampleRH; |
265 |
|
|
tsid::trajectories::TrajectorySample m_sampleLH; |
266 |
|
|
tsid::trajectories::TrajectorySample m_samplePosture; |
267 |
|
|
|
268 |
|
|
double m_w_com; |
269 |
|
|
double m_w_posture; |
270 |
|
|
double m_w_hands; |
271 |
|
|
|
272 |
|
|
tsid::math::Vector m_dv_sot; /// desired accelerations (sot order) |
273 |
|
|
tsid::math::Vector m_dv_urdf; /// desired accelerations (urdf order) |
274 |
|
|
tsid::math::Vector m_f; /// desired force coefficients (24d) |
275 |
|
|
tsid::math::Vector6 m_f_RF; /// desired 6d wrench right foot |
276 |
|
|
tsid::math::Vector6 m_f_LF; /// desired 6d wrench left foot |
277 |
|
|
tsid::math::Vector3 m_com_offset; /// 3d CoM offset |
278 |
|
|
tsid::math::Vector3 m_zmp_des_LF; /// 3d desired zmp left foot |
279 |
|
|
tsid::math::Vector3 m_zmp_des_RF; /// 3d desired zmp left foot |
280 |
|
|
tsid::math::Vector3 |
281 |
|
|
m_zmp_des_LF_local; /// 3d desired zmp left foot expressed in local frame |
282 |
|
|
tsid::math::Vector3 |
283 |
|
|
m_zmp_des_RF_local; /// 3d desired zmp left foot expressed in local frame |
284 |
|
|
tsid::math::Vector3 m_zmp_des; /// 3d desired global zmp |
285 |
|
|
tsid::math::Vector3 m_zmp_LF; /// 3d zmp left foot |
286 |
|
|
tsid::math::Vector3 m_zmp_RF; /// 3d zmp left foot |
287 |
|
|
tsid::math::Vector3 m_zmp; /// 3d global zmp |
288 |
|
|
tsid::math::Vector m_tau_sot; |
289 |
|
|
tsid::math::Vector m_q_urdf; |
290 |
|
|
tsid::math::Vector m_v_urdf; |
291 |
|
|
|
292 |
|
|
typedef pinocchio::Data::Matrix6x Matrix6x; |
293 |
|
|
Matrix6x m_J_RF; |
294 |
|
|
Matrix6x m_J_LF; |
295 |
|
|
Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR; |
296 |
|
|
Eigen::ColPivHouseholderQR<Matrix6x> m_J_LF_QR; |
297 |
|
|
tsid::math::Vector6 m_v_RF_int; |
298 |
|
|
tsid::math::Vector6 m_v_LF_int; |
299 |
|
|
|
300 |
|
|
unsigned int m_timeLast; |
301 |
|
|
RobotUtilShrPtr m_robot_util; |
302 |
|
|
|
303 |
|
|
}; // class InverseDynamicsBalanceController |
304 |
|
|
} // namespace torque_control |
305 |
|
|
} // namespace sot |
306 |
|
|
} // namespace dynamicgraph |
307 |
|
|
|
308 |
|
|
#endif // #ifndef __sot_torque_control_inverse_dynamics_balance_controller_H__ |