sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
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>
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
57namespace dynamicgraph {
58namespace sot {
59namespace torque_control {
60namespace dg = ::dynamicgraph;
61using namespace dg;
62using namespace dg::command;
63using namespace std;
64using namespace tsid;
65using namespace tsid::trajectories;
66using namespace tsid::math;
67using namespace tsid::contacts;
68using namespace tsid::tasks;
69using namespace tsid::solvers;
70using namespace dg::sot;
71
72typedef SolverHQuadProgRT<60, 36, 34> SolverHQuadProgRT60x36x34;
73typedef 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
131typedef InverseDynamicsBalanceController EntityClassName;
132
133typedef Eigen::Matrix<double, 2, 1> Vector2;
134typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN;
135typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN6;
136/* --- DG FACTORY ---------------------------------------------------- */
138 "InverseDynamicsBalanceController");
139
140/* ------------------------------------------------------------------- */
141/* --- CONSTRUCTION -------------------------------------------------- */
142/* ------------------------------------------------------------------- */
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(
293 docCommandVoid0(
294 "Update the offset on the CoM based on the CoP measurement.")));
295
296 addCommand(
297 "removeRightFootContact",
298 makeCommandVoid1(
300 docCommandVoid1("Remove the contact at the right foot.",
301 "Transition time in seconds (double)")));
302
303 addCommand(
304 "removeLeftFootContact",
305 makeCommandVoid1(*this,
307 docCommandVoid1("Remove the contact at the left foot.",
308 "Transition time in seconds (double)")));
309 addCommand("addTaskRightHand",
310 makeCommandVoid0(
312 docCommandVoid0("Adds an SE3 task for the right hand.")));
313 addCommand("removeTaskRightHand",
314 makeCommandVoid1(
316 docCommandVoid1("Removes the SE3 task for the right hand.",
317 "Transition time in seconds (double)")));
318 addCommand("addTaskLeftHand",
319 makeCommandVoid0(
321 docCommandVoid0("Raises the left hand.")));
322 addCommand("removeTaskLeftHand",
323 makeCommandVoid1(
325 docCommandVoid1("lowers the left hand.",
326 "Transition time in seconds (double)")));
327}
328
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
337 const double& transitionTime) {
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) {
354 m_contactTransitionTime = m_t + transitionTime;
355 } else
357 }
358}
359
361 const double& transitionTime) {
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) {
378 m_contactTransitionTime = m_t + transitionTime;
379 } else
381 }
382}
383
385 /*const double& transitionTime*/) {
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;*/
400}
401
403 /*const double& transitionTime*/) {
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;*/
418}
419
421 const double& transitionTime) {
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);
429 }
430}
431
433 const double& transitionTime) {
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);
441 }
442}
443
445 const double& transitionTime) {
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);
452 }
453}
454
456 const double& transitionTime) {
458 SEND_MSG("Removes left hand SE3 task in " + toString(transitionTime) + " s",
459 MSG_TYPE_INFO);
460 m_invDyn->removeTask(m_taskLH->name(), transitionTime);
462 }
463}
464
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
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
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/* ------------------------------------------------------------------- */
629DEFINE_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
668DEFINE_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
962DEFINE_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
974DEFINE_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
986DEFINE_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
1002DEFINE_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
1018DEFINE_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
1030DEFINE_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
1042DEFINE_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
1063DEFINE_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
1082DEFINE_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
1105DEFINE_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
1128DEFINE_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
1144DEFINE_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
1184DEFINE_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
1208DEFINE_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
1231DEFINE_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
1250DEFINE_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
1264DEFINE_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
1278DEFINE_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
1292DEFINE_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
1308DEFINE_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
1322DEFINE_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
1338DEFINE_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
1352DEFINE_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
1366DEFINE_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
1380DEFINE_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
1393DEFINE_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
1406DEFINE_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
1421DEFINE_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
1432DEFINE_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
1446DEFINE_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
1457DEFINE_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
1471DEFINE_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
1491void 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
#define INPUT_SIGNALS
#define OUTPUT_SIGNALS
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 RESETDEBUG5()
#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)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
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
to read text file
Definition: treeview.dox:22