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.