sot-torque-control  1.5.3
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
inverse-dynamics-balance-controller.cpp File Reference
#include <boost/test/unit_test.hpp>
#include <tsid/utils/stop-watch.hpp>
#include <tsid/utils/statistics.hpp>
#include <tsid/solvers/solver-HQP-factory.hxx>
#include <tsid/solvers/solver-HQP-eiquadprog.hpp>
#include <tsid/solvers/solver-HQP-eiquadprog-rt.hpp>
#include <tsid/solvers/utils.hpp>
#include <tsid/math/utils.hpp>
#include <dynamic-graph/factory.h>
#include <sot/core/debug.hh>
#include <sot/torque_control/commands-helper.hh>
#include <sot/torque_control/inverse-dynamics-balance-controller.hh>
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 32 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

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

◆ ODEBUG

#define ODEBUG (   x)

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

◆ ODEBUG3

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

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

◆ ODEBUG4

#define ODEBUG4 (   x)

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

◆ ODEBUG4FULL

#define ODEBUG4FULL (   x)

Definition at line 56 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 47 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 40 of file inverse-dynamics-balance-controller.cpp.

◆ OUTPUT_SIGNALS

#define OUTPUT_SIGNALS

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

◆ PROFILE_HQP_SOLUTION

#define PROFILE_HQP_SOLUTION   "InvDynBalCtrl: HQP"

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

◆ PROFILE_PREPARE_INV_DYN

#define PROFILE_PREPARE_INV_DYN   "InvDynBalCtrl: prepare inv-dyn"

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

◆ PROFILE_READ_INPUT_SIGNALS

#define PROFILE_READ_INPUT_SIGNALS   "InvDynBalCtrl: read input signals"

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

◆ PROFILE_TAU_DES_COMPUTATION

#define PROFILE_TAU_DES_COMPUTATION   "InvDynBalCtrl: desired tau"

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

◆ REQUIRE_FINITE

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

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

◆ RESETDEBUG4

#define RESETDEBUG4 ( )

Definition at line 55 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 34 of file inverse-dynamics-balance-controller.cpp.

◆ ZERO_FORCE_THRESHOLD

#define ZERO_FORCE_THRESHOLD   1e-3

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

DBGFILE
#define DBGFILE
Definition: inverse-dynamics-balance-controller.cpp:32