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