simple-inverse-dyn.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/simple-inverse-dyn.hh>
#include <tsid/math/utils.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 simple-inverse-dyn.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-torque-control.dat"
#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   m_tau_desSOUT << m_dv_desSOUT << m_v_desSOUT << m_q_desSOUT << m_uSOUT
#define	PROFILE_HQP_SOLUTION   "SimpleInverseDyn: HQP"
#define	PROFILE_PREPARE_INV_DYN   "SimpleInverseDyn: prepare inv-dyn"
#define	PROFILE_READ_INPUT_SIGNALS   "SimpleInverseDyn: read input signals"
#define	PROFILE_TAU_DES_COMPUTATION   "SimpleInverseDyn: desired tau"
#define	REQUIRE_FINITE(A)   assert(is_finite(A))
#define	RESETDEBUG4()
#define	RESETDEBUG5()
#define	ZERO_FORCE_THRESHOLD   1e-3

Functions
	DEFINE_SIGNAL_INNER_FUNCTION (active_joints_checked, dynamicgraph::Vector)
	DEFINE_SIGNAL_OUT_FUNCTION (dv_des, dynamicgraph::Vector)
	DEFINE_SIGNAL_OUT_FUNCTION (q_des, dynamicgraph::Vector)
	DEFINE_SIGNAL_OUT_FUNCTION (tau_des, dynamicgraph::Vector)
	DEFINE_SIGNAL_OUT_FUNCTION (u, dynamicgraph::Vector)
	DEFINE_SIGNAL_OUT_FUNCTION (v_des, dynamicgraph::Vector)
	DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (SimpleInverseDyn, "SimpleInverseDyn")

Macro Definition Documentation

DBGFILE

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

Definition at line 36 of file simple-inverse-dyn.cpp.

INPUT_SIGNALS

#define INPUT_SIGNALS

Value:

  m_posture_ref_posSIN << m_posture_ref_velSIN << m_posture_ref_accSIN         \
                       << m_kp_postureSIN << m_kd_postureSIN << m_w_postureSIN \
                       << m_kp_posSIN << m_kd_posSIN << m_com_ref_posSIN       \
                       << m_com_ref_velSIN << m_com_ref_accSIN << m_kp_comSIN  \
                       << m_kd_comSIN << m_w_comSIN << m_kp_contactSIN         \
                       << m_kd_contactSIN << m_w_forcesSIN << m_muSIN          \
                       << m_contact_pointsSIN << m_contact_normalSIN           \
                       << m_f_minSIN << m_f_maxSIN << m_waist_ref_posSIN       \
                       << m_waist_ref_velSIN << m_waist_ref_accSIN             \
                       << m_kp_waistSIN << m_kd_waistSIN << m_w_waistSIN       \
                       << m_qSIN << m_vSIN << m_active_jointsSIN

Definition at line 89 of file simple-inverse-dyn.cpp.

ODEBUG

#define ODEBUG

(

x

)

Definition at line 32 of file simple-inverse-dyn.cpp.

ODEBUG3

#define ODEBUG3

(

x

)

std::cout << x << std::endl

Definition at line 34 of file simple-inverse-dyn.cpp.

ODEBUG4

#define ODEBUG4

(

x

)

Definition at line 62 of file simple-inverse-dyn.cpp.

ODEBUG4FULL

#define ODEBUG4FULL

(

x

)

Definition at line 61 of file simple-inverse-dyn.cpp.

ODEBUG5

#define ODEBUG5

(

x

)

Value:

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

Definition at line 52 of file simple-inverse-dyn.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 44 of file simple-inverse-dyn.cpp.

OUTPUT_SIGNALS

#define OUTPUT_SIGNALS   m_tau_desSOUT << m_dv_desSOUT << m_v_desSOUT << m_q_desSOUT << m_uSOUT

Definition at line 102 of file simple-inverse-dyn.cpp.

PROFILE_HQP_SOLUTION

#define PROFILE_HQP_SOLUTION   "SimpleInverseDyn: HQP"

Definition at line 83 of file simple-inverse-dyn.cpp.

PROFILE_PREPARE_INV_DYN

#define PROFILE_PREPARE_INV_DYN   "SimpleInverseDyn: prepare inv-dyn"

Definition at line 84 of file simple-inverse-dyn.cpp.

PROFILE_READ_INPUT_SIGNALS

#define PROFILE_READ_INPUT_SIGNALS   "SimpleInverseDyn: read input signals"

Definition at line 85 of file simple-inverse-dyn.cpp.

PROFILE_TAU_DES_COMPUTATION

#define PROFILE_TAU_DES_COMPUTATION   "SimpleInverseDyn: desired tau"

Definition at line 82 of file simple-inverse-dyn.cpp.

REQUIRE_FINITE

#define REQUIRE_FINITE

(

A

)

assert(is_finite(A))

Definition at line 79 of file simple-inverse-dyn.cpp.

RESETDEBUG4

#define RESETDEBUG4

(

)

Definition at line 60 of file simple-inverse-dyn.cpp.

RESETDEBUG5

#define RESETDEBUG5

(

)

Value:

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

Definition at line 38 of file simple-inverse-dyn.cpp.

ZERO_FORCE_THRESHOLD

#define ZERO_FORCE_THRESHOLD   1e-3

Definition at line 87 of file simple-inverse-dyn.cpp.