sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
inverse-dynamics-balance-controller.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017, Andrea Del Prete, LAAS-CNRS
3  *
4  */
5 
6 #define EIGEN_RUNTIME_NO_MALLOC
7 
8 #include <dynamic-graph/factory.h>
9 
10 #include <boost/test/unit_test.hpp>
11 #include <sot/core/debug.hh>
12 #include <sot/torque_control/commands-helper.hh>
13 #include <sot/torque_control/inverse-dynamics-balance-controller.hh>
14 #include <tsid/math/utils.hpp>
15 #include <tsid/solvers/solver-HQP-eiquadprog-rt.hpp>
16 #include <tsid/solvers/solver-HQP-eiquadprog.hpp>
17 #include <tsid/solvers/solver-HQP-factory.hxx>
18 #include <tsid/solvers/utils.hpp>
19 #include <tsid/utils/statistics.hpp>
20 #include <tsid/utils/stop-watch.hpp>
21 
22 #if DEBUG
23 #define ODEBUG(x) std::cout << x << std::endl
24 #else
25 #define ODEBUG(x)
26 #endif
27 #define ODEBUG3(x) std::cout << x << std::endl
28 
29 #define DBGFILE "/tmp/debug-sot-torqe-control.dat"
30 
31 #define RESETDEBUG5() \
32  { \
33  std::ofstream DebugFile; \
34  DebugFile.open(DBGFILE, std::ofstream::out); \
35  DebugFile.close(); \
36  }
37 #define ODEBUG5FULL(x) \
38  { \
39  std::ofstream DebugFile; \
40  DebugFile.open(DBGFILE, std::ofstream::app); \
41  DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \
42  << "):" << x << std::endl; \
43  DebugFile.close(); \
44  }
45 #define ODEBUG5(x) \
46  { \
47  std::ofstream DebugFile; \
48  DebugFile.open(DBGFILE, std::ofstream::app); \
49  DebugFile << x << std::endl; \
50  DebugFile.close(); \
51  }
52 
53 #define RESETDEBUG4()
54 #define ODEBUG4FULL(x)
55 #define ODEBUG4(x)
56 
57 namespace dynamicgraph {
58 namespace sot {
59 namespace torque_control {
60 namespace dg = ::dynamicgraph;
61 using namespace dg;
62 using namespace dg::command;
63 using namespace std;
64 using namespace tsid;
65 using namespace tsid::trajectories;
66 using namespace tsid::math;
67 using namespace tsid::contacts;
68 using namespace tsid::tasks;
69 using namespace tsid::solvers;
70 using namespace dg::sot;
71 
72 typedef SolverHQuadProgRT<60, 36, 34> SolverHQuadProgRT60x36x34;
73 typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17;
74 
75 #define REQUIRE_FINITE(A) assert(is_finite(A))
76 
77 // Size to be aligned "-------------------------------------------------------"
78 #define PROFILE_TAU_DES_COMPUTATION "InvDynBalCtrl: desired tau"
79 #define PROFILE_HQP_SOLUTION "InvDynBalCtrl: HQP"
80 #define PROFILE_PREPARE_INV_DYN "InvDynBalCtrl: prepare inv-dyn"
81 #define PROFILE_READ_INPUT_SIGNALS "InvDynBalCtrl: read input signals"
82 
83 #define ZERO_FORCE_THRESHOLD 1e-3
84 
85 #define INPUT_SIGNALS \
86  m_com_ref_posSIN << m_com_ref_velSIN << m_com_ref_accSIN << m_rf_ref_posSIN \
87  << m_rf_ref_velSIN << m_rf_ref_accSIN << m_lf_ref_posSIN \
88  << m_lf_ref_velSIN << m_lf_ref_accSIN << m_rh_ref_posSIN \
89  << m_rh_ref_velSIN << m_rh_ref_accSIN << m_lh_ref_posSIN \
90  << m_lh_ref_velSIN << m_lh_ref_accSIN \
91  << m_posture_ref_posSIN << m_posture_ref_velSIN \
92  << m_posture_ref_accSIN << m_base_orientation_ref_posSIN \
93  << m_base_orientation_ref_velSIN \
94  << m_base_orientation_ref_accSIN << m_f_ref_right_footSIN \
95  << m_f_ref_left_footSIN << m_kp_base_orientationSIN \
96  << m_kd_base_orientationSIN << m_kp_constraintsSIN \
97  << m_kd_constraintsSIN << m_kp_comSIN << m_kd_comSIN \
98  << m_kp_feetSIN << m_kd_feetSIN << m_kp_handsSIN \
99  << m_kd_handsSIN << m_kp_postureSIN << m_kd_postureSIN \
100  << m_kp_posSIN << m_kd_posSIN << m_w_comSIN << m_w_feetSIN \
101  << m_w_handsSIN << m_w_postureSIN \
102  << m_w_base_orientationSIN << m_w_torquesSIN \
103  << m_w_forcesSIN << m_weight_contact_forcesSIN << m_muSIN \
104  << m_contact_pointsSIN << m_contact_normalSIN << m_f_minSIN \
105  << m_f_max_right_footSIN << m_f_max_left_footSIN \
106  << m_rotor_inertiasSIN << m_gear_ratiosSIN << m_tau_maxSIN \
107  << m_q_minSIN << m_q_maxSIN << m_dq_maxSIN << m_ddq_maxSIN \
108  << m_dt_joint_pos_limitsSIN << m_tau_estimatedSIN << m_qSIN \
109  << m_vSIN << m_wrench_baseSIN << m_wrench_left_footSIN \
110  << m_wrench_right_footSIN << m_active_jointsSIN
111 
112 #define OUTPUT_SIGNALS \
113  m_tau_desSOUT << m_MSOUT << m_dv_desSOUT << m_f_des_right_footSOUT \
114  << m_f_des_left_footSOUT << m_zmp_des_right_footSOUT \
115  << m_zmp_des_left_footSOUT << m_zmp_des_right_foot_localSOUT \
116  << m_zmp_des_left_foot_localSOUT << m_zmp_desSOUT \
117  << m_zmp_refSOUT << m_zmp_right_footSOUT \
118  << m_zmp_left_footSOUT << m_zmpSOUT << m_comSOUT \
119  << m_com_velSOUT << m_com_accSOUT << m_com_acc_desSOUT \
120  << m_base_orientationSOUT << m_left_foot_posSOUT \
121  << m_right_foot_posSOUT << m_left_hand_posSOUT \
122  << m_right_hand_posSOUT << m_left_foot_velSOUT \
123  << m_right_foot_velSOUT << m_left_hand_velSOUT \
124  << m_right_hand_velSOUT << m_left_foot_accSOUT \
125  << m_right_foot_accSOUT << m_left_hand_accSOUT \
126  << m_right_hand_accSOUT << m_right_foot_acc_desSOUT \
127  << m_left_foot_acc_desSOUT
128 
131 typedef InverseDynamicsBalanceController EntityClassName;
132 
133 typedef Eigen::Matrix<double, 2, 1> Vector2;
134 typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN;
135 typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN6;
136 /* --- DG FACTORY ---------------------------------------------------- */
137 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(InverseDynamicsBalanceController,
138  "InverseDynamicsBalanceController");
139 
140 /* ------------------------------------------------------------------- */
141 /* --- CONSTRUCTION -------------------------------------------------- */
142 /* ------------------------------------------------------------------- */
143 InverseDynamicsBalanceController::InverseDynamicsBalanceController(
144  const std::string& name)
145  : Entity(name),
146  CONSTRUCT_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector),
147  CONSTRUCT_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector),
148  CONSTRUCT_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector),
149  CONSTRUCT_SIGNAL_IN(rf_ref_pos, dynamicgraph::Vector),
150  CONSTRUCT_SIGNAL_IN(rf_ref_vel, dynamicgraph::Vector),
151  CONSTRUCT_SIGNAL_IN(rf_ref_acc, dynamicgraph::Vector),
152  CONSTRUCT_SIGNAL_IN(lf_ref_pos, dynamicgraph::Vector),
153  CONSTRUCT_SIGNAL_IN(lf_ref_vel, dynamicgraph::Vector),
154  CONSTRUCT_SIGNAL_IN(lf_ref_acc, dynamicgraph::Vector),
155  CONSTRUCT_SIGNAL_IN(rh_ref_pos, dynamicgraph::Vector),
156  CONSTRUCT_SIGNAL_IN(rh_ref_vel, dynamicgraph::Vector),
157  CONSTRUCT_SIGNAL_IN(rh_ref_acc, dynamicgraph::Vector),
158  CONSTRUCT_SIGNAL_IN(lh_ref_pos, dynamicgraph::Vector),
159  CONSTRUCT_SIGNAL_IN(lh_ref_vel, dynamicgraph::Vector),
160  CONSTRUCT_SIGNAL_IN(lh_ref_acc, dynamicgraph::Vector),
161  CONSTRUCT_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector),
162  CONSTRUCT_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector),
163  CONSTRUCT_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector),
164  CONSTRUCT_SIGNAL_IN(base_orientation_ref_pos, dynamicgraph::Vector),
165  CONSTRUCT_SIGNAL_IN(base_orientation_ref_vel, dynamicgraph::Vector),
166  CONSTRUCT_SIGNAL_IN(base_orientation_ref_acc, dynamicgraph::Vector),
167  CONSTRUCT_SIGNAL_IN(f_ref_right_foot, dynamicgraph::Vector),
168  CONSTRUCT_SIGNAL_IN(f_ref_left_foot, dynamicgraph::Vector),
169  CONSTRUCT_SIGNAL_IN(kp_base_orientation, dynamicgraph::Vector),
170  CONSTRUCT_SIGNAL_IN(kd_base_orientation, dynamicgraph::Vector),
171  CONSTRUCT_SIGNAL_IN(kp_constraints, dynamicgraph::Vector),
172  CONSTRUCT_SIGNAL_IN(kd_constraints, dynamicgraph::Vector),
173  CONSTRUCT_SIGNAL_IN(kp_com, dynamicgraph::Vector),
174  CONSTRUCT_SIGNAL_IN(kd_com, dynamicgraph::Vector),
175  CONSTRUCT_SIGNAL_IN(kp_feet, dynamicgraph::Vector),
176  CONSTRUCT_SIGNAL_IN(kd_feet, dynamicgraph::Vector),
177  CONSTRUCT_SIGNAL_IN(kp_hands, dynamicgraph::Vector),
178  CONSTRUCT_SIGNAL_IN(kd_hands, dynamicgraph::Vector),
179  CONSTRUCT_SIGNAL_IN(kp_posture, dynamicgraph::Vector),
180  CONSTRUCT_SIGNAL_IN(kd_posture, dynamicgraph::Vector),
181  CONSTRUCT_SIGNAL_IN(kp_pos, dynamicgraph::Vector),
182  CONSTRUCT_SIGNAL_IN(kd_pos, dynamicgraph::Vector),
183  CONSTRUCT_SIGNAL_IN(w_com, double),
184  CONSTRUCT_SIGNAL_IN(w_feet, double),
185  CONSTRUCT_SIGNAL_IN(w_hands, double),
186  CONSTRUCT_SIGNAL_IN(w_posture, double),
187  CONSTRUCT_SIGNAL_IN(w_base_orientation, double),
188  CONSTRUCT_SIGNAL_IN(w_torques, double),
189  CONSTRUCT_SIGNAL_IN(w_forces, double),
190  CONSTRUCT_SIGNAL_IN(weight_contact_forces, dynamicgraph::Vector),
191  CONSTRUCT_SIGNAL_IN(mu, double),
192  CONSTRUCT_SIGNAL_IN(contact_points, dynamicgraph::Matrix),
193  CONSTRUCT_SIGNAL_IN(contact_normal, dynamicgraph::Vector),
194  CONSTRUCT_SIGNAL_IN(f_min, double),
195  CONSTRUCT_SIGNAL_IN(f_max_right_foot, double),
196  CONSTRUCT_SIGNAL_IN(f_max_left_foot, double),
197  CONSTRUCT_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector),
198  CONSTRUCT_SIGNAL_IN(gear_ratios, dynamicgraph::Vector),
199  CONSTRUCT_SIGNAL_IN(tau_max, dynamicgraph::Vector),
200  CONSTRUCT_SIGNAL_IN(q_min, dynamicgraph::Vector),
201  CONSTRUCT_SIGNAL_IN(q_max, dynamicgraph::Vector),
202  CONSTRUCT_SIGNAL_IN(dq_max, dynamicgraph::Vector),
203  CONSTRUCT_SIGNAL_IN(ddq_max, dynamicgraph::Vector),
204  CONSTRUCT_SIGNAL_IN(dt_joint_pos_limits, double),
205  CONSTRUCT_SIGNAL_IN(tau_estimated, dynamicgraph::Vector),
206  CONSTRUCT_SIGNAL_IN(q, dynamicgraph::Vector),
207  CONSTRUCT_SIGNAL_IN(v, dynamicgraph::Vector),
208  CONSTRUCT_SIGNAL_IN(wrench_base, dynamicgraph::Vector),
209  CONSTRUCT_SIGNAL_IN(wrench_left_foot, dynamicgraph::Vector),
210  CONSTRUCT_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector),
211  CONSTRUCT_SIGNAL_IN(active_joints, dynamicgraph::Vector),
212  CONSTRUCT_SIGNAL_OUT(tau_des, dynamicgraph::Vector, INPUT_SIGNALS),
213  CONSTRUCT_SIGNAL_OUT(M, dg::Matrix, m_tau_desSOUT),
214  CONSTRUCT_SIGNAL_OUT(dv_des, dg::Vector, m_tau_desSOUT),
215  CONSTRUCT_SIGNAL_OUT(f_des_right_foot, dynamicgraph::Vector,
216  m_tau_desSOUT),
217  CONSTRUCT_SIGNAL_OUT(f_des_left_foot, dynamicgraph::Vector,
218  m_tau_desSOUT),
219  CONSTRUCT_SIGNAL_OUT(zmp_des_right_foot, dynamicgraph::Vector,
220  m_f_des_right_footSOUT),
221  CONSTRUCT_SIGNAL_OUT(zmp_des_left_foot, dynamicgraph::Vector,
222  m_f_des_left_footSOUT),
223  CONSTRUCT_SIGNAL_OUT(zmp_des_right_foot_local, dynamicgraph::Vector,
224  m_f_des_right_footSOUT),
225  CONSTRUCT_SIGNAL_OUT(zmp_des_left_foot_local, dynamicgraph::Vector,
226  m_f_des_left_footSOUT),
227  CONSTRUCT_SIGNAL_OUT(zmp_des, dynamicgraph::Vector,
228  m_zmp_des_left_footSOUT << m_zmp_des_right_footSOUT),
229  CONSTRUCT_SIGNAL_OUT(zmp_ref, dynamicgraph::Vector,
230  m_f_ref_left_footSIN << m_f_ref_right_footSIN),
231  CONSTRUCT_SIGNAL_OUT(zmp_right_foot, dg::Vector, m_wrench_right_footSIN),
232  CONSTRUCT_SIGNAL_OUT(zmp_left_foot, dg::Vector, m_wrench_left_footSIN),
233  CONSTRUCT_SIGNAL_OUT(zmp, dg::Vector,
234  m_wrench_left_footSIN << m_wrench_right_footSIN
235  << m_zmp_left_footSOUT
236  << m_zmp_right_footSOUT),
237  CONSTRUCT_SIGNAL_OUT(com, dg::Vector, m_tau_desSOUT),
238  CONSTRUCT_SIGNAL_OUT(com_vel, dg::Vector, m_tau_desSOUT),
239  CONSTRUCT_SIGNAL_OUT(com_acc, dg::Vector, m_tau_desSOUT),
240  CONSTRUCT_SIGNAL_OUT(com_acc_des, dg::Vector, m_tau_desSOUT),
241  CONSTRUCT_SIGNAL_OUT(base_orientation, dg::Vector, m_tau_desSOUT),
242  CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT),
243  CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT),
244  CONSTRUCT_SIGNAL_OUT(right_hand_pos, dg::Vector, m_tau_desSOUT),
245  CONSTRUCT_SIGNAL_OUT(left_hand_pos, dg::Vector, m_tau_desSOUT),
246  CONSTRUCT_SIGNAL_OUT(right_foot_vel, dg::Vector, m_tau_desSOUT),
247  CONSTRUCT_SIGNAL_OUT(left_foot_vel, dg::Vector, m_tau_desSOUT),
248  CONSTRUCT_SIGNAL_OUT(right_hand_vel, dg::Vector, m_tau_desSOUT),
249  CONSTRUCT_SIGNAL_OUT(left_hand_vel, dg::Vector, m_tau_desSOUT),
250  CONSTRUCT_SIGNAL_OUT(right_foot_acc, dg::Vector, m_tau_desSOUT),
251  CONSTRUCT_SIGNAL_OUT(left_foot_acc, dg::Vector, m_tau_desSOUT),
252  CONSTRUCT_SIGNAL_OUT(right_hand_acc, dg::Vector, m_tau_desSOUT),
253  CONSTRUCT_SIGNAL_OUT(left_hand_acc, dg::Vector, m_tau_desSOUT),
254  CONSTRUCT_SIGNAL_OUT(right_foot_acc_des, dg::Vector, m_tau_desSOUT),
255  CONSTRUCT_SIGNAL_OUT(left_foot_acc_des, dg::Vector, m_tau_desSOUT),
256  CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector,
257  m_active_jointsSIN),
258  m_t(0.0),
259  m_initSucceeded(false),
260  m_enabled(false),
261  m_firstTime(true),
262  m_contactState(DOUBLE_SUPPORT),
263  m_rightHandState(TASK_RIGHT_HAND_OFF),
264  m_leftHandState(TASK_LEFT_HAND_OFF),
265  m_timeLast(0),
266  m_robot_util(RefVoidRobotUtil()) {
267  RESETDEBUG5();
268  Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
269 
270  m_zmp_des_RF.setZero();
271  m_zmp_des_LF.setZero();
272  m_zmp_des_RF_local.setZero();
273  m_zmp_des_LF_local.setZero();
274  m_zmp_des.setZero();
275  m_zmp_RF.setZero();
276  m_zmp_LF.setZero();
277  m_zmp.setZero();
278  m_com_offset.setZero();
279  m_v_RF_int.setZero();
280  m_v_LF_int.setZero();
281 
282  /* Commands. */
283  addCommand("init",
284  makeCommandVoid2(*this, &InverseDynamicsBalanceController::init,
285  docCommandVoid2("Initialize the entity.",
286  "Time period in seconds (double)",
287  "Robot reference (string)")));
288 
289  addCommand(
290  "updateComOffset",
291  makeCommandVoid0(
292  *this, &InverseDynamicsBalanceController::updateComOffset,
293  docCommandVoid0(
294  "Update the offset on the CoM based on the CoP measurement.")));
295 
296  addCommand(
297  "removeRightFootContact",
298  makeCommandVoid1(
299  *this, &InverseDynamicsBalanceController::removeRightFootContact,
300  docCommandVoid1("Remove the contact at the right foot.",
301  "Transition time in seconds (double)")));
302 
303  addCommand(
304  "removeLeftFootContact",
305  makeCommandVoid1(*this,
306  &InverseDynamicsBalanceController::removeLeftFootContact,
307  docCommandVoid1("Remove the contact at the left foot.",
308  "Transition time in seconds (double)")));
309  addCommand("addTaskRightHand",
310  makeCommandVoid0(
311  *this, &InverseDynamicsBalanceController::addTaskRightHand,
312  docCommandVoid0("Adds an SE3 task for the right hand.")));
313  addCommand("removeTaskRightHand",
314  makeCommandVoid1(
315  *this, &InverseDynamicsBalanceController::removeTaskRightHand,
316  docCommandVoid1("Removes the SE3 task for the right hand.",
317  "Transition time in seconds (double)")));
318  addCommand("addTaskLeftHand",
319  makeCommandVoid0(
320  *this, &InverseDynamicsBalanceController::addTaskLeftHand,
321  docCommandVoid0("Raises the left hand.")));
322  addCommand("removeTaskLeftHand",
323  makeCommandVoid1(
324  *this, &InverseDynamicsBalanceController::removeTaskLeftHand,
325  docCommandVoid1("lowers the left hand.",
326  "Transition time in seconds (double)")));
327 }
328 
329 void InverseDynamicsBalanceController::updateComOffset() {
330  const Vector3& com = m_robot->com(m_invDyn->data());
331  m_com_offset = m_zmp - com;
332  m_com_offset(2) = 0.0;
333  SEND_MSG("CoM offset updated: " + toString(m_com_offset), MSG_TYPE_INFO);
334 }
335 
336 void InverseDynamicsBalanceController::removeRightFootContact(
337  const double& transitionTime) {
338  if (m_contactState == DOUBLE_SUPPORT) {
339  SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s",
340  MSG_TYPE_INFO);
341  bool res =
342  m_invDyn->removeRigidContact(m_contactRF->name(), transitionTime);
343  if (!res) {
344  const HQPData& hqpData =
345  m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
346  SEND_MSG("Error while remove right foot contact." +
347  tsid::solvers::HQPDataToString(hqpData, false),
348  MSG_TYPE_ERROR);
349  }
350  const double& w_feet = m_w_feetSIN.accessCopy();
351  m_invDyn->addMotionTask(*m_taskRF, w_feet, 1);
352  if (transitionTime > m_dt) {
353  m_contactState = LEFT_SUPPORT_TRANSITION;
354  m_contactTransitionTime = m_t + transitionTime;
355  } else
356  m_contactState = LEFT_SUPPORT;
357  }
358 }
359 
360 void InverseDynamicsBalanceController::removeLeftFootContact(
361  const double& transitionTime) {
362  if (m_contactState == DOUBLE_SUPPORT) {
363  SEND_MSG("Remove left foot contact in " + toString(transitionTime) + " s",
364  MSG_TYPE_INFO);
365  bool res =
366  m_invDyn->removeRigidContact(m_contactLF->name(), transitionTime);
367  if (!res) {
368  const HQPData& hqpData =
369  m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
370  SEND_MSG("Error while remove right foot contact." +
371  tsid::solvers::HQPDataToString(hqpData, false),
372  MSG_TYPE_ERROR);
373  }
374  const double& w_feet = m_w_feetSIN.accessCopy();
375  m_invDyn->addMotionTask(*m_taskLF, w_feet, 1);
376  if (transitionTime > m_dt) {
377  m_contactState = RIGHT_SUPPORT_TRANSITION;
378  m_contactTransitionTime = m_t + transitionTime;
379  } else
380  m_contactState = RIGHT_SUPPORT;
381  }
382 }
383 
384 void InverseDynamicsBalanceController::addTaskRightHand(
385  /*const double& transitionTime*/) {
386  if (m_rightHandState == TASK_RIGHT_HAND_OFF) {
387  SEND_MSG("Adds right hand SE3 task in " /*+toString(transitionTime)+" s"*/,
388  MSG_TYPE_INFO);
389  const double& w_hands = m_w_handsSIN.accessCopy();
390  m_invDyn->addMotionTask(*m_taskRH, w_hands, 1);
391  }
392  /*if(transitionTime>m_dt)
393  {
394  m_rightHandState = TASK_RIGHT_HAND_TRANSITION;
395  m_handsTransitionTime = m_t + transitionTime;
396  }
397  else
398  m_rightHandState = TASK_RIGHT_HAND_ON;*/
399  m_rightHandState = TASK_RIGHT_HAND_ON;
400 }
401 
402 void InverseDynamicsBalanceController::addTaskLeftHand(
403  /*const double& transitionTime*/) {
404  if (m_leftHandState == TASK_LEFT_HAND_OFF) {
405  SEND_MSG("Raise left hand in " /*+toString(transitionTime)+" s"*/,
406  MSG_TYPE_INFO);
407  const double& w_hands = m_w_handsSIN.accessCopy();
408  m_invDyn->addMotionTask(*m_taskLH, w_hands, 1);
409  }
410  /*if(transitionTime>m_dt)
411  {
412  m_leftHandState = TASK_LEFT_HAND_TRANSITION;
413  m_handsTransitionTime = m_t + transitionTime;
414  }
415  else
416  m_leftHandState = TASK_LEFT_HAND_ON;*/
417  m_leftHandState = TASK_LEFT_HAND_ON;
418 }
419 
420 void InverseDynamicsBalanceController::addRightFootContact(
421  const double& transitionTime) {
422  if (m_contactState == LEFT_SUPPORT) {
423  SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s",
424  MSG_TYPE_INFO);
425  const double& w_forces = m_w_forcesSIN.accessCopy();
426  m_invDyn->addRigidContact(*m_contactRF, w_forces);
427  m_invDyn->removeTask(m_taskRF->name(), transitionTime);
428  m_contactState = DOUBLE_SUPPORT;
429  }
430 }
431 
432 void InverseDynamicsBalanceController::addLeftFootContact(
433  const double& transitionTime) {
434  if (m_contactState == RIGHT_SUPPORT) {
435  SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s",
436  MSG_TYPE_INFO);
437  const double& w_forces = m_w_forcesSIN.accessCopy();
438  m_invDyn->addRigidContact(*m_contactLF, w_forces);
439  m_invDyn->removeTask(m_taskLF->name(), transitionTime);
440  m_contactState = DOUBLE_SUPPORT;
441  }
442 }
443 
444 void InverseDynamicsBalanceController::removeTaskRightHand(
445  const double& transitionTime) {
446  if (m_rightHandState == TASK_RIGHT_HAND_ON) {
447  SEND_MSG(
448  "Removes right hand SE3 task in " + toString(transitionTime) + " s",
449  MSG_TYPE_INFO);
450  m_invDyn->removeTask(m_taskRH->name(), transitionTime);
451  m_rightHandState = TASK_RIGHT_HAND_OFF;
452  }
453 }
454 
455 void InverseDynamicsBalanceController::removeTaskLeftHand(
456  const double& transitionTime) {
457  if (m_leftHandState == TASK_LEFT_HAND_ON) {
458  SEND_MSG("Removes left hand SE3 task in " + toString(transitionTime) + " s",
459  MSG_TYPE_INFO);
460  m_invDyn->removeTask(m_taskLH->name(), transitionTime);
461  m_leftHandState = TASK_LEFT_HAND_OFF;
462  }
463 }
464 
465 void InverseDynamicsBalanceController::init(const double& dt,
466  const std::string& robotRef) {
467  if (dt <= 0.0)
468  return SEND_MSG("Init failed: Timestep must be positive", MSG_TYPE_ERROR);
469 
470  /* Retrieve m_robot_util informations */
471  std::string localName(robotRef);
472  if (isNameInRobotUtil(localName))
473  m_robot_util = getRobotUtil(localName);
474  else {
475  SEND_MSG("You should have an entity controller manager initialized before",
476  MSG_TYPE_ERROR);
477  return;
478  }
479 
480  const Eigen::Matrix<double, 3, 4>& contactPoints = m_contact_pointsSIN(0);
481  const Eigen::Vector3d& contactNormal = m_contact_normalSIN(0);
482  // const Eigen::VectorXd w_forceReg = m_weight_contact_forcesSIN(0);
483  const dg::sot::Vector6d& kp_contact = m_kp_constraintsSIN(0);
484  const dg::sot::Vector6d& kd_contact = m_kd_constraintsSIN(0);
485  const Eigen::Vector3d& kp_com = m_kp_comSIN(0);
486  const Eigen::Vector3d& kd_com = m_kd_comSIN(0);
487  const dg::sot::Vector6d& kd_hands = m_kd_handsSIN(0);
488  const dg::sot::Vector6d& kp_hands = m_kp_handsSIN(0);
489  const dg::sot::Vector6d& kp_feet = m_kp_feetSIN(0);
490  const dg::sot::Vector6d& kd_feet = m_kd_feetSIN(0);
491  const VectorN& kp_posture = m_kp_postureSIN(0);
492  const VectorN& kd_posture = m_kd_postureSIN(0);
493  const VectorN& rotor_inertias_sot = m_rotor_inertiasSIN(0);
494  const VectorN& gear_ratios_sot = m_gear_ratiosSIN(0);
495 
496  assert(kp_posture.size() ==
497  static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
498  assert(kd_posture.size() ==
499  static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
500  assert(rotor_inertias_sot.size() ==
501  static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
502  assert(gear_ratios_sot.size() ==
503  static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
504 
505  m_w_hands = m_w_handsSIN(0);
506  m_w_com = m_w_comSIN(0);
507  m_w_posture = m_w_postureSIN(0);
508  const double& w_forces = m_w_forcesSIN(0);
509  // const double & w_base_orientation = m_w_base_orientationSIN(0);
510  // const double & w_torques = m_w_torquesSIN(0);
511  const double& mu = m_muSIN(0);
512  const double& fMin = m_f_minSIN(0);
513  const double& fMaxRF = m_f_max_right_footSIN(0);
514  const double& fMaxLF = m_f_max_left_footSIN(0);
515 
516  try {
517  vector<string> package_dirs;
518  m_robot =
519  new robots::RobotWrapper(m_robot_util->m_urdf_filename, package_dirs,
520  pinocchio::JointModelFreeFlyer());
521 
522  assert(m_robot->nv() >= 6);
523  m_robot_util->m_nbJoints = m_robot->nv() - 6;
524 
525  Vector rotor_inertias_urdf(rotor_inertias_sot.size());
526  Vector gear_ratios_urdf(gear_ratios_sot.size());
527  m_robot_util->joints_sot_to_urdf(rotor_inertias_sot, rotor_inertias_urdf);
528  m_robot_util->joints_sot_to_urdf(gear_ratios_sot, gear_ratios_urdf);
529  m_robot->rotor_inertias(rotor_inertias_urdf);
530  m_robot->gear_ratios(gear_ratios_urdf);
531 
532  m_dv_sot.setZero(m_robot->nv());
533  m_tau_sot.setZero(m_robot->nv() - 6);
534  m_f.setZero(24);
535  m_q_urdf.setZero(m_robot->nq());
536  m_v_urdf.setZero(m_robot->nv());
537  m_J_RF.setZero(6, m_robot->nv());
538  m_J_LF.setZero(6, m_robot->nv());
539 
540  m_invDyn = new InverseDynamicsFormulationAccForce("invdyn", *m_robot);
541 
542  m_contactRF =
543  new Contact6d("contact_rfoot", *m_robot,
544  m_robot_util->m_foot_util.m_Right_Foot_Frame_Name,
545  contactPoints, contactNormal, mu, fMin, fMaxRF);
546  m_contactRF->Kp(kp_contact);
547  m_contactRF->Kd(kd_contact);
548  m_invDyn->addRigidContact(*m_contactRF, w_forces);
549 
550  m_contactLF =
551  new Contact6d("contact_lfoot", *m_robot,
552  m_robot_util->m_foot_util.m_Left_Foot_Frame_Name,
553  contactPoints, contactNormal, mu, fMin, fMaxLF);
554  m_contactLF->Kp(kp_contact);
555  m_contactLF->Kd(kd_contact);
556  m_invDyn->addRigidContact(*m_contactLF, w_forces);
557 
558  if (m_f_ref_left_footSIN.isPlugged() && m_f_ref_right_footSIN.isPlugged()) {
559  m_contactLF->setRegularizationTaskWeightVector(Vector6::Ones());
560  m_contactRF->setRegularizationTaskWeightVector(Vector6::Ones());
561  }
562 
563  m_taskCom = new TaskComEquality("task-com", *m_robot);
564  m_taskCom->Kp(kp_com);
565  m_taskCom->Kd(kd_com);
566  m_invDyn->addMotionTask(*m_taskCom, m_w_com, 1);
567 
568  m_taskRF = new TaskSE3Equality(
569  "task-rf", *m_robot, m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
570  m_taskRF->Kp(kp_feet);
571  m_taskRF->Kd(kd_feet);
572 
573  m_taskLF = new TaskSE3Equality(
574  "task-lf", *m_robot, m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
575  m_taskLF->Kp(kp_feet);
576  m_taskLF->Kd(kd_feet);
577 
578  m_taskPosture = new TaskJointPosture("task-posture", *m_robot);
579  m_taskPosture->Kp(kp_posture);
580  m_taskPosture->Kd(kd_posture);
581  m_invDyn->addMotionTask(*m_taskPosture, m_w_posture, 1);
582 
583  m_taskRH = new TaskSE3Equality(
584  "task-rh", *m_robot, m_robot_util->m_hand_util.m_Right_Hand_Frame_Name);
585  m_taskRH->Kp(kp_hands);
586  m_taskRH->Kd(kd_hands);
587 
588  m_taskLH = new TaskSE3Equality(
589  "task-lh", *m_robot, m_robot_util->m_hand_util.m_Left_Hand_Frame_Name);
590  m_taskLH->Kp(kp_hands);
591  m_taskLH->Kd(kd_hands);
592 
593  m_sampleCom = TrajectorySample(3);
594  m_samplePosture = TrajectorySample(m_robot->nv() - 6);
595  m_sampleRH = TrajectorySample(3);
596 
597  m_frame_id_rf = (int)m_robot->model().getFrameId(
598  m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
599  m_frame_id_lf = (int)m_robot->model().getFrameId(
600  m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
601 
602  m_frame_id_rh = (int)m_robot->model().getFrameId(
603  m_robot_util->m_hand_util.m_Right_Hand_Frame_Name);
604  m_frame_id_lh = (int)m_robot->model().getFrameId(
605  m_robot_util->m_hand_util.m_Left_Hand_Frame_Name);
606 
607  m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST,
608  "eiquadprog-fast");
609  m_hqpSolver->resize(m_invDyn->nVar(), m_invDyn->nEq(), m_invDyn->nIn());
610  m_hqpSolver_60_36_34 = SolverHQPFactory::createNewSolver<60, 36, 34>(
611  SOLVER_HQP_EIQUADPROG_RT, "eiquadprog_rt_60_36_34");
612  m_hqpSolver_48_30_17 = SolverHQPFactory::createNewSolver<48, 30, 17>(
613  SOLVER_HQP_EIQUADPROG_RT, "eiquadprog_rt_48_30_17");
614  } catch (const std::exception& e) {
615  std::cout << e.what();
616  return SEND_MSG(
617  "Init failed: Could load URDF :" + m_robot_util->m_urdf_filename,
618  MSG_TYPE_ERROR);
619  }
620  m_dt = dt;
621  m_initSucceeded = true;
622 }
623 
624 /* ------------------------------------------------------------------- */
625 /* --- SIGNALS ------------------------------------------------------- */
626 /* ------------------------------------------------------------------- */
629 DEFINE_SIGNAL_INNER_FUNCTION(active_joints_checked, dynamicgraph::Vector) {
630  if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
631  s.resize(m_robot_util->m_nbJoints);
632 
633  const Eigen::VectorXd& active_joints_sot = m_active_jointsSIN(iter);
634  if (m_enabled == false) {
635  if (active_joints_sot.any()) {
636  /* from all OFF to some ON */
637  m_enabled = true;
638 
639  s = active_joints_sot;
640  Eigen::VectorXd active_joints_urdf(m_robot_util->m_nbJoints);
641  m_robot_util->joints_sot_to_urdf(active_joints_sot, active_joints_urdf);
642  // joints_sot_to_urdf(active_joints_sot, active_joints_urdf);
643 
644  m_taskBlockedJoints = new TaskJointPosture("task-posture", *m_robot);
645  Eigen::VectorXd blocked_joints(m_robot_util->m_nbJoints);
646  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
647  if (active_joints_urdf(i) == 0.0)
648  blocked_joints(i) = 1.0;
649  else
650  blocked_joints(i) = 0.0;
651  SEND_MSG("Blocked joints: " + toString(blocked_joints.transpose()),
652  MSG_TYPE_INFO);
653  m_taskBlockedJoints->mask(blocked_joints);
654  TrajectorySample ref_zero(
655  static_cast<unsigned int>(m_robot_util->m_nbJoints));
656  m_taskBlockedJoints->setReference(ref_zero);
657  m_invDyn->addMotionTask(*m_taskBlockedJoints, 1.0, 0);
658  }
659  } else if (!active_joints_sot.any()) {
660  /* from some ON to all OFF */
661  m_enabled = false;
662  }
663  if (m_enabled == false)
664  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) s(i) = false;
665  return s;
666 }
667 
668 DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) {
669  if (!m_initSucceeded) {
670  SEND_WARNING_STREAM_MSG(
671  "Cannot compute signal tau_des before initialization!");
672  return s;
673  }
674  if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
675  s.resize(m_robot_util->m_nbJoints);
676 
677  getProfiler().start(PROFILE_TAU_DES_COMPUTATION);
678 
679  // use reference contact wrenches (if plugged) to determine contact phase
680  if (m_f_ref_left_footSIN.isPlugged() && m_f_ref_right_footSIN.isPlugged()) {
681  const Vector6& f_ref_left_foot = m_f_ref_left_footSIN(iter);
682  const Vector6& f_ref_right_foot = m_f_ref_right_footSIN(iter);
683  m_contactLF->setForceReference(f_ref_left_foot);
684  m_contactRF->setForceReference(f_ref_right_foot);
685 
686  if (m_contactState == DOUBLE_SUPPORT) {
687  if (f_ref_left_foot.norm() < ZERO_FORCE_THRESHOLD) {
688  removeLeftFootContact(0.0);
689  } else if (f_ref_right_foot.norm() < ZERO_FORCE_THRESHOLD) {
690  removeRightFootContact(0.0);
691  }
692  } else if (m_contactState == LEFT_SUPPORT &&
693  f_ref_right_foot.norm() > ZERO_FORCE_THRESHOLD) {
694  addRightFootContact(0.0);
695  } else if (m_contactState == RIGHT_SUPPORT &&
696  f_ref_left_foot.norm() > ZERO_FORCE_THRESHOLD) {
697  addLeftFootContact(0.0);
698  }
699  }
700 
701  if (m_contactState == RIGHT_SUPPORT_TRANSITION &&
702  m_t >= m_contactTransitionTime) {
703  m_contactState = RIGHT_SUPPORT;
704  } else if (m_contactState == LEFT_SUPPORT_TRANSITION &&
705  m_t >= m_contactTransitionTime) {
706  m_contactState = LEFT_SUPPORT;
707  }
708  /*if(m_rightHandState == TASK_RIGHT_HAND_TRANSITION && m_t >=
709  m_handsTransitionTime)
710  {
711  m_rightHandState = TASK_RIGHT_HAND_ON;
712  }
713  if(m_leftHandState == TASK_LEFT_HAND_TRANSITION && m_t >=
714  m_handsTransitionTime)
715  {
716  m_leftHandState = TASK_LEFT_HAND_ON;
717  }*/
718 
719  getProfiler().start(PROFILE_READ_INPUT_SIGNALS);
720  m_w_feetSIN(iter);
721  m_active_joints_checkedSINNER(iter);
722  const VectorN6& q_sot = m_qSIN(iter);
723  assert(q_sot.size() ==
724  static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6));
725  const VectorN6& v_sot = m_vSIN(iter);
726  assert(v_sot.size() ==
727  static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6));
728  const Vector3& x_com_ref = m_com_ref_posSIN(iter);
729  const Vector3& dx_com_ref = m_com_ref_velSIN(iter);
730  const Vector3& ddx_com_ref = m_com_ref_accSIN(iter);
731  const VectorN& q_ref = m_posture_ref_posSIN(iter);
732  assert(q_ref.size() ==
733  static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
734  const VectorN& dq_ref = m_posture_ref_velSIN(iter);
735  assert(dq_ref.size() ==
736  static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
737  const VectorN& ddq_ref = m_posture_ref_accSIN(iter);
738  assert(ddq_ref.size() ==
739  static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
740 
741  const Vector6& kp_contact = m_kp_constraintsSIN(iter);
742  const Vector6& kd_contact = m_kd_constraintsSIN(iter);
743  const Vector3& kp_com = m_kp_comSIN(iter);
744  const Vector3& kd_com = m_kd_comSIN(iter);
745 
746  const VectorN& kp_posture = m_kp_postureSIN(iter);
747  assert(kp_posture.size() ==
748  static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
749  const VectorN& kd_posture = m_kd_postureSIN(iter);
750  assert(kd_posture.size() ==
751  static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
752  const VectorN& kp_pos = m_kp_posSIN(iter);
753  assert(kp_pos.size() ==
754  static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
755  const VectorN& kd_pos = m_kd_posSIN(iter);
756  assert(kd_pos.size() ==
757  static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
758 
759  /*const double & w_hands = m_w_handsSIN(iter);*/
760  const double& w_com = m_w_comSIN(iter);
761  const double& w_posture = m_w_postureSIN(iter);
762  const double& w_forces = m_w_forcesSIN(iter);
763 
764  if (m_contactState == LEFT_SUPPORT ||
765  m_contactState == LEFT_SUPPORT_TRANSITION) {
766  const Eigen::Matrix<double, 12, 1>& x_rf_ref = m_rf_ref_posSIN(iter);
767  const Vector6& dx_rf_ref = m_rf_ref_velSIN(iter);
768  const Vector6& ddx_rf_ref = m_rf_ref_accSIN(iter);
769  const Vector6& kp_feet = m_kp_feetSIN(iter);
770  const Vector6& kd_feet = m_kd_feetSIN(iter);
771  m_sampleRF.pos = x_rf_ref;
772  m_sampleRF.vel = dx_rf_ref;
773  m_sampleRF.acc = ddx_rf_ref;
774  m_taskRF->setReference(m_sampleRF);
775  m_taskRF->Kp(kp_feet);
776  m_taskRF->Kd(kd_feet);
777  } else if (m_contactState == RIGHT_SUPPORT ||
778  m_contactState == RIGHT_SUPPORT_TRANSITION) {
779  const Eigen::Matrix<double, 12, 1>& x_lf_ref = m_lf_ref_posSIN(iter);
780  const Vector6& dx_lf_ref = m_lf_ref_velSIN(iter);
781  const Vector6& ddx_lf_ref = m_lf_ref_accSIN(iter);
782  const Vector6& kp_feet = m_kp_feetSIN(iter);
783  const Vector6& kd_feet = m_kd_feetSIN(iter);
784  m_sampleLF.pos = x_lf_ref;
785  m_sampleLF.vel = dx_lf_ref;
786  m_sampleLF.acc = ddx_lf_ref;
787  m_taskLF->setReference(m_sampleLF);
788  m_taskLF->Kp(kp_feet);
789  m_taskLF->Kd(kd_feet);
790  }
791  if (m_rightHandState == TASK_RIGHT_HAND_ON /*|| m_rightHandState == TASK_RIGHT_HAND_TRANSITION*/) {
792  const Eigen::Matrix<double, 12, 1>& x_rh_ref = m_rh_ref_posSIN(iter);
793  const Vector6& dx_rh_ref = m_rh_ref_velSIN(iter);
794  const Vector6& ddx_rh_ref = m_rh_ref_accSIN(iter);
795  const Vector6& kp_hands = m_kp_handsSIN(iter);
796  const Vector6& kd_hands = m_kd_handsSIN(iter);
797  m_sampleRH.pos = x_rh_ref;
798  m_sampleRH.vel = dx_rh_ref;
799  m_sampleRH.acc = ddx_rh_ref;
800  m_taskRH->setReference(m_sampleRH);
801  m_taskRH->Kp(kp_hands);
802  m_taskRH->Kd(kd_hands);
803  }
804  if (m_leftHandState ==
805  TASK_LEFT_HAND_ON /*|| m_leftHandState == TASK_LEFT_HAND_TRANSITION*/) {
806  const Eigen::Matrix<double, 12, 1>& x_lh_ref = m_lh_ref_posSIN(iter);
807  const Vector6& dx_lh_ref = m_lh_ref_velSIN(iter);
808  const Vector6& ddx_lh_ref = m_lh_ref_accSIN(iter);
809  const Vector6& kp_hands = m_kp_handsSIN(iter);
810  const Vector6& kd_hands = m_kd_handsSIN(iter);
811  m_sampleLH.pos = x_lh_ref;
812  m_sampleLH.vel = dx_lh_ref;
813  m_sampleLH.acc = ddx_lh_ref;
814  m_taskLH->setReference(m_sampleLH);
815  m_taskLH->Kp(kp_hands);
816  m_taskLH->Kd(kd_hands);
817  }
818 
819  getProfiler().stop(PROFILE_READ_INPUT_SIGNALS);
820 
821  getProfiler().start(PROFILE_PREPARE_INV_DYN);
822  m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf);
823  m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf);
824 
825  m_sampleCom.pos = x_com_ref - m_com_offset;
826  m_sampleCom.vel = dx_com_ref;
827  m_sampleCom.acc = ddx_com_ref;
828  m_taskCom->setReference(m_sampleCom);
829  m_taskCom->Kp(kp_com);
830  m_taskCom->Kd(kd_com);
831  if (m_w_com != w_com) {
832  // SEND_MSG("Change w_com from "+toString(m_w_com)+" to
833  // "+toString(w_com), MSG_TYPE_INFO);
834  m_w_com = w_com;
835  m_invDyn->updateTaskWeight(m_taskCom->name(), w_com);
836  }
837 
838  m_robot_util->joints_sot_to_urdf(q_ref, m_samplePosture.pos);
839  m_robot_util->joints_sot_to_urdf(dq_ref, m_samplePosture.vel);
840  m_robot_util->joints_sot_to_urdf(ddq_ref, m_samplePosture.acc);
841  m_taskPosture->setReference(m_samplePosture);
842  m_taskPosture->Kp(kp_posture);
843  m_taskPosture->Kd(kd_posture);
844  if (m_w_posture != w_posture) {
845  // SEND_MSG("Change posture from "+toString(m_w_posture)+" to
846  // "+toString(w_posture), MSG_TYPE_INFO);
847  m_w_posture = w_posture;
848  m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture);
849  }
850 
851  /*m_sampleRH.pos = x_rh_ref;
852  m_sampleRH.vel = dx_rh_ref;
853  m_sampleRH.acc = ddx_rh_ref;
854  m_taskRH->setReference(m_sampleRH);
855  m_taskRH->Kp(kp_hands);
856  m_taskRH->Kd(kd_hands);*/
857 
858  const double& fMin = m_f_minSIN(0);
859  const double& fMaxRF = m_f_max_right_footSIN(iter);
860  const double& fMaxLF = m_f_max_left_footSIN(iter);
861  m_contactLF->setMinNormalForce(fMin);
862  m_contactRF->setMinNormalForce(fMin);
863  m_contactLF->setMaxNormalForce(fMaxLF);
864  m_contactRF->setMaxNormalForce(fMaxRF);
865  m_contactLF->Kp(kp_contact);
866  m_contactLF->Kd(kd_contact);
867  m_contactRF->Kp(kp_contact);
868  m_contactRF->Kd(kd_contact);
869  m_invDyn->updateRigidContactWeights(m_contactLF->name(), w_forces);
870  m_invDyn->updateRigidContactWeights(m_contactRF->name(), w_forces);
871 
872  if (m_firstTime) {
873  m_firstTime = false;
874  m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
875  // m_robot->computeAllTerms(m_invDyn->data(), q, v);
876  pinocchio::SE3 H_lf = m_robot->position(
877  m_invDyn->data(),
878  m_robot->model().getJointId(
879  m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
880  m_contactLF->setReference(H_lf);
881  SEND_MSG("Setting left foot reference to " + toString(H_lf),
882  MSG_TYPE_DEBUG);
883 
884  pinocchio::SE3 H_rf = m_robot->position(
885  m_invDyn->data(),
886  m_robot->model().getJointId(
887  m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
888  m_contactRF->setReference(H_rf);
889  SEND_MSG("Setting right foot reference to " + toString(H_rf),
890  MSG_TYPE_DEBUG);
891  } else if (m_timeLast != static_cast<unsigned int>(iter - 1)) {
892  SEND_MSG("Last time " + toString(m_timeLast) +
893  " is not current time-1: " + toString(iter),
894  MSG_TYPE_ERROR);
895  if (m_timeLast == static_cast<unsigned int>(iter)) {
896  s = m_tau_sot;
897  return s;
898  }
899  }
900  m_timeLast = static_cast<unsigned int>(iter);
901 
902  const HQPData& hqpData =
903  m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
904 
905  getProfiler().stop(PROFILE_PREPARE_INV_DYN);
906 
907  getProfiler().start(PROFILE_HQP_SOLUTION);
908  SolverHQPBase* solver = m_hqpSolver;
909  if (m_invDyn->nVar() == 60 && m_invDyn->nEq() == 36 &&
910  m_invDyn->nIn() == 34) {
911  solver = m_hqpSolver_60_36_34;
912  getStatistics().store("solver fixed size 60_36_34", 1.0);
913  } else if (m_invDyn->nVar() == 48 && m_invDyn->nEq() == 30 &&
914  m_invDyn->nIn() == 17) {
915  solver = m_hqpSolver_48_30_17;
916  getStatistics().store("solver fixed size 48_30_17", 1.0);
917  } else
918  getStatistics().store("solver dynamic size", 1.0);
919 
920  const HQPOutput& sol = solver->solve(hqpData);
921  getProfiler().stop(PROFILE_HQP_SOLUTION);
922 
923  if (sol.status != HQP_STATUS_OPTIMAL) {
924  SEND_ERROR_STREAM_MSG("HQP solver failed to find a solution: " +
925  toString(sol.status));
926  SEND_DEBUG_STREAM_MSG(tsid::solvers::HQPDataToString(hqpData, false));
927  SEND_DEBUG_STREAM_MSG("q=" + toString(q_sot.transpose(), 1, 5));
928  SEND_DEBUG_STREAM_MSG("v=" + toString(v_sot.transpose(), 1, 5));
929  s.setZero();
930  return s;
931  }
932 
933  getStatistics().store("active inequalities",
934  static_cast<double>(sol.activeSet.size()));
935  getStatistics().store("solver iterations", sol.iterations);
936  if (ddx_com_ref.norm() > 1e-3)
937  getStatistics().store(
938  "com ff ratio",
939  ddx_com_ref.norm() / m_taskCom->getConstraint().vector().norm());
940 
941  m_dv_urdf = m_invDyn->getAccelerations(sol);
942  m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_dv_urdf, m_dv_sot);
943  Eigen::Matrix<double, 12, 1> tmp;
944  if (m_invDyn->getContactForces(m_contactRF->name(), sol, tmp))
945  m_f_RF = m_contactRF->getForceGeneratorMatrix() * tmp;
946  if (m_invDyn->getContactForces(m_contactLF->name(), sol, tmp))
947  m_f_LF = m_contactLF->getForceGeneratorMatrix() * tmp;
948  m_robot_util->joints_urdf_to_sot(m_invDyn->getActuatorForces(sol), m_tau_sot);
949 
950  m_tau_sot +=
951  kp_pos.cwiseProduct(q_ref - q_sot.tail(m_robot_util->m_nbJoints)) +
952  kd_pos.cwiseProduct(dq_ref - v_sot.tail(m_robot_util->m_nbJoints));
953 
954  getProfiler().stop(PROFILE_TAU_DES_COMPUTATION);
955  m_t += m_dt;
956 
957  s = m_tau_sot;
958 
959  return s;
960 }
961 
962 DEFINE_SIGNAL_OUT_FUNCTION(M, dynamicgraph::Matrix) {
963  if (!m_initSucceeded) {
964  SEND_WARNING_STREAM_MSG("Cannot compute signal M before initialization!");
965  return s;
966  }
967  if (s.cols() != m_robot->nv() || s.rows() != m_robot->nv())
968  s.resize(m_robot->nv(), m_robot->nv());
969  m_tau_desSOUT(iter);
970  s = m_robot->mass(m_invDyn->data());
971  return s;
972 }
973 
974 DEFINE_SIGNAL_OUT_FUNCTION(dv_des, dynamicgraph::Vector) {
975  if (!m_initSucceeded) {
976  SEND_WARNING_STREAM_MSG(
977  "Cannot compute signal dv_des before initialization!");
978  return s;
979  }
980  if (s.size() != m_robot->nv()) s.resize(m_robot->nv());
981  m_tau_desSOUT(iter);
982  s = m_dv_sot;
983  return s;
984 }
985 
986 DEFINE_SIGNAL_OUT_FUNCTION(f_des_right_foot, dynamicgraph::Vector) {
987  if (!m_initSucceeded) {
988  SEND_WARNING_STREAM_MSG(
989  "Cannot compute signal f_des_right_foot before initialization!");
990  return s;
991  }
992  if (s.size() != 6) s.resize(6);
993  m_tau_desSOUT(iter);
994  if (m_contactState == LEFT_SUPPORT) {
995  s.setZero();
996  return s;
997  }
998  s = m_f_RF;
999  return s;
1000 }
1001 
1002 DEFINE_SIGNAL_OUT_FUNCTION(f_des_left_foot, dynamicgraph::Vector) {
1003  if (!m_initSucceeded) {
1004  SEND_WARNING_STREAM_MSG(
1005  "Cannot compute signal f_des_left_foot before initialization!");
1006  return s;
1007  }
1008  if (s.size() != 6) s.resize(6);
1009  m_tau_desSOUT(iter);
1010  if (m_contactState == RIGHT_SUPPORT) {
1011  s.setZero();
1012  return s;
1013  }
1014  s = m_f_LF;
1015  return s;
1016 }
1017 
1018 DEFINE_SIGNAL_OUT_FUNCTION(com_acc_des, dynamicgraph::Vector) {
1019  if (!m_initSucceeded) {
1020  SEND_WARNING_STREAM_MSG(
1021  "Cannot compute signal com_acc_des before initialization!");
1022  return s;
1023  }
1024  if (s.size() != 3) s.resize(3);
1025  m_tau_desSOUT(iter);
1026  s = m_taskCom->getDesiredAcceleration();
1027  return s;
1028 }
1029 
1030 DEFINE_SIGNAL_OUT_FUNCTION(com_acc, dynamicgraph::Vector) {
1031  if (!m_initSucceeded) {
1032  SEND_WARNING_STREAM_MSG(
1033  "Cannot compute signal com_acc before initialization!");
1034  return s;
1035  }
1036  if (s.size() != 3) s.resize(3);
1037  m_tau_desSOUT(iter);
1038  s = m_taskCom->getAcceleration(m_dv_urdf);
1039  return s;
1040 }
1041 
1042 DEFINE_SIGNAL_OUT_FUNCTION(zmp_des_right_foot_local, dynamicgraph::Vector) {
1043  if (!m_initSucceeded) {
1044  SEND_WARNING_STREAM_MSG(
1045  "Cannot compute signal zmp_des_right_foot_local before "
1046  "initialization!");
1047  return s;
1048  }
1049  if (s.size() != 2) s.resize(2);
1050 
1051  m_f_des_right_footSOUT(iter);
1052  if (fabs(m_f_RF(2) > 1.0)) {
1053  m_zmp_des_RF_local(0) = -m_f_RF(4) / m_f_RF(2);
1054  m_zmp_des_RF_local(1) = m_f_RF(3) / m_f_RF(2);
1055  m_zmp_des_RF_local(2) = 0.0;
1056  } else
1057  m_zmp_des_RF_local.setZero();
1058 
1059  s = m_zmp_des_RF_local.head<2>();
1060  return s;
1061 }
1062 
1063 DEFINE_SIGNAL_OUT_FUNCTION(zmp_des_left_foot_local, dynamicgraph::Vector) {
1064  if (!m_initSucceeded) {
1065  SEND_WARNING_STREAM_MSG(
1066  "Cannot compute signal zmp_des_left_foot_local before initialization!");
1067  return s;
1068  }
1069  if (s.size() != 2) s.resize(2);
1070  m_f_des_left_footSOUT(iter);
1071  if (fabs(m_f_LF(2) > 1.0)) {
1072  m_zmp_des_LF_local(0) = -m_f_LF(4) / m_f_LF(2);
1073  m_zmp_des_LF_local(1) = m_f_LF(3) / m_f_LF(2);
1074  m_zmp_des_LF_local(2) = 0.0;
1075  } else
1076  m_zmp_des_LF_local.setZero();
1077 
1078  s = m_zmp_des_LF_local.head<2>();
1079  return s;
1080 }
1081 
1082 DEFINE_SIGNAL_OUT_FUNCTION(zmp_des_right_foot, dynamicgraph::Vector) {
1083  if (!m_initSucceeded) {
1084  SEND_WARNING_STREAM_MSG(
1085  "Cannot compute signal zmp_des_right_foot before initialization!");
1086  return s;
1087  }
1088  if (s.size() != 2) s.resize(2);
1089  m_f_des_right_footSOUT(iter);
1090  pinocchio::SE3 H_rf = m_robot->position(
1091  m_invDyn->data(), m_robot->model().getJointId(
1092  m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
1093  if (fabs(m_f_RF(2) > 1.0)) {
1094  m_zmp_des_RF(0) = -m_f_RF(4) / m_f_RF(2);
1095  m_zmp_des_RF(1) = m_f_RF(3) / m_f_RF(2);
1096  m_zmp_des_RF(2) = -H_rf.translation()(2);
1097  } else
1098  m_zmp_des_RF.setZero();
1099 
1100  m_zmp_des_RF = H_rf.act(m_zmp_des_RF);
1101  s = m_zmp_des_RF.head<2>();
1102  return s;
1103 }
1104 
1105 DEFINE_SIGNAL_OUT_FUNCTION(zmp_des_left_foot, dynamicgraph::Vector) {
1106  if (!m_initSucceeded) {
1107  SEND_WARNING_STREAM_MSG(
1108  "Cannot compute signal zmp_des_left_foot before initialization!");
1109  return s;
1110  }
1111  if (s.size() != 2) s.resize(2);
1112  m_f_des_left_footSOUT(iter);
1113  pinocchio::SE3 H_lf = m_robot->position(
1114  m_invDyn->data(), m_robot->model().getJointId(
1115  m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
1116  if (fabs(m_f_LF(2) > 1.0)) {
1117  m_zmp_des_LF(0) = -m_f_LF(4) / m_f_LF(2);
1118  m_zmp_des_LF(1) = m_f_LF(3) / m_f_LF(2);
1119  m_zmp_des_LF(2) = -H_lf.translation()(2);
1120  } else
1121  m_zmp_des_LF.setZero();
1122 
1123  m_zmp_des_LF = H_lf.act(m_zmp_des_LF);
1124  s = m_zmp_des_LF.head<2>();
1125  return s;
1126 }
1127 
1128 DEFINE_SIGNAL_OUT_FUNCTION(zmp_des, dynamicgraph::Vector) {
1129  if (!m_initSucceeded) {
1130  SEND_WARNING_STREAM_MSG(
1131  "Cannot compute signal zmp_des before initialization!");
1132  return s;
1133  }
1134  if (s.size() != 2) s.resize(2);
1135  m_zmp_des_left_footSOUT(iter);
1136  m_zmp_des_right_footSOUT(iter);
1137 
1138  m_zmp_des = (m_f_RF(2) * m_zmp_des_RF + m_f_LF(2) * m_zmp_des_LF) /
1139  (m_f_LF(2) + m_f_RF(2));
1140  s = m_zmp_des.head<2>();
1141  return s;
1142 }
1143 
1144 DEFINE_SIGNAL_OUT_FUNCTION(zmp_ref, dynamicgraph::Vector) {
1145  if (!m_initSucceeded) {
1146  SEND_WARNING_STREAM_MSG(
1147  "Cannot compute signal zmp_ref before initialization!");
1148  return s;
1149  }
1150  if (s.size() != 2) s.resize(2);
1151  const Vector6& f_LF = m_f_ref_left_footSIN(iter);
1152  const Vector6& f_RF = m_f_ref_right_footSIN(iter);
1153 
1154  pinocchio::SE3 H_lf = m_robot->position(
1155  m_invDyn->data(), m_robot->model().getJointId(
1156  m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
1157  Vector3 zmp_LF, zmp_RF;
1158  if (fabs(f_LF(2) > 1.0)) {
1159  zmp_LF(0) = -f_LF(4) / f_LF(2);
1160  zmp_LF(1) = f_LF(3) / f_LF(2);
1161  zmp_LF(2) = -H_lf.translation()(2);
1162  } else
1163  zmp_LF.setZero();
1164  zmp_LF = H_lf.act(zmp_LF);
1165 
1166  pinocchio::SE3 H_rf = m_robot->position(
1167  m_invDyn->data(), m_robot->model().getJointId(
1168  m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
1169  if (fabs(f_RF(2) > 1.0)) {
1170  zmp_RF(0) = -f_RF(4) / f_RF(2);
1171  zmp_RF(1) = f_RF(3) / f_RF(2);
1172  zmp_RF(2) = -H_rf.translation()(2);
1173  } else
1174  zmp_RF.setZero();
1175  zmp_RF = H_rf.act(zmp_RF);
1176 
1177  if (f_LF(2) + f_RF(2) != 0.0)
1178  s = (f_RF(2) * zmp_RF.head<2>() + f_LF(2) * zmp_LF.head<2>()) /
1179  (f_LF(2) + f_RF(2));
1180 
1181  return s;
1182 }
1183 
1184 DEFINE_SIGNAL_OUT_FUNCTION(zmp_right_foot, dynamicgraph::Vector) {
1185  if (!m_initSucceeded) {
1186  SEND_WARNING_STREAM_MSG(
1187  "Cannot compute signal zmp_right_foot before initialization!");
1188  return s;
1189  }
1190  if (s.size() != 2) s.resize(2);
1191  const Vector6& f = m_wrench_right_footSIN(iter);
1192  assert(f.size() == 6);
1193  pinocchio::SE3 H_rf = m_robot->position(
1194  m_invDyn->data(), m_robot->model().getJointId(
1195  m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
1196  if (fabs(f(2) > 1.0)) {
1197  m_zmp_RF(0) = -f(4) / f(2);
1198  m_zmp_RF(1) = f(3) / f(2);
1199  m_zmp_RF(2) = -H_rf.translation()(2);
1200  } else
1201  m_zmp_RF.setZero();
1202 
1203  m_zmp_RF = H_rf.act(m_zmp_RF);
1204  s = m_zmp_RF.head<2>();
1205  return s;
1206 }
1207 
1208 DEFINE_SIGNAL_OUT_FUNCTION(zmp_left_foot, dynamicgraph::Vector) {
1209  if (!m_initSucceeded) {
1210  SEND_WARNING_STREAM_MSG(
1211  "Cannot compute signal zmp_left_foot before initialization!");
1212  return s;
1213  }
1214  if (s.size() != 2) s.resize(2);
1215  const Vector6& f = m_wrench_left_footSIN(iter);
1216  pinocchio::SE3 H_lf = m_robot->position(
1217  m_invDyn->data(), m_robot->model().getJointId(
1218  m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
1219  if (fabs(f(2) > 1.0)) {
1220  m_zmp_LF(0) = -f(4) / f(2);
1221  m_zmp_LF(1) = f(3) / f(2);
1222  m_zmp_LF(2) = -H_lf.translation()(2);
1223  } else
1224  m_zmp_LF.setZero();
1225 
1226  m_zmp_LF = H_lf.act(m_zmp_LF);
1227  s = m_zmp_LF.head<2>();
1228  return s;
1229 }
1230 
1231 DEFINE_SIGNAL_OUT_FUNCTION(zmp, dynamicgraph::Vector) {
1232  if (!m_initSucceeded) {
1233  std::ostringstream oss("Cannot compute signal zmp before initialization!");
1234  oss << iter;
1235  SEND_WARNING_STREAM_MSG(oss.str());
1236  return s;
1237  }
1238  if (s.size() != 2) s.resize(2);
1239  const Vector6& f_LF = m_wrench_left_footSIN(iter);
1240  const Vector6& f_RF = m_wrench_right_footSIN(iter);
1241  m_zmp_left_footSOUT(iter);
1242  m_zmp_right_footSOUT(iter);
1243 
1244  if (f_LF(2) + f_RF(2) > 1.0)
1245  m_zmp = (f_RF(2) * m_zmp_RF + f_LF(2) * m_zmp_LF) / (f_LF(2) + f_RF(2));
1246  s = m_zmp.head<2>();
1247  return s;
1248 }
1249 
1250 DEFINE_SIGNAL_OUT_FUNCTION(com, dynamicgraph::Vector) {
1251  if (!m_initSucceeded) {
1252  std::ostringstream oss(
1253  "Cannot compute signal com before initialization! iter:");
1254  oss << iter;
1255  SEND_WARNING_STREAM_MSG(oss.str());
1256  return s;
1257  }
1258  if (s.size() != 3) s.resize(3);
1259  const Vector3& com = m_robot->com(m_invDyn->data());
1260  s = com + m_com_offset;
1261  return s;
1262 }
1263 
1264 DEFINE_SIGNAL_OUT_FUNCTION(com_vel, dynamicgraph::Vector) {
1265  if (!m_initSucceeded) {
1266  std::ostringstream oss(
1267  "Cannot compute signal com_vel before initialization! iter:");
1268  oss << iter;
1269  SEND_WARNING_STREAM_MSG(oss.str());
1270  return s;
1271  }
1272  if (s.size() != 3) s.resize(3);
1273  const Vector3& com_vel = m_robot->com_vel(m_invDyn->data());
1274  s = com_vel;
1275  return s;
1276 }
1277 
1278 DEFINE_SIGNAL_OUT_FUNCTION(base_orientation, dynamicgraph::Vector) {
1279  if (!m_initSucceeded) {
1280  std::ostringstream oss(
1281  "Cannot compute signal com_vel before initialization! iter:");
1282  oss << iter;
1283  SEND_WARNING_STREAM_MSG(oss.str());
1284  return s;
1285  }
1286  /*
1287  * Code
1288  */
1289  return s;
1290 }
1291 
1292 DEFINE_SIGNAL_OUT_FUNCTION(left_foot_pos, dynamicgraph::Vector) {
1293  if (!m_initSucceeded) {
1294  std::ostringstream oss(
1295  "Cannot compute signal left_foot_pos before initialization! iter:");
1296  oss << iter;
1297  SEND_WARNING_STREAM_MSG(oss.str());
1298  return s;
1299  }
1300  m_tau_desSOUT(iter);
1301  pinocchio::SE3 oMi;
1302  s.resize(12);
1303  m_robot->framePosition(m_invDyn->data(), m_frame_id_lf, oMi);
1304  tsid::math::SE3ToVector(oMi, s);
1305  return s;
1306 }
1307 
1308 DEFINE_SIGNAL_OUT_FUNCTION(left_hand_pos, dynamicgraph::Vector) {
1309  if (!m_initSucceeded) {
1310  SEND_WARNING_STREAM_MSG(
1311  "Cannot compute signal left hand_pos before initialization!");
1312  return s;
1313  }
1314  m_tau_desSOUT(iter);
1315  pinocchio::SE3 oMi;
1316  s.resize(12);
1317  m_robot->framePosition(m_invDyn->data(), m_frame_id_lh, oMi);
1318  tsid::math::SE3ToVector(oMi, s);
1319  return s;
1320 }
1321 
1322 DEFINE_SIGNAL_OUT_FUNCTION(right_foot_pos, dynamicgraph::Vector) {
1323  if (!m_initSucceeded) {
1324  std::ostringstream oss(
1325  "Cannot compute signal rigt_foot_pos before initialization! iter:");
1326  oss << iter;
1327  SEND_WARNING_STREAM_MSG(oss.str());
1328  return s;
1329  }
1330  m_tau_desSOUT(iter);
1331  pinocchio::SE3 oMi;
1332  s.resize(12);
1333  m_robot->framePosition(m_invDyn->data(), m_frame_id_rf, oMi);
1334  tsid::math::SE3ToVector(oMi, s);
1335  return s;
1336 }
1337 
1338 DEFINE_SIGNAL_OUT_FUNCTION(right_hand_pos, dynamicgraph::Vector) {
1339  if (!m_initSucceeded) {
1340  SEND_WARNING_STREAM_MSG(
1341  "Cannot compute signal right_hand_pos before initialization!");
1342  return s;
1343  }
1344  m_tau_desSOUT(iter);
1345  pinocchio::SE3 oMi;
1346  s.resize(12);
1347  m_robot->framePosition(m_invDyn->data(), m_frame_id_rh, oMi);
1348  tsid::math::SE3ToVector(oMi, s);
1349  return s;
1350 }
1351 
1352 DEFINE_SIGNAL_OUT_FUNCTION(left_foot_vel, dynamicgraph::Vector) {
1353  if (!m_initSucceeded) {
1354  std::ostringstream oss(
1355  "Cannot compute signal left_foot_vel before initialization!");
1356  oss << iter;
1357  SEND_WARNING_STREAM_MSG(oss.str());
1358  return s;
1359  }
1360  pinocchio::Motion v;
1361  m_robot->frameVelocity(m_invDyn->data(), m_frame_id_lf, v);
1362  s = v.toVector();
1363  return s;
1364 }
1365 
1366 DEFINE_SIGNAL_OUT_FUNCTION(left_hand_vel, dynamicgraph::Vector) {
1367  if (!m_initSucceeded) {
1368  std::ostringstream oss(
1369  "Cannot compute signal left_hand_vel before initialization!:");
1370  oss << iter;
1371  SEND_WARNING_STREAM_MSG(oss.str());
1372  return s;
1373  }
1374  pinocchio::Motion v;
1375  m_robot->frameVelocity(m_invDyn->data(), m_frame_id_lh, v);
1376  s = v.toVector();
1377  return s;
1378 }
1379 
1380 DEFINE_SIGNAL_OUT_FUNCTION(right_foot_vel, dynamicgraph::Vector) {
1381  if (!m_initSucceeded) {
1382  SEND_WARNING_STREAM_MSG(
1383  "Cannot compute signal right_foot_vel before initialization! iter:" +
1384  toString(iter));
1385  return s;
1386  }
1387  pinocchio::Motion v;
1388  m_robot->frameVelocity(m_invDyn->data(), m_frame_id_rf, v);
1389  s = v.toVector();
1390  return s;
1391 }
1392 
1393 DEFINE_SIGNAL_OUT_FUNCTION(right_hand_vel, dynamicgraph::Vector) {
1394  if (!m_initSucceeded) {
1395  SEND_WARNING_STREAM_MSG(
1396  "Cannot compute signal right_hand_vel before initialization! " +
1397  toString(iter));
1398  return s;
1399  }
1400  pinocchio::Motion v;
1401  m_robot->frameVelocity(m_invDyn->data(), m_frame_id_rh, v);
1402  s = v.toVector();
1403  return s;
1404 }
1405 
1406 DEFINE_SIGNAL_OUT_FUNCTION(left_foot_acc, dynamicgraph::Vector) {
1407  if (!m_initSucceeded) {
1408  SEND_WARNING_STREAM_MSG(
1409  "Cannot compute signal left_foot_acc before initialization! " +
1410  toString(iter));
1411  return s;
1412  }
1413  m_tau_desSOUT(iter);
1414  if (m_contactState == RIGHT_SUPPORT)
1415  s = m_taskLF->getAcceleration(m_dv_urdf);
1416  else
1417  s = m_contactLF->getMotionTask().getAcceleration(m_dv_urdf);
1418  return s;
1419 }
1420 
1421 DEFINE_SIGNAL_OUT_FUNCTION(left_hand_acc, dynamicgraph::Vector) {
1422  if (!m_initSucceeded) {
1423  SEND_WARNING_STREAM_MSG(
1424  "Cannot compute signal left_hand_acc before initialization!");
1425  return s;
1426  }
1427  m_tau_desSOUT(iter);
1428  s = m_contactLH->getMotionTask().getAcceleration(m_dv_urdf);
1429  return s;
1430 }
1431 
1432 DEFINE_SIGNAL_OUT_FUNCTION(right_foot_acc, dynamicgraph::Vector) {
1433  if (!m_initSucceeded) {
1434  SEND_WARNING_STREAM_MSG(
1435  "Cannot compute signal right_foot_acc before initialization!");
1436  return s;
1437  }
1438  m_tau_desSOUT(iter);
1439  if (m_contactState == LEFT_SUPPORT)
1440  s = m_taskRF->getAcceleration(m_dv_urdf);
1441  else
1442  s = m_contactRF->getMotionTask().getAcceleration(m_dv_urdf);
1443  return s;
1444 }
1445 
1446 DEFINE_SIGNAL_OUT_FUNCTION(right_hand_acc, dynamicgraph::Vector) {
1447  if (!m_initSucceeded) {
1448  SEND_WARNING_STREAM_MSG(
1449  "Cannot compute signal right_hand_acc before initialization!");
1450  return s;
1451  }
1452  m_tau_desSOUT(iter);
1453  s = m_contactRH->getMotionTask().getAcceleration(m_dv_urdf);
1454  return s;
1455 }
1456 
1457 DEFINE_SIGNAL_OUT_FUNCTION(left_foot_acc_des, dynamicgraph::Vector) {
1458  if (!m_initSucceeded) {
1459  SEND_WARNING_STREAM_MSG(
1460  "Cannot compute signal left_foot_acc_des before initialization!");
1461  return s;
1462  }
1463  m_tau_desSOUT(iter);
1464  if (m_contactState == RIGHT_SUPPORT)
1465  s = m_taskLF->getDesiredAcceleration();
1466  else
1467  s = m_contactLF->getMotionTask().getDesiredAcceleration();
1468  return s;
1469 }
1470 
1471 DEFINE_SIGNAL_OUT_FUNCTION(right_foot_acc_des, dynamicgraph::Vector) {
1472  if (!m_initSucceeded) {
1473  SEND_WARNING_STREAM_MSG(
1474  "Cannot compute signal right_foot_acc_des before initialization!");
1475  return s;
1476  }
1477  m_tau_desSOUT(iter);
1478  if (m_contactState == LEFT_SUPPORT)
1479  s = m_taskRF->getDesiredAcceleration();
1480  else
1481  s = m_contactRF->getMotionTask().getDesiredAcceleration();
1482  return s;
1483 }
1484 
1485 /* --- COMMANDS ---------------------------------------------------------- */
1486 
1487 /* ------------------------------------------------------------------- */
1488 /* --- ENTITY -------------------------------------------------------- */
1489 /* ------------------------------------------------------------------- */
1490 
1491 void InverseDynamicsBalanceController::display(std::ostream& os) const {
1492  os << "InverseDynamicsBalanceController " << getName();
1493  try {
1494  getProfiler().report_all(3, os);
1495  getStatistics().report_all(1, os);
1496  os << "QP size: nVar " << m_invDyn->nVar() << " nEq " << m_invDyn->nEq()
1497  << " nIn " << m_invDyn->nIn() << "\n";
1498  } catch (ExceptionSignal e) {
1499  }
1500 }
1501 } // namespace torque_control
1502 } // namespace sot
1503 } // namespace dynamicgraph
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_v_RF_int
tsid::math::Vector6 m_v_RF_int
Definition: inverse-dynamics-balance-controller.hh:297
RESETDEBUG5
#define RESETDEBUG5()
Definition: inverse-dynamics-balance-controller.cpp:31
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_sampleRH
tsid::trajectories::TrajectorySample m_sampleRH
Definition: inverse-dynamics-balance-controller.hh:264
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_initSucceeded
bool m_initSucceeded
Definition: inverse-dynamics-balance-controller.hh:206
dynamicgraph::sot::torque_control::Vector3
Eigen::Matrix< double, 3, 1 > Vector3
Definition: admittance-controller.cpp:53
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_taskLF
tsid::tasks::TaskSE3Equality * m_taskLF
Definition: inverse-dynamics-balance-controller.hh:255
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_invDyn
tsid::InverseDynamicsFormulationAccForce * m_invDyn
Definition: inverse-dynamics-balance-controller.hh:248
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_zmp_LF
tsid::math::Vector3 m_zmp_LF
3d desired global zmp
Definition: inverse-dynamics-balance-controller.hh:285
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_contactTransitionTime
double m_contactTransitionTime
Definition: inverse-dynamics-balance-controller.hh:218
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_taskPosture
tsid::tasks::TaskJointPosture * m_taskPosture
Definition: inverse-dynamics-balance-controller.hh:258
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_contactRF
tsid::contacts::Contact6d * m_contactRF
Definition: inverse-dynamics-balance-controller.hh:249
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::init
void init(const double &dt, const std::string &robotRef)
Definition: inverse-dynamics-balance-controller.cpp:465
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_J_LF
Matrix6x m_J_LF
Definition: inverse-dynamics-balance-controller.hh:294
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_samplePosture
tsid::trajectories::TrajectorySample m_samplePosture
Definition: inverse-dynamics-balance-controller.hh:266
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::TASK_LEFT_HAND_ON
@ TASK_LEFT_HAND_ON
Definition: inverse-dynamics-balance-controller.hh:229
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_zmp_RF
tsid::math::Vector3 m_zmp_RF
3d zmp left foot
Definition: inverse-dynamics-balance-controller.hh:286
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_zmp
tsid::math::Vector3 m_zmp
3d zmp left foot
Definition: inverse-dynamics-balance-controller.hh:287
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_rightHandState
RightHandState m_rightHandState
Definition: inverse-dynamics-balance-controller.hh:226
dynamicgraph
to read text file
Definition: treeview.dox:22
PROFILE_READ_INPUT_SIGNALS
#define PROFILE_READ_INPUT_SIGNALS
Definition: inverse-dynamics-balance-controller.cpp:81
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_taskCom
tsid::tasks::TaskComEquality * m_taskCom
Definition: inverse-dynamics-balance-controller.hh:253
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_dt
double m_dt
Definition: inverse-dynamics-balance-controller.hh:203
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_leftHandState
LeftHandState m_leftHandState
Definition: inverse-dynamics-balance-controller.hh:233
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::updateComOffset
void updateComOffset()
Definition: inverse-dynamics-balance-controller.cpp:329
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::removeRightFootContact
void removeRightFootContact(const double &transitionTime)
Definition: inverse-dynamics-balance-controller.cpp:336
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_hqpSolver_60_36_34
tsid::solvers::SolverHQPBase * m_hqpSolver_60_36_34
Definition: inverse-dynamics-balance-controller.hh:246
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_zmp_des_RF
tsid::math::Vector3 m_zmp_des_RF
3d desired zmp left foot
Definition: inverse-dynamics-balance-controller.hh:279
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::addRightFootContact
void addRightFootContact(const double &transitionTime)
Definition: inverse-dynamics-balance-controller.cpp:420
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_robot_util
RobotUtilShrPtr m_robot_util
Definition: inverse-dynamics-balance-controller.hh:301
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_w_posture
double m_w_posture
Definition: inverse-dynamics-balance-controller.hh:269
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_sampleCom
tsid::trajectories::TrajectorySample m_sampleCom
Definition: inverse-dynamics-balance-controller.hh:261
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::TASK_RIGHT_HAND_ON
@ TASK_RIGHT_HAND_ON
Definition: inverse-dynamics-balance-controller.hh:222
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::InverseDynamicsBalanceController
EIGEN_MAKE_ALIGNED_OPERATOR_NEW InverseDynamicsBalanceController(const std::string &name)
Definition: inverse-dynamics-balance-controller.cpp:143
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::addTaskLeftHand
void addTaskLeftHand()
Definition: inverse-dynamics-balance-controller.cpp:402
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_hqpSolver_48_30_17
tsid::solvers::SolverHQPBase * m_hqpSolver_48_30_17
Definition: inverse-dynamics-balance-controller.hh:247
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_hqpSolver
tsid::solvers::SolverHQPBase * m_hqpSolver
Definition: inverse-dynamics-balance-controller.hh:245
PROFILE_HQP_SOLUTION
#define PROFILE_HQP_SOLUTION
Definition: inverse-dynamics-balance-controller.cpp:79
commands-helper.hh
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::removeTaskRightHand
void removeTaskRightHand(const double &transitionTime)
Definition: inverse-dynamics-balance-controller.cpp:444
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_taskLH
tsid::tasks::TaskSE3Equality * m_taskLH
Definition: inverse-dynamics-balance-controller.hh:257
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_zmp_des_LF_local
tsid::math::Vector3 m_zmp_des_LF_local
3d desired zmp left foot
Definition: inverse-dynamics-balance-controller.hh:281
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_frame_id_lh
int m_frame_id_lh
frame id of right hand
Definition: inverse-dynamics-balance-controller.hh:241
dynamicgraph::sot::torque_control::VectorN6
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN6
Definition: inverse-dynamics-balance-controller.cpp:135
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_q_urdf
tsid::math::Vector m_q_urdf
Definition: inverse-dynamics-balance-controller.hh:289
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_zmp_des_RF_local
tsid::math::Vector3 m_zmp_des_RF_local
3d desired zmp left foot expressed in local frame
Definition: inverse-dynamics-balance-controller.hh:283
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_taskRH
tsid::tasks::TaskSE3Equality * m_taskRH
Definition: inverse-dynamics-balance-controller.hh:256
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::TASK_LEFT_HAND_OFF
@ TASK_LEFT_HAND_OFF
Definition: inverse-dynamics-balance-controller.hh:231
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_v_urdf
tsid::math::Vector m_v_urdf
Definition: inverse-dynamics-balance-controller.hh:290
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::LEFT_SUPPORT
@ LEFT_SUPPORT
Definition: inverse-dynamics-balance-controller.hh:212
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::removeTaskLeftHand
void removeTaskLeftHand(const double &transitionTime)
Definition: inverse-dynamics-balance-controller.cpp:455
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::DOUBLE_SUPPORT
@ DOUBLE_SUPPORT
Definition: inverse-dynamics-balance-controller.hh:211
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::LEFT_SUPPORT_TRANSITION
@ LEFT_SUPPORT_TRANSITION
Definition: inverse-dynamics-balance-controller.hh:213
inverse-dynamics-balance-controller.hh
dynamicgraph::sot::torque_control::SolverHQuadProgRT48x30x17
SolverHQuadProgRT< 48, 30, 17 > SolverHQuadProgRT48x30x17
Definition: inverse-dynamics-balance-controller.cpp:73
ZERO_FORCE_THRESHOLD
#define ZERO_FORCE_THRESHOLD
Definition: inverse-dynamics-balance-controller.cpp:83
OUTPUT_SIGNALS
#define OUTPUT_SIGNALS
Definition: inverse-dynamics-balance-controller.cpp:112
PROFILE_TAU_DES_COMPUTATION
#define PROFILE_TAU_DES_COMPUTATION
Definition: inverse-dynamics-balance-controller.cpp:78
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::display
virtual void display(std::ostream &os) const
Definition: inverse-dynamics-balance-controller.cpp:1491
dynamicgraph::sot::torque_control::DEFINE_SIGNAL_INNER_FUNCTION
DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector)
Definition: base-estimator.cpp:587
dynamicgraph::sot::torque_control::Vector2
Eigen::Matrix< double, 2, 1 > Vector2
Definition: inverse-dynamics-balance-controller.cpp:133
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_w_hands
double m_w_hands
Definition: inverse-dynamics-balance-controller.hh:270
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController
Definition: inverse-dynamics-balance-controller.hh:57
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::TASK_RIGHT_HAND_OFF
@ TASK_RIGHT_HAND_OFF
Definition: inverse-dynamics-balance-controller.hh:224
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_frame_id_lf
int m_frame_id_lf
frame id of right foot
Definition: inverse-dynamics-balance-controller.hh:238
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_contactLF
tsid::contacts::Contact6d * m_contactLF
Definition: inverse-dynamics-balance-controller.hh:250
dynamicgraph::sot::torque_control::VectorN
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN
Definition: inverse-dynamics-balance-controller.cpp:134
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_zmp_des
tsid::math::Vector3 m_zmp_des
3d desired zmp left foot expressed in local frame
Definition: inverse-dynamics-balance-controller.hh:284
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_f
tsid::math::Vector m_f
desired accelerations (urdf order)
Definition: inverse-dynamics-balance-controller.hh:274
dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
INPUT_SIGNALS
#define INPUT_SIGNALS
Definition: inverse-dynamics-balance-controller.cpp:85
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_robot
tsid::robots::RobotWrapper * m_robot
frame id of left hand
Definition: inverse-dynamics-balance-controller.hh:244
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::RIGHT_SUPPORT_TRANSITION
@ RIGHT_SUPPORT_TRANSITION
Definition: inverse-dynamics-balance-controller.hh:215
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_contactState
ContactState m_contactState
Definition: inverse-dynamics-balance-controller.hh:217
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_frame_id_rh
int m_frame_id_rh
frame id of left foot
Definition: inverse-dynamics-balance-controller.hh:240
dynamicgraph::sot::torque_control::Vector6
Eigen::Matrix< double, 6, 1 > Vector6
Definition: admittance-controller.cpp:54
PROFILE_PREPARE_INV_DYN
#define PROFILE_PREPARE_INV_DYN
Definition: inverse-dynamics-balance-controller.cpp:80
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_taskRF
tsid::tasks::TaskSE3Equality * m_taskRF
Definition: inverse-dynamics-balance-controller.hh:254
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_dv_sot
tsid::math::Vector m_dv_sot
Definition: inverse-dynamics-balance-controller.hh:272
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:51
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::addLeftFootContact
void addLeftFootContact(const double &transitionTime)
Definition: inverse-dynamics-balance-controller.cpp:432
dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
Definition: admittance-controller.cpp:193
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::RIGHT_SUPPORT
@ RIGHT_SUPPORT
Definition: inverse-dynamics-balance-controller.hh:214
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_frame_id_rf
int m_frame_id_rf
Definition: inverse-dynamics-balance-controller.hh:237
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::removeLeftFootContact
void removeLeftFootContact(const double &transitionTime)
Definition: inverse-dynamics-balance-controller.cpp:360
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_zmp_des_LF
tsid::math::Vector3 m_zmp_des_LF
3d CoM offset
Definition: inverse-dynamics-balance-controller.hh:278
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_t
double m_t
control loop time period
Definition: inverse-dynamics-balance-controller.hh:204
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_tau_sot
tsid::math::Vector m_tau_sot
3d global zmp
Definition: inverse-dynamics-balance-controller.hh:288
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_v_LF_int
tsid::math::Vector6 m_v_LF_int
Definition: inverse-dynamics-balance-controller.hh:298
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_com_offset
tsid::math::Vector3 m_com_offset
desired 6d wrench left foot
Definition: inverse-dynamics-balance-controller.hh:277
dynamicgraph::sot::torque_control::SolverHQuadProgRT60x36x34
SolverHQuadProgRT< 60, 36, 34 > SolverHQuadProgRT60x36x34
Definition: inverse-dynamics-balance-controller.cpp:72
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_J_RF
Matrix6x m_J_RF
Definition: inverse-dynamics-balance-controller.hh:293
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::addTaskRightHand
void addTaskRightHand()
Definition: inverse-dynamics-balance-controller.cpp:384
dynamicgraph::sot::torque_control::InverseDynamicsBalanceController::m_w_com
double m_w_com
Definition: inverse-dynamics-balance-controller.hh:268