#include <dynamic-graph/factory.h>
#include <sot/core/debug.hh>
#include <sot/core/stop-watch.hh>
#include <sot/torque_control/base-estimator.hh>
#include <sot/torque_control/commands-helper.hh>
#include "pinocchio/algorithm/frames.hpp"

Include dependency graph for base-estimator.cpp:

Namespaces
	dynamicgraph
	to read text file

	dynamicgraph::sot

	dynamicgraph::sot::torque_control

Macros
#define	INPUT_SIGNALS

#define	OUTPUT_SIGNALS

#define	PROFILE_BASE_KINEMATICS_COMPUTATION "base-est kinematics computation"

#define	PROFILE_BASE_POSITION_ESTIMATION "base-est position estimation"

#define	PROFILE_BASE_VELOCITY_ESTIMATION "base-est velocity estimation"

Functions
	DEFINE_SIGNAL_INNER_FUNCTION (kinematics_computations, dynamicgraph::Vector)

	DEFINE_SIGNAL_OUT_FUNCTION (a_ac, dynamicgraph::Vector)

	DEFINE_SIGNAL_OUT_FUNCTION (lf_xyzquat, dynamicgraph::Vector)

	DEFINE_SIGNAL_OUT_FUNCTION (q, dynamicgraph::Vector)

	DEFINE_SIGNAL_OUT_FUNCTION (q_imu, dynamicgraph::Vector)

	DEFINE_SIGNAL_OUT_FUNCTION (q_lf, dynamicgraph::Vector)

	DEFINE_SIGNAL_OUT_FUNCTION (q_rf, dynamicgraph::Vector)

	DEFINE_SIGNAL_OUT_FUNCTION (rf_xyzquat, dynamicgraph::Vector)

	DEFINE_SIGNAL_OUT_FUNCTION (v, dynamicgraph::Vector)

	DEFINE_SIGNAL_OUT_FUNCTION (v_ac, dynamicgraph::Vector)

	DEFINE_SIGNAL_OUT_FUNCTION (v_flex, dynamicgraph::Vector)

	DEFINE_SIGNAL_OUT_FUNCTION (v_gyr, dynamicgraph::Vector)

	DEFINE_SIGNAL_OUT_FUNCTION (v_imu, dynamicgraph::Vector)

	DEFINE_SIGNAL_OUT_FUNCTION (v_kin, dynamicgraph::Vector)

	DEFINE_SIGNAL_OUT_FUNCTION (w_lf, double)

	DEFINE_SIGNAL_OUT_FUNCTION (w_lf_filtered, double)

	DEFINE_SIGNAL_OUT_FUNCTION (w_rf, double)

	DEFINE_SIGNAL_OUT_FUNCTION (w_rf_filtered, double)

	DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (BaseEstimator, "BaseEstimator")

void	matrixToRpy (const Eigen::Matrix3d &M, Eigen::Vector3d &rpy)

void	pointRotationByQuaternion (const Eigen::Vector3d &point, const Eigen::Vector4d &quat, Eigen::Vector3d &rotatedPoint)

void	quanternionMult (const Eigen::Vector4d &q1, const Eigen::Vector4d &q2, Eigen::Vector4d &q12)

void	rpyToMatrix (const Eigen::Vector3d &rpy, Eigen::Matrix3d &R)

void	rpyToMatrix (double r, double p, double y, Eigen::Matrix3d &R)

void	se3Interp (const pinocchio::SE3 &s1, const pinocchio::SE3 &s2, const double alpha, pinocchio::SE3 &s12)

Macro Definition Documentation

◆ INPUT_SIGNALS

#define INPUT_SIGNALS

Value:

  m_joint_positionsSIN << m_joint_velocitiesSIN << m_imu_quaternionSIN        \
                       << m_forceLLEGSIN << m_forceRLEGSIN << m_dforceLLEGSIN \
                       << m_dforceRLEGSIN << m_w_lf_inSIN << m_w_rf_inSIN     \
                       << m_K_fb_feet_posesSIN << m_lf_ref_xyzquatSIN         \
                       << m_rf_ref_xyzquatSIN << m_accelerometerSIN           \
                       << m_gyroscopeSIN

Definition at line 84 of file base-estimator.cpp.

◆ OUTPUT_SIGNALS

#define OUTPUT_SIGNALS

Value:

  m_qSOUT << m_vSOUT << m_v_kinSOUT << m_v_flexSOUT << m_v_imuSOUT             \
          << m_v_gyrSOUT << m_lf_xyzquatSOUT << m_rf_xyzquatSOUT << m_a_acSOUT \
          << m_v_acSOUT << m_q_lfSOUT << m_q_rfSOUT << m_q_imuSOUT             \
          << m_w_lfSOUT << m_w_rfSOUT << m_w_lf_filteredSOUT                   \
          << m_w_rf_filteredSOUT

Definition at line 91 of file base-estimator.cpp.

◆ PROFILE_BASE_KINEMATICS_COMPUTATION

#define PROFILE_BASE_KINEMATICS_COMPUTATION "base-est kinematics computation"

Definition at line 82 of file base-estimator.cpp.

◆ PROFILE_BASE_POSITION_ESTIMATION

#define PROFILE_BASE_POSITION_ESTIMATION "base-est position estimation"

Definition at line 80 of file base-estimator.cpp.

◆ PROFILE_BASE_VELOCITY_ESTIMATION

#define PROFILE_BASE_VELOCITY_ESTIMATION "base-est velocity estimation"

Definition at line 81 of file base-estimator.cpp.

Namespaces