6 #define EIGEN_RUNTIME_NO_MALLOC 8 #include <dynamic-graph/factory.h> 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> 23 #define ODEBUG(x) std::cout << x << std::endl 27 #define ODEBUG3(x) std::cout << x << std::endl 29 #define DBGFILE "/tmp/debug-sot-torqe-control.dat" 31 #define RESETDEBUG5() \ 33 std::ofstream DebugFile; \ 34 DebugFile.open(DBGFILE, std::ofstream::out); \ 37 #define ODEBUG5FULL(x) \ 39 std::ofstream DebugFile; \ 40 DebugFile.open(DBGFILE, std::ofstream::app); \ 41 DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \ 42 << "):" << x << std::endl; \ 47 std::ofstream DebugFile; \ 48 DebugFile.open(DBGFILE, std::ofstream::app); \ 49 DebugFile << x << std::endl; \ 54 #define ODEBUG4FULL(x) 60 namespace dg = ::dynamicgraph;
75 #define REQUIRE_FINITE(A) assert(is_finite(A)) 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" 83 #define ZERO_FORCE_THRESHOLD 1e-3 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 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 134 typedef Eigen::Matrix<double, Eigen::Dynamic, 1>
VectorN;
135 typedef Eigen::Matrix<double, Eigen::Dynamic, 1>
VectorN6;
138 "InverseDynamicsBalanceController");
144 const std::string& name)
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),
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),
204 CONSTRUCT_SIGNAL_IN(dt_joint_pos_limits, double),
205 CONSTRUCT_SIGNAL_IN(tau_estimated,
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),
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,
217 CONSTRUCT_SIGNAL_OUT(f_des_left_foot,
dynamicgraph::Vector,
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),
228 m_zmp_des_left_footSOUT << m_zmp_des_right_footSOUT),
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,
259 m_initSucceeded(false),
262 m_contactState(DOUBLE_SUPPORT),
263 m_rightHandState(TASK_RIGHT_HAND_OFF),
264 m_leftHandState(TASK_LEFT_HAND_OFF),
266 m_robot_util(RefVoidRobotUtil()) {
285 docCommandVoid2(
"Initialize the entity.",
286 "Time period in seconds (double)",
287 "Robot reference (string)")));
294 "Update the offset on the CoM based on the CoP measurement.")));
297 "removeRightFootContact",
300 docCommandVoid1(
"Remove the contact at the right foot.",
301 "Transition time in seconds (double)")));
304 "removeLeftFootContact",
305 makeCommandVoid1(*
this,
307 docCommandVoid1(
"Remove the contact at the left foot.",
308 "Transition time in seconds (double)")));
309 addCommand(
"addTaskRightHand",
312 docCommandVoid0(
"Adds an SE3 task for the right hand.")));
313 addCommand(
"removeTaskRightHand",
316 docCommandVoid1(
"Removes the SE3 task for the right hand.",
317 "Transition time in seconds (double)")));
318 addCommand(
"addTaskLeftHand",
321 docCommandVoid0(
"Raises the left hand.")));
322 addCommand(
"removeTaskLeftHand",
325 docCommandVoid1(
"lowers the left hand.",
326 "Transition time in seconds (double)")));
333 SEND_MSG(
"CoM offset updated: " + toString(
m_com_offset), MSG_TYPE_INFO);
337 const double& transitionTime) {
339 SEND_MSG(
"Remove right foot contact in " + toString(transitionTime) +
" s",
344 const HQPData& hqpData =
346 SEND_MSG(
"Error while remove right foot contact." +
347 tsid::solvers::HQPDataToString(hqpData,
false),
350 const double& w_feet = m_w_feetSIN.accessCopy();
352 if (transitionTime >
m_dt) {
361 const double& transitionTime) {
363 SEND_MSG(
"Remove left foot contact in " + toString(transitionTime) +
" s",
368 const HQPData& hqpData =
370 SEND_MSG(
"Error while remove right foot contact." +
371 tsid::solvers::HQPDataToString(hqpData,
false),
374 const double& w_feet = m_w_feetSIN.accessCopy();
376 if (transitionTime >
m_dt) {
387 SEND_MSG(
"Adds right hand SE3 task in " ,
389 const double& w_hands = m_w_handsSIN.accessCopy();
405 SEND_MSG(
"Raise left hand in " ,
407 const double& w_hands = m_w_handsSIN.accessCopy();
421 const double& transitionTime) {
423 SEND_MSG(
"Add right foot contact in " + toString(transitionTime) +
" s",
425 const double& w_forces = m_w_forcesSIN.accessCopy();
433 const double& transitionTime) {
435 SEND_MSG(
"Add left foot contact in " + toString(transitionTime) +
" s",
437 const double& w_forces = m_w_forcesSIN.accessCopy();
445 const double& transitionTime) {
448 "Removes right hand SE3 task in " + toString(transitionTime) +
" s",
456 const double& transitionTime) {
458 SEND_MSG(
"Removes left hand SE3 task in " + toString(transitionTime) +
" s",
466 const std::string& robotRef) {
468 return SEND_MSG(
"Init failed: Timestep must be positive", MSG_TYPE_ERROR);
471 std::string localName(robotRef);
472 if (isNameInRobotUtil(localName))
475 SEND_MSG(
"You should have an entity controller manager initialized before",
480 const Eigen::Matrix<double, 3, 4>& contactPoints = m_contact_pointsSIN(0);
481 const Eigen::Vector3d& contactNormal = m_contact_normalSIN(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);
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));
508 const double& w_forces = m_w_forcesSIN(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);
517 vector<string> package_dirs;
519 new robots::RobotWrapper(
m_robot_util->m_urdf_filename, package_dirs,
520 pinocchio::JointModelFreeFlyer());
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);
543 new Contact6d(
"contact_rfoot", *
m_robot,
545 contactPoints, contactNormal, mu, fMin, fMaxRF);
551 new Contact6d(
"contact_lfoot", *
m_robot,
553 contactPoints, contactNormal, mu, fMin, fMaxLF);
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());
607 m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST,
611 SOLVER_HQP_EIQUADPROG_RT,
"eiquadprog_rt_60_36_34");
613 SOLVER_HQP_EIQUADPROG_RT,
"eiquadprog_rt_48_30_17");
614 }
catch (
const std::exception& e) {
615 std::cout << e.what();
617 "Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename,
630 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints))
633 const Eigen::VectorXd& active_joints_sot = m_active_jointsSIN(iter);
635 if (active_joints_sot.any()) {
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);
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;
650 blocked_joints(i) = 0.0;
651 SEND_MSG(
"Blocked joints: " + toString(blocked_joints.transpose()),
654 TrajectorySample ref_zero(
659 }
else if (!active_joints_sot.any()) {
664 for (
unsigned int i = 0; i <
m_robot_util->m_nbJoints; i++) s(i) =
false;
670 SEND_WARNING_STREAM_MSG(
671 "Cannot compute signal tau_des before initialization!");
674 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints))
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);
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));
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);
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));
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);
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);
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);
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);
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);
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);
876 pinocchio::SE3 H_lf =
m_robot->position(
881 SEND_MSG(
"Setting left foot reference to " + toString(H_lf),
884 pinocchio::SE3 H_rf =
m_robot->position(
889 SEND_MSG(
"Setting right foot reference to " + toString(H_rf),
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),
895 if (
m_timeLast == static_cast<unsigned int>(iter)) {
902 const HQPData& hqpData =
912 getStatistics().store(
"solver fixed size 60_36_34", 1.0);
916 getStatistics().store(
"solver fixed size 48_30_17", 1.0);
918 getStatistics().store(
"solver dynamic size", 1.0);
920 const HQPOutput& sol = solver->solve(hqpData);
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));
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(
939 ddx_com_ref.norm() /
m_taskCom->getConstraint().vector().norm());
943 Eigen::Matrix<double, 12, 1> tmp;
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));
964 SEND_WARNING_STREAM_MSG(
"Cannot compute signal M before initialization!");
976 SEND_WARNING_STREAM_MSG(
977 "Cannot compute signal dv_des before initialization!");
988 SEND_WARNING_STREAM_MSG(
989 "Cannot compute signal f_des_right_foot before initialization!");
992 if (s.size() != 6) s.resize(6);
1004 SEND_WARNING_STREAM_MSG(
1005 "Cannot compute signal f_des_left_foot before initialization!");
1008 if (s.size() != 6) s.resize(6);
1009 m_tau_desSOUT(iter);
1020 SEND_WARNING_STREAM_MSG(
1021 "Cannot compute signal com_acc_des before initialization!");
1024 if (s.size() != 3) s.resize(3);
1025 m_tau_desSOUT(iter);
1026 s =
m_taskCom->getDesiredAcceleration();
1032 SEND_WARNING_STREAM_MSG(
1033 "Cannot compute signal com_acc before initialization!");
1036 if (s.size() != 3) s.resize(3);
1037 m_tau_desSOUT(iter);
1044 SEND_WARNING_STREAM_MSG(
1045 "Cannot compute signal zmp_des_right_foot_local before " 1049 if (s.size() != 2) s.resize(2);
1051 m_f_des_right_footSOUT(iter);
1052 if (fabs(
m_f_RF(2) > 1.0)) {
1065 SEND_WARNING_STREAM_MSG(
1066 "Cannot compute signal zmp_des_left_foot_local before initialization!");
1069 if (s.size() != 2) s.resize(2);
1070 m_f_des_left_footSOUT(iter);
1071 if (fabs(
m_f_LF(2) > 1.0)) {
1084 SEND_WARNING_STREAM_MSG(
1085 "Cannot compute signal zmp_des_right_foot before initialization!");
1088 if (s.size() != 2) s.resize(2);
1089 m_f_des_right_footSOUT(iter);
1090 pinocchio::SE3 H_rf =
m_robot->position(
1093 if (fabs(
m_f_RF(2) > 1.0)) {
1107 SEND_WARNING_STREAM_MSG(
1108 "Cannot compute signal zmp_des_left_foot before initialization!");
1111 if (s.size() != 2) s.resize(2);
1112 m_f_des_left_footSOUT(iter);
1113 pinocchio::SE3 H_lf =
m_robot->position(
1116 if (fabs(
m_f_LF(2) > 1.0)) {
1130 SEND_WARNING_STREAM_MSG(
1131 "Cannot compute signal zmp_des before initialization!");
1134 if (s.size() != 2) s.resize(2);
1135 m_zmp_des_left_footSOUT(iter);
1136 m_zmp_des_right_footSOUT(iter);
1146 SEND_WARNING_STREAM_MSG(
1147 "Cannot compute signal zmp_ref before initialization!");
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);
1154 pinocchio::SE3 H_lf =
m_robot->position(
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);
1164 zmp_LF = H_lf.act(zmp_LF);
1166 pinocchio::SE3 H_rf =
m_robot->position(
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);
1175 zmp_RF = H_rf.act(zmp_RF);
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));
1186 SEND_WARNING_STREAM_MSG(
1187 "Cannot compute signal zmp_right_foot before initialization!");
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(
1196 if (fabs(f(2) > 1.0)) {
1199 m_zmp_RF(2) = -H_rf.translation()(2);
1210 SEND_WARNING_STREAM_MSG(
1211 "Cannot compute signal zmp_left_foot before initialization!");
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(
1219 if (fabs(f(2) > 1.0)) {
1222 m_zmp_LF(2) = -H_lf.translation()(2);
1233 std::ostringstream oss(
"Cannot compute signal zmp before initialization!");
1235 SEND_WARNING_STREAM_MSG(oss.str());
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);
1244 if (f_LF(2) + f_RF(2) > 1.0)
1246 s =
m_zmp.head<2>();
1252 std::ostringstream oss(
1253 "Cannot compute signal com before initialization! iter:");
1255 SEND_WARNING_STREAM_MSG(oss.str());
1258 if (s.size() != 3) s.resize(3);
1266 std::ostringstream oss(
1267 "Cannot compute signal com_vel before initialization! iter:");
1269 SEND_WARNING_STREAM_MSG(oss.str());
1272 if (s.size() != 3) s.resize(3);
1280 std::ostringstream oss(
1281 "Cannot compute signal com_vel before initialization! iter:");
1283 SEND_WARNING_STREAM_MSG(oss.str());
1294 std::ostringstream oss(
1295 "Cannot compute signal left_foot_pos before initialization! iter:");
1297 SEND_WARNING_STREAM_MSG(oss.str());
1300 m_tau_desSOUT(iter);
1304 tsid::math::SE3ToVector(oMi, s);
1310 SEND_WARNING_STREAM_MSG(
1311 "Cannot compute signal left hand_pos before initialization!");
1314 m_tau_desSOUT(iter);
1318 tsid::math::SE3ToVector(oMi, s);
1324 std::ostringstream oss(
1325 "Cannot compute signal rigt_foot_pos before initialization! iter:");
1327 SEND_WARNING_STREAM_MSG(oss.str());
1330 m_tau_desSOUT(iter);
1334 tsid::math::SE3ToVector(oMi, s);
1340 SEND_WARNING_STREAM_MSG(
1341 "Cannot compute signal right_hand_pos before initialization!");
1344 m_tau_desSOUT(iter);
1348 tsid::math::SE3ToVector(oMi, s);
1354 std::ostringstream oss(
1355 "Cannot compute signal left_foot_vel before initialization!");
1357 SEND_WARNING_STREAM_MSG(oss.str());
1360 pinocchio::Motion v;
1368 std::ostringstream oss(
1369 "Cannot compute signal left_hand_vel before initialization!:");
1371 SEND_WARNING_STREAM_MSG(oss.str());
1374 pinocchio::Motion v;
1382 SEND_WARNING_STREAM_MSG(
1383 "Cannot compute signal right_foot_vel before initialization! iter:" +
1387 pinocchio::Motion v;
1395 SEND_WARNING_STREAM_MSG(
1396 "Cannot compute signal right_hand_vel before initialization! " +
1400 pinocchio::Motion v;
1408 SEND_WARNING_STREAM_MSG(
1409 "Cannot compute signal left_foot_acc before initialization! " +
1413 m_tau_desSOUT(iter);
1423 SEND_WARNING_STREAM_MSG(
1424 "Cannot compute signal left_hand_acc before initialization!");
1427 m_tau_desSOUT(iter);
1434 SEND_WARNING_STREAM_MSG(
1435 "Cannot compute signal right_foot_acc before initialization!");
1438 m_tau_desSOUT(iter);
1448 SEND_WARNING_STREAM_MSG(
1449 "Cannot compute signal right_hand_acc before initialization!");
1452 m_tau_desSOUT(iter);
1459 SEND_WARNING_STREAM_MSG(
1460 "Cannot compute signal left_foot_acc_des before initialization!");
1463 m_tau_desSOUT(iter);
1465 s =
m_taskLF->getDesiredAcceleration();
1467 s =
m_contactLF->getMotionTask().getDesiredAcceleration();
1473 SEND_WARNING_STREAM_MSG(
1474 "Cannot compute signal right_foot_acc_des before initialization!");
1477 m_tau_desSOUT(iter);
1479 s =
m_taskRF->getDesiredAcceleration();
1481 s =
m_contactRF->getMotionTask().getDesiredAcceleration();
1492 os <<
"InverseDynamicsBalanceController " << getName();
1494 getProfiler().report_all(3, os);
1495 getStatistics().report_all(1, os);
1497 <<
" nIn " <<
m_invDyn->nIn() <<
"\n";
1498 }
catch (ExceptionSignal e) {
SolverHQuadProgRT< 60, 36, 34 > SolverHQuadProgRT60x36x34
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::tasks::TaskJointPosture * m_taskBlockedJoints
tsid::math::Vector3 m_com_offset
desired 6d wrench left foot
tsid::InverseDynamicsFormulationAccForce * m_invDyn
tsid::contacts::Contact6d * m_contactLH
tsid::math::Vector3 m_zmp_RF
3d zmp left foot
double m_t
control loop time period
bool m_enabled
true if the entity has been successfully initialized
tsid::trajectories::TrajectorySample m_samplePosture
RobotUtilShrPtr m_robot_util
#define PROFILE_HQP_SOLUTION
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN
tsid::math::Vector m_dv_urdf
desired accelerations (sot order)
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
bool m_firstTime
True if controler is enabled.
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
tsid::contacts::Contact6d * m_contactRH
double m_contactTransitionTime
tsid::trajectories::TrajectorySample m_sampleLF
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::math::Vector6 m_f_RF
desired force coefficients (24d)
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::trajectories::TrajectorySample m_sampleLH
tsid::math::Vector6 m_f_LF
desired 6d wrench right foot
tsid::solvers::SolverHQPBase * m_hqpSolver_48_30_17
tsid::trajectories::TrajectorySample m_sampleRF
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
tsid::math::Vector m_f
desired accelerations (urdf order)
tsid::tasks::TaskSE3Equality * m_taskRF