sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
Namespaces | Macros | Typedefs | Functions
inverse-dynamics-balance-controller.cpp File Reference
#include <dynamic-graph/factory.h>
#include <boost/test/unit_test.hpp>
#include <sot/core/debug.hh>
#include <sot/torque_control/commands-helper.hh>
#include <sot/torque_control/inverse-dynamics-balance-controller.hh>
#include <tsid/math/utils.hpp>
#include <tsid/solvers/solver-HQP-eiquadprog-rt.hpp>
#include <tsid/solvers/solver-HQP-eiquadprog.hpp>
#include <tsid/solvers/solver-HQP-factory.hxx>
#include <tsid/solvers/utils.hpp>
#include <tsid/utils/statistics.hpp>
#include <tsid/utils/stop-watch.hpp>
Include dependency graph for inverse-dynamics-balance-controller.cpp:

Go to the source code of this file.

Namespaces

 dynamicgraph
 to read text file
 
 dynamicgraph::sot
 
 dynamicgraph::sot::torque_control
 

Macros

#define DBGFILE   "/tmp/debug-sot-torqe-control.dat"
 
#define EIGEN_RUNTIME_NO_MALLOC
 
#define INPUT_SIGNALS
 
#define ODEBUG(x)
 
#define ODEBUG3(x)   std::cout << x << std::endl
 
#define ODEBUG4(x)
 
#define ODEBUG4FULL(x)
 
#define ODEBUG5(x)
 
#define ODEBUG5FULL(x)
 
#define OUTPUT_SIGNALS
 
#define PROFILE_HQP_SOLUTION   "InvDynBalCtrl: HQP"
 
#define PROFILE_PREPARE_INV_DYN   "InvDynBalCtrl: prepare inv-dyn"
 
#define PROFILE_READ_INPUT_SIGNALS   "InvDynBalCtrl: read input signals"
 
#define PROFILE_TAU_DES_COMPUTATION   "InvDynBalCtrl: desired tau"
 
#define REQUIRE_FINITE(A)   assert(is_finite(A))
 
#define RESETDEBUG4()
 
#define RESETDEBUG5()
 
#define ZERO_FORCE_THRESHOLD   1e-3
 

Typedefs

typedef SolverHQuadProgRT< 48, 30, 17 > SolverHQuadProgRT48x30x17
 
typedef SolverHQuadProgRT< 60, 36, 34 > SolverHQuadProgRT60x36x34
 
typedef Eigen::Matrix< double, 2, 1 > Vector2
 
typedef Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN
 
typedef Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN6
 

