6 #define EIGEN_RUNTIME_NO_MALLOC 8 #include <boost/test/unit_test.hpp> 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> 18 #include <dynamic-graph/factory.h> 20 #include <sot/core/debug.hh> 26 #define ODEBUG(x) std::cout << x << std::endl 30 #define ODEBUG3(x) std::cout << x << std::endl 32 #define DBGFILE "/tmp/debug-sot-torqe-control.dat" 34 #define RESETDEBUG5() \ 36 std::ofstream DebugFile; \ 37 DebugFile.open(DBGFILE, std::ofstream::out); \ 40 #define ODEBUG5FULL(x) \ 42 std::ofstream DebugFile; \ 43 DebugFile.open(DBGFILE, std::ofstream::app); \ 44 DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ << "):" << x << std::endl; \ 49 std::ofstream DebugFile; \ 50 DebugFile.open(DBGFILE, std::ofstream::app); \ 51 DebugFile << x << std::endl; \ 56 #define ODEBUG4FULL(x) 62 namespace dg = ::dynamicgraph;
77 #define REQUIRE_FINITE(A) assert(is_finite(A)) 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" 85 #define ZERO_FORCE_THRESHOLD 1e-3 87 #define INPUT_SIGNALS m_com_ref_posSIN \ 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 \ 128 << m_w_base_orientationSIN \ 131 << m_weight_contact_forcesSIN \ 133 << m_contact_pointsSIN \ 134 << m_contact_normalSIN \ 136 << m_f_max_right_footSIN \ 137 << m_f_max_left_footSIN \ 138 << m_rotor_inertiasSIN \ 139 << m_gear_ratiosSIN \ 145 << m_dt_joint_pos_limitsSIN \ 146 << m_tau_estimatedSIN \ 149 << m_wrench_baseSIN \ 150 << m_wrench_left_footSIN \ 151 << m_wrench_right_footSIN \ 152 << m_active_jointsSIN 154 #define OUTPUT_SIGNALS m_tau_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 \ 165 << m_zmp_right_footSOUT \ 166 << m_zmp_left_footSOUT \ 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 193 typedef Eigen::Matrix<double, Eigen::Dynamic, 1>
VectorN;
194 typedef Eigen::Matrix<double, Eigen::Dynamic, 1>
VectorN6;
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),
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),
261 CONSTRUCT_SIGNAL_IN(dt_joint_pos_limits, double),
262 CONSTRUCT_SIGNAL_IN(tau_estimated,
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),
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),
304 m_initSucceeded(false),
307 m_contactState(DOUBLE_SUPPORT),
308 m_rightHandState(TASK_RIGHT_HAND_OFF),
309 m_leftHandState(TASK_LEFT_HAND_OFF),
311 m_robot_util(RefVoidRobotUtil()) {
329 docCommandVoid2(
"Initialize the entity.",
"Time period in seconds (double)",
330 "Robot reference (string)")));
332 addCommand(
"updateComOffset",
334 docCommandVoid0(
"Update the offset on the CoM based on the CoP measurement.")));
336 addCommand(
"removeRightFootContact",
339 docCommandVoid1(
"Remove the contact at the right foot.",
"Transition time in seconds (double)")));
342 docCommandVoid1(
"Remove the contact at the left foot.",
343 "Transition time in seconds (double)")));
345 docCommandVoid0(
"Adds an SE3 task for the right hand.")));
347 docCommandVoid1(
"Removes the SE3 task for the right hand.",
348 "Transition time in seconds (double)")));
350 docCommandVoid0(
"Raises the left hand.")));
351 addCommand(
"removeTaskLeftHand",
353 docCommandVoid1(
"lowers the left hand.",
"Transition time in seconds (double)")));
360 SEND_MSG(
"CoM offset updated: " + toString(
m_com_offset), MSG_TYPE_INFO);
365 SEND_MSG(
"Remove right foot contact in " + toString(transitionTime) +
" s", MSG_TYPE_INFO);
369 SEND_MSG(
"Error while remove right foot contact." + tsid::solvers::HQPDataToString(hqpData,
false),
372 const double& w_feet = m_w_feetSIN.accessCopy();
374 if (transitionTime >
m_dt) {
384 SEND_MSG(
"Remove left foot contact in " + toString(transitionTime) +
" s", MSG_TYPE_INFO);
388 SEND_MSG(
"Error while remove right foot contact." + tsid::solvers::HQPDataToString(hqpData,
false),
391 const double& w_feet = m_w_feetSIN.accessCopy();
393 if (transitionTime >
m_dt) {
403 SEND_MSG(
"Adds right hand SE3 task in " , MSG_TYPE_INFO);
404 const double& w_hands = m_w_handsSIN.accessCopy();
419 SEND_MSG(
"Raise left hand in " , MSG_TYPE_INFO);
420 const double& w_hands = m_w_handsSIN.accessCopy();
435 SEND_MSG(
"Add right foot contact in " + toString(transitionTime) +
" s", MSG_TYPE_INFO);
436 const double& w_forces = m_w_forcesSIN.accessCopy();
445 SEND_MSG(
"Add left foot contact in " + toString(transitionTime) +
" s", MSG_TYPE_INFO);
446 const double& w_forces = m_w_forcesSIN.accessCopy();
455 SEND_MSG(
"Removes right hand SE3 task in " + toString(transitionTime) +
" s", MSG_TYPE_INFO);
463 SEND_MSG(
"Removes left hand SE3 task in " + toString(transitionTime) +
" s", MSG_TYPE_INFO);
470 if (dt <= 0.0)
return SEND_MSG(
"Init failed: Timestep must be positive", MSG_TYPE_ERROR);
473 std::string localName(robotRef);
474 if (isNameInRobotUtil(localName))
477 SEND_MSG(
"You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
481 const Eigen::Matrix<double, 3, 4>& contactPoints = m_contact_pointsSIN(0);
482 const Eigen::Vector3d& contactNormal = m_contact_normalSIN(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);
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));
505 const double& w_forces = m_w_forcesSIN(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);
514 vector<string> package_dirs;
515 m_robot =
new robots::RobotWrapper(
m_robot_util->m_urdf_filename, package_dirs, pinocchio::JointModelFreeFlyer());
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);
538 contactPoints, contactNormal, mu, fMin, fMaxRF);
544 contactPoints, contactNormal, mu, fMin, fMaxLF);
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());
590 m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST,
"eiquadprog-fast");
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);
609 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints)) s.resize(m_robot_util->m_nbJoints);
611 const Eigen::VectorXd& active_joints_sot = m_active_jointsSIN(iter);
612 if (m_enabled ==
false) {
613 if (active_joints_sot.any()) {
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);
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;
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);
635 }
else if (!active_joints_sot.any()) {
639 if (m_enabled ==
false)
640 for (
unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) s(i) =
false;
645 if (!m_initSucceeded) {
646 SEND_WARNING_STREAM_MSG(
"Cannot compute signal tau_des before initialization!");
649 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints)) s.resize(m_robot_util->m_nbJoints);
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);
660 if (m_contactState == DOUBLE_SUPPORT) {
662 removeLeftFootContact(0.0);
664 removeRightFootContact(0.0);
667 addRightFootContact(0.0);
669 addLeftFootContact(0.0);
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;
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));
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);
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));
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);
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);
748 if (m_rightHandState == TASK_RIGHT_HAND_ON ) {
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);
761 if (m_leftHandState == TASK_LEFT_HAND_ON ) {
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);
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);
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) {
790 m_invDyn->updateTaskWeight(m_taskCom->name(), w_com);
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) {
801 m_w_posture = w_posture;
802 m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture);
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);
828 m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
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);
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)) {
846 m_timeLast =
static_cast<unsigned int>(iter);
848 const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
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);
861 getStatistics().store(
"solver dynamic size", 1.0);
863 const HQPOutput& sol = solver->solve(hqpData);
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));
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());
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);
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));
899 if (!m_initSucceeded) {
900 SEND_WARNING_STREAM_MSG(
"Cannot compute signal M before initialization!");
903 if (s.cols() != m_robot->nv() || s.rows() != m_robot->nv()) s.resize(m_robot->nv(), m_robot->nv());
905 s = m_robot->mass(m_invDyn->data());
910 if (!m_initSucceeded) {
911 SEND_WARNING_STREAM_MSG(
"Cannot compute signal dv_des before initialization!");
914 if (s.size() != m_robot->nv()) s.resize(m_robot->nv());
921 if (!m_initSucceeded) {
922 SEND_WARNING_STREAM_MSG(
"Cannot compute signal f_des_right_foot before initialization!");
925 if (s.size() != 6) s.resize(6);
927 if (m_contactState == LEFT_SUPPORT) {
936 if (!m_initSucceeded) {
937 SEND_WARNING_STREAM_MSG(
"Cannot compute signal f_des_left_foot before initialization!");
940 if (s.size() != 6) s.resize(6);
942 if (m_contactState == RIGHT_SUPPORT) {
951 if (!m_initSucceeded) {
952 SEND_WARNING_STREAM_MSG(
"Cannot compute signal com_acc_des before initialization!");
955 if (s.size() != 3) s.resize(3);
957 s = m_taskCom->getDesiredAcceleration();
962 if (!m_initSucceeded) {
963 SEND_WARNING_STREAM_MSG(
"Cannot compute signal com_acc before initialization!");
966 if (s.size() != 3) s.resize(3);
968 s = m_taskCom->getAcceleration(m_dv_urdf);
973 if (!m_initSucceeded) {
974 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmp_des_right_foot_local before initialization!");
977 if (s.size() != 2) s.resize(2);
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;
985 m_zmp_des_RF_local.setZero();
987 s = m_zmp_des_RF_local.head<2>();
992 if (!m_initSucceeded) {
993 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmp_des_left_foot_local before initialization!");
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;
1003 m_zmp_des_LF_local.setZero();
1005 s = m_zmp_des_LF_local.head<2>();
1010 if (!m_initSucceeded) {
1011 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmp_des_right_foot before initialization!");
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);
1023 m_zmp_des_RF.setZero();
1025 m_zmp_des_RF = H_rf.act(m_zmp_des_RF);
1026 s = m_zmp_des_RF.head<2>();
1031 if (!m_initSucceeded) {
1032 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmp_des_left_foot before initialization!");
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);
1044 m_zmp_des_LF.setZero();
1046 m_zmp_des_LF = H_lf.act(m_zmp_des_LF);
1047 s = m_zmp_des_LF.head<2>();
1052 if (!m_initSucceeded) {
1053 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmp_des before initialization!");
1056 if (s.size() != 2) s.resize(2);
1057 m_zmp_des_left_footSOUT(iter);
1058 m_zmp_des_right_footSOUT(iter);
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>();
1066 if (!m_initSucceeded) {
1067 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmp_ref before initialization!");
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);
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));
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);
1083 zmp_LF = H_lf.act(zmp_LF);
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);
1093 zmp_RF = H_rf.act(zmp_RF);
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));
1101 if (!m_initSucceeded) {
1102 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmp_right_foot before initialization!");
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);
1117 m_zmp_RF = H_rf.act(m_zmp_RF);
1118 s = m_zmp_RF.head<2>();
1123 if (!m_initSucceeded) {
1124 SEND_WARNING_STREAM_MSG(
"Cannot compute signal zmp_left_foot before initialization!");
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);
1138 m_zmp_LF = H_lf.act(m_zmp_LF);
1139 s = m_zmp_LF.head<2>();
1144 if (!m_initSucceeded) {
1145 std::ostringstream oss(
"Cannot compute signal zmp before initialization!");
1147 SEND_WARNING_STREAM_MSG(oss.str());
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);
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>();
1162 if (!m_initSucceeded) {
1163 std::ostringstream oss(
"Cannot compute signal com before initialization! iter:");
1165 SEND_WARNING_STREAM_MSG(oss.str());
1168 if (s.size() != 3) s.resize(3);
1169 const Vector3& com = m_robot->com(m_invDyn->data());
1170 s = com + m_com_offset;
1175 if (!m_initSucceeded) {
1176 std::ostringstream oss(
"Cannot compute signal com_vel before initialization! iter:");
1178 SEND_WARNING_STREAM_MSG(oss.str());
1181 if (s.size() != 3) s.resize(3);
1182 const Vector3& com_vel = m_robot->com_vel(m_invDyn->data());
1188 if (!m_initSucceeded) {
1189 std::ostringstream oss(
"Cannot compute signal com_vel before initialization! iter:");
1191 SEND_WARNING_STREAM_MSG(oss.str());
1201 if (!m_initSucceeded) {
1202 std::ostringstream oss(
"Cannot compute signal left_foot_pos before initialization! iter:");
1204 SEND_WARNING_STREAM_MSG(oss.str());
1207 m_tau_desSOUT(iter);
1210 m_robot->framePosition(m_invDyn->data(), m_frame_id_lf, oMi);
1211 tsid::math::SE3ToVector(oMi, s);
1216 if (!m_initSucceeded) {
1217 SEND_WARNING_STREAM_MSG(
"Cannot compute signal left hand_pos before initialization!");
1220 m_tau_desSOUT(iter);
1223 m_robot->framePosition(m_invDyn->data(), m_frame_id_lh, oMi);
1224 tsid::math::SE3ToVector(oMi, s);
1229 if (!m_initSucceeded) {
1230 std::ostringstream oss(
"Cannot compute signal rigt_foot_pos before initialization! iter:");
1232 SEND_WARNING_STREAM_MSG(oss.str());
1235 m_tau_desSOUT(iter);
1238 m_robot->framePosition(m_invDyn->data(), m_frame_id_rf, oMi);
1239 tsid::math::SE3ToVector(oMi, s);
1244 if (!m_initSucceeded) {
1245 SEND_WARNING_STREAM_MSG(
"Cannot compute signal right_hand_pos before initialization!");
1248 m_tau_desSOUT(iter);
1251 m_robot->framePosition(m_invDyn->data(), m_frame_id_rh, oMi);
1252 tsid::math::SE3ToVector(oMi, s);
1257 if (!m_initSucceeded) {
1258 std::ostringstream oss(
"Cannot compute signal left_foot_vel before initialization!");
1260 SEND_WARNING_STREAM_MSG(oss.str());
1263 pinocchio::Motion v;
1264 m_robot->frameVelocity(m_invDyn->data(), m_frame_id_lf, v);
1270 if (!m_initSucceeded) {
1271 std::ostringstream oss(
"Cannot compute signal left_hand_vel before initialization!:");
1273 SEND_WARNING_STREAM_MSG(oss.str());
1276 pinocchio::Motion v;
1277 m_robot->frameVelocity(m_invDyn->data(), m_frame_id_lh, v);
1283 if (!m_initSucceeded) {
1284 SEND_WARNING_STREAM_MSG(
"Cannot compute signal right_foot_vel before initialization! iter:" + toString(iter));
1287 pinocchio::Motion v;
1288 m_robot->frameVelocity(m_invDyn->data(), m_frame_id_rf, v);
1294 if (!m_initSucceeded) {
1295 SEND_WARNING_STREAM_MSG(
"Cannot compute signal right_hand_vel before initialization! " + toString(iter));
1298 pinocchio::Motion v;
1299 m_robot->frameVelocity(m_invDyn->data(), m_frame_id_rh, v);
1305 if (!m_initSucceeded) {
1306 SEND_WARNING_STREAM_MSG(
"Cannot compute signal left_foot_acc before initialization! " + toString(iter));
1309 m_tau_desSOUT(iter);
1310 if (m_contactState == RIGHT_SUPPORT)
1311 s = m_taskLF->getAcceleration(m_dv_urdf);
1313 s = m_contactLF->getMotionTask().getAcceleration(m_dv_urdf);
1318 if (!m_initSucceeded) {
1319 SEND_WARNING_STREAM_MSG(
"Cannot compute signal left_hand_acc before initialization!");
1322 m_tau_desSOUT(iter);
1323 s = m_contactLH->getMotionTask().getAcceleration(m_dv_urdf);
1328 if (!m_initSucceeded) {
1329 SEND_WARNING_STREAM_MSG(
"Cannot compute signal right_foot_acc before initialization!");
1332 m_tau_desSOUT(iter);
1333 if (m_contactState == LEFT_SUPPORT)
1334 s = m_taskRF->getAcceleration(m_dv_urdf);
1336 s = m_contactRF->getMotionTask().getAcceleration(m_dv_urdf);
1341 if (!m_initSucceeded) {
1342 SEND_WARNING_STREAM_MSG(
"Cannot compute signal right_hand_acc before initialization!");
1345 m_tau_desSOUT(iter);
1346 s = m_contactRH->getMotionTask().getAcceleration(m_dv_urdf);
1351 if (!m_initSucceeded) {
1352 SEND_WARNING_STREAM_MSG(
"Cannot compute signal left_foot_acc_des before initialization!");
1355 m_tau_desSOUT(iter);
1356 if (m_contactState == RIGHT_SUPPORT)
1357 s = m_taskLF->getDesiredAcceleration();
1359 s = m_contactLF->getMotionTask().getDesiredAcceleration();
1364 if (!m_initSucceeded) {
1365 SEND_WARNING_STREAM_MSG(
"Cannot compute signal right_foot_acc_des before initialization!");
1368 m_tau_desSOUT(iter);
1369 if (m_contactState == LEFT_SUPPORT)
1370 s = m_taskRF->getDesiredAcceleration();
1372 s = m_contactRF->getMotionTask().getDesiredAcceleration();
1383 os <<
"InverseDynamicsBalanceController " << getName();
1385 getProfiler().report_all(3, os);
1386 getStatistics().report_all(1, os);
1388 }
catch (ExceptionSignal e) {
SolverHQuadProgRT< 60, 36, 34 > SolverHQuadProgRT60x36x34
int m_frame_id_rf
end time of the current transition (if any)
void removeLeftFootContact(const double &transitionTime)
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN6
tsid::trajectories::TrajectorySample m_sampleCom
#define PROFILE_READ_INPUT_SIGNALS
int m_frame_id_lf
frame id of right foot
Eigen::Matrix< double, 3, 1 > Vector3
tsid::math::Vector m_dv_sot
tsid::math::Vector3 m_zmp_des_LF
3d CoM offset
tsid::math::Vector3 m_zmp_des
3d desired zmp left foot expressed in local frame
RightHandState m_rightHandState
tsid::solvers::SolverHQPBase * m_hqpSolver_60_36_34
void init(const double &dt, const std::string &robotRef)
SolverHQuadProgRT< 48, 30, 17 > SolverHQuadProgRT48x30x17
tsid::math::Vector3 m_zmp_des_RF_local
3d desired zmp left foot expressed in local frame
tsid::math::Vector3 m_com_offset
desired 6d wrench left foot
tsid::InverseDynamicsFormulationAccForce * m_invDyn
tsid::math::Vector3 m_zmp_RF
3d zmp left foot
double m_t
control loop time period
tsid::trajectories::TrajectorySample m_samplePosture
RobotUtilShrPtr m_robot_util
#define PROFILE_HQP_SOLUTION
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN
Eigen::Matrix< double, 2, 1 > Vector2
tsid::tasks::TaskComEquality * m_taskCom
tsid::math::Vector m_tau_sot
3d global zmp
ContactState m_contactState
EIGEN_MAKE_ALIGNED_OPERATOR_NEW InverseDynamicsBalanceController(const std::string &name)
DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector)
#define ZERO_FORCE_THRESHOLD
tsid::math::Vector3 m_zmp_LF
3d desired global zmp
tsid::tasks::TaskSE3Equality * m_taskLF
tsid::math::Vector m_q_urdf
tsid::math::Vector3 m_zmp_des_RF
3d desired zmp left foot
virtual void display(std::ostream &os) const
void removeTaskRightHand(const double &transitionTime)
tsid::math::Vector6 m_v_LF_int
tsid::tasks::TaskJointPosture * m_taskPosture
tsid::contacts::Contact6d * m_contactRF
Eigen::Matrix< double, 6, 1 > Vector6
tsid::math::Vector m_v_urdf
void removeTaskLeftHand(const double &transitionTime)
#define PROFILE_TAU_DES_COMPUTATION
tsid::tasks::TaskSE3Equality * m_taskLH
tsid::trajectories::TrajectorySample m_sampleRH
tsid::solvers::SolverHQPBase * m_hqpSolver
int m_frame_id_lh
frame id of right hand
tsid::math::Vector3 m_zmp_des_LF_local
3d desired zmp left foot
LeftHandState m_leftHandState
double m_contactTransitionTime
tsid::robots::RobotWrapper * m_robot
frame id of left hand
tsid::math::Vector3 m_zmp
3d zmp left foot
#define PROFILE_PREPARE_INV_DYN
AdmittanceController EntityClassName
tsid::contacts::Contact6d * m_contactLF
tsid::tasks::TaskSE3Equality * m_taskRH
tsid::math::Vector6 m_v_RF_int
void addLeftFootContact(const double &transitionTime)
void addRightFootContact(const double &transitionTime)
void removeRightFootContact(const double &transitionTime)
int m_frame_id_rh
frame id of left foot
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
tsid::solvers::SolverHQPBase * m_hqpSolver_48_30_17
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
tsid::math::Vector m_f
desired accelerations (urdf order)
tsid::tasks::TaskSE3Equality * m_taskRF