Functions

 DEFINE_SIGNAL_INNER_FUNCTION (active_joints_checked, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (base_orientation, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (com, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (com_acc, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (com_acc_des, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (com_vel, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (dv_des, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (f_des_left_foot, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (f_des_right_foot, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (left_foot_acc, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (left_foot_acc_des, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (left_foot_pos, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (left_foot_vel, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (left_hand_acc, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (left_hand_pos, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (left_hand_vel, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (M, dynamicgraph::Matrix)
 
 DEFINE_SIGNAL_OUT_FUNCTION (right_foot_acc, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (right_foot_acc_des, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (right_foot_pos, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (right_foot_vel, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (right_hand_acc, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (right_hand_pos, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (right_hand_vel, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (tau_des, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp_des, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp_des_left_foot, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp_des_left_foot_local, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp_des_right_foot, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp_des_right_foot_local, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp_left_foot, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp_ref, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp_right_foot, dynamicgraph::Vector)
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (InverseDynamicsBalanceController, "InverseDynamicsBalanceController")
 

Macro Definition Documentation

◆ DBGFILE

#define DBGFILE   "/tmp/debug-sot-torqe-control.dat"

Definition at line 29 of file inverse-dynamics-balance-controller.cpp.

◆ EIGEN_RUNTIME_NO_MALLOC

#define EIGEN_RUNTIME_NO_MALLOC

Definition at line 6 of file inverse-dynamics-balance-controller.cpp.

◆ INPUT_SIGNALS

#define INPUT_SIGNALS
Value:
m_com_ref_posSIN << m_com_ref_velSIN << m_com_ref_accSIN << m_rf_ref_posSIN \
<< m_rf_ref_velSIN << m_rf_ref_accSIN << m_lf_ref_posSIN \
<< m_lf_ref_velSIN << m_lf_ref_accSIN << m_rh_ref_posSIN \
<< m_rh_ref_velSIN << m_rh_ref_accSIN << m_lh_ref_posSIN \
<< m_lh_ref_velSIN << m_lh_ref_accSIN \
<< m_posture_ref_posSIN << m_posture_ref_velSIN \
<< m_posture_ref_accSIN << m_base_orientation_ref_posSIN \
<< m_base_orientation_ref_velSIN \
<< m_base_orientation_ref_accSIN << m_f_ref_right_footSIN \
<< m_f_ref_left_footSIN << m_kp_base_orientationSIN \
<< m_kd_base_orientationSIN << m_kp_constraintsSIN \
<< m_kd_constraintsSIN << m_kp_comSIN << m_kd_comSIN \
<< m_kp_feetSIN << m_kd_feetSIN << m_kp_handsSIN \
<< m_kd_handsSIN << m_kp_postureSIN << m_kd_postureSIN \
<< m_kp_posSIN << m_kd_posSIN << m_w_comSIN << m_w_feetSIN \
<< m_w_handsSIN << m_w_postureSIN \
<< m_w_base_orientationSIN << m_w_torquesSIN \
<< m_w_forcesSIN << m_weight_contact_forcesSIN << m_muSIN \
<< m_contact_pointsSIN << m_contact_normalSIN << m_f_minSIN \
<< m_f_max_right_footSIN << m_f_max_left_footSIN \
<< m_rotor_inertiasSIN << m_gear_ratiosSIN << m_tau_maxSIN \
<< m_q_minSIN << m_q_maxSIN << m_dq_maxSIN << m_ddq_maxSIN \
<< m_dt_joint_pos_limitsSIN << m_tau_estimatedSIN << m_qSIN \
<< m_vSIN << m_wrench_baseSIN << m_wrench_left_footSIN \
<< m_wrench_right_footSIN << m_active_jointsSIN

Definition at line 85 of file inverse-dynamics-balance-controller.cpp.

◆ ODEBUG

#define ODEBUG (   x)

Definition at line 25 of file inverse-dynamics-balance-controller.cpp.

◆ ODEBUG3

#define ODEBUG3 (   x)    std::cout << x << std::endl

Definition at line 27 of file inverse-dynamics-balance-controller.cpp.

◆ ODEBUG4

#define ODEBUG4 (   x)

Definition at line 55 of file inverse-dynamics-balance-controller.cpp.

◆ ODEBUG4FULL

#define ODEBUG4FULL (   x)

Definition at line 54 of file inverse-dynamics-balance-controller.cpp.

◆ ODEBUG5

#define ODEBUG5 (   x)
Value:
{ \
std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::app); \
DebugFile << x << std::endl; \
DebugFile.close(); \
}

Definition at line 45 of file inverse-dynamics-balance-controller.cpp.

◆ ODEBUG5FULL

#define ODEBUG5FULL (   x)
Value:
{ \
std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::app); \
DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \
<< "):" << x << std::endl; \
DebugFile.close(); \
}

Definition at line 37 of file inverse-dynamics-balance-controller.cpp.

◆ OUTPUT_SIGNALS

#define OUTPUT_SIGNALS
Value:
m_tau_desSOUT << m_MSOUT << m_dv_desSOUT << m_f_des_right_footSOUT \
<< m_f_des_left_footSOUT << m_zmp_des_right_footSOUT \
<< m_zmp_des_left_footSOUT << m_zmp_des_right_foot_localSOUT \
<< m_zmp_des_left_foot_localSOUT << m_zmp_desSOUT \
<< m_zmp_refSOUT << m_zmp_right_footSOUT \
<< m_zmp_left_footSOUT << m_zmpSOUT << m_comSOUT \
<< m_com_velSOUT << m_com_accSOUT << m_com_acc_desSOUT \
<< m_base_orientationSOUT << m_left_foot_posSOUT \
<< m_right_foot_posSOUT << m_left_hand_posSOUT \
<< m_right_hand_posSOUT << m_left_foot_velSOUT \
<< m_right_foot_velSOUT << m_left_hand_velSOUT \
<< m_right_hand_velSOUT << m_left_foot_accSOUT \
<< m_right_foot_accSOUT << m_left_hand_accSOUT \
<< m_right_hand_accSOUT << m_right_foot_acc_desSOUT \
<< m_left_foot_acc_desSOUT

Definition at line 112 of file inverse-dynamics-balance-controller.cpp.

◆ PROFILE_HQP_SOLUTION

#define PROFILE_HQP_SOLUTION   "InvDynBalCtrl: HQP"

Definition at line 79 of file inverse-dynamics-balance-controller.cpp.

◆ PROFILE_PREPARE_INV_DYN

#define PROFILE_PREPARE_INV_DYN   "InvDynBalCtrl: prepare inv-dyn"

Definition at line 80 of file inverse-dynamics-balance-controller.cpp.

◆ PROFILE_READ_INPUT_SIGNALS

#define PROFILE_READ_INPUT_SIGNALS   "InvDynBalCtrl: read input signals"

Definition at line 81 of file inverse-dynamics-balance-controller.cpp.

◆ PROFILE_TAU_DES_COMPUTATION

#define PROFILE_TAU_DES_COMPUTATION   "InvDynBalCtrl: desired tau"

Definition at line 78 of file inverse-dynamics-balance-controller.cpp.

◆ REQUIRE_FINITE

#define REQUIRE_FINITE (   A)    assert(is_finite(A))

Definition at line 75 of file inverse-dynamics-balance-controller.cpp.

◆ RESETDEBUG4

#define RESETDEBUG4 ( )

Definition at line 53 of file inverse-dynamics-balance-controller.cpp.

◆ RESETDEBUG5

#define RESETDEBUG5 ( )
Value:
{ \
std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::out); \
DebugFile.close(); \
}

Definition at line 31 of file inverse-dynamics-balance-controller.cpp.

◆ ZERO_FORCE_THRESHOLD

#define ZERO_FORCE_THRESHOLD   1e-3

Definition at line 83 of file inverse-dynamics-balance-controller.cpp.