Namespace mim_estimation

namespace mim_estimation

Functions

template<class Vector>
inline boost::python::list std_vector_to_py_list(const Vector &vector)

boost::python::list get_measurement(BaseEkfWithImuKin *obj)

void bind_base_ekf_with_imu_kin()

void bind_end_effector_force_estimator()

void bind_robot_state_estimator()

BOOST_PYTHON_MODULE(mim_estimation_cpp)

void bind_robot_state_estimator_settings()

class BaseEkfWithImuKin

#include <base_ekf_with_imu_kin.hpp>

Extended Kalman Filter implementation for base state estimation using the imu and the kinematics information.

Details:

Todo:

explain here the math around the EKF

Public Functions

BaseEkfWithImuKin()

Construct a new Base Ekf With Imu and Kinematics object.

~BaseEkfWithImuKin()

Destroy the Base Ekf With Imu Kin object.

void initialize(const BaseEkfWithImuKinSettings &settings)

Get the EKF settings and initialize the filter from them.

Parameters:

settings –

void set_initial_state(Eigen::Ref<const Eigen::Vector3d> base_position, const Eigen::Quaterniond &base_attitude, Eigen::Ref<const Eigen::Vector3d> base_linear_velocity, Eigen::Ref<const Eigen::Vector3d> base_angular_velocity)

Set initial state.

Parameters:

• base_position – Base position with respect to the world frame.

• base_attitude – Base orientation with respect to the world frame.

• base_linear_velocity – Base linear velocity with respect to the base frame.

• base_angular_velocity – Base angular velocity with respect to the base frame.

void set_initial_state(Eigen::Ref<const Eigen::Matrix<double, 7, 1>> base_se3_position, Eigen::Ref<const Eigen::Matrix<double, 6, 1>> base_se3_velocity)

Set initial state.

Parameters:

• base_se3_position – [XYZ Quaternion] SE3 representation of the base position with respect to the world frame.

• base_se3_velocity – [Linear Angular] base velocity in the base frame.

void update_filter(const std::vector<bool> &contact_schedule, Eigen::Ref<const Eigen::Vector3d> imu_accelerometer, Eigen::Ref<const Eigen::Vector3d> imu_gyroscope, Eigen::Ref<const Eigen::VectorXd> joint_position, Eigen::Ref<const Eigen::VectorXd> joint_velocity)

Feed in the sensor raw data from the robot and update the filter output.

Parameters:

• contact_schedule – vector of boolean, if one boolean is true it means that the corresponding end-effector is in contact.

• imu_accelerometer – imu raw accelerometer data.

• imu_gyroscope – imu raw gyroscope data.

• joint_position – joint position data (in pinocchio ordering).

• joint_velocity – joint velocity data (in pinocchio ordering).

void get_filter_output(Eigen::Ref<Eigen::VectorXd> robot_configuration, Eigen::Ref<Eigen::VectorXd> robot_velocity)

Get the filter output which is the robot state.

Parameters:

• robot_configuration –

• robot_velocity –

const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &get_measurement()

Get measurement.

Parameters:

root_velocities –

struct BaseEkfWithImuKinSettings

#include <base_ekf_with_imu_kin.hpp>

Subclassed by mim_estimation::RobotStateEstimatorSettings

Public Functions

inline virtual std::string to_string()

Public Members

bool is_imu_frame = true

Is the EKF estimating the imu or the base frame.

std::vector<std::string> end_effector_frame_names

Potential contact end effector frame names.

pinocchio::Model pinocchio_model

Rigid body dynamic model.

pinocchio::SE3 imu_in_base = pinocchio::SE3::Identity()

Imu position and orientation in the base frame.

double dt = 0.001

discretization time.

Eigen::Vector3d noise_accelerometer = 0.0001962 * 0.0001962 * (Eigen::Vector3d() << dt, dt, dt).finished()

Accelerometer noise covariance.

Eigen::Vector3d noise_gyroscope = 0.0000873 * 0.0000873 * (Eigen::Vector3d() << dt, dt, dt).finished()

Gyroscope noise covariance.

Eigen::Vector3d noise_bias_accelerometer = 0.0001 * 0.0001 * (Eigen::Vector3d() << 1.0, 1.0, 1.0).finished()

Accelerometer bias noise covariance.

Eigen::Vector3d noise_bias_gyroscope = 0.000309 * 0.000309 * (Eigen::Vector3d() << 1.0, 1.0, 1.0).finished()

Gyroscope bias noise covariance.

Eigen::Vector3d meas_noise_cov = (Eigen::Vector3d() << 1e-5, 1e-5, 1e-5).finished()

Continuous measurement noise covariance.

class EndEffectorForceEstimator

#include <end_effector_force_estimator.hpp>

Public Types

typedef std::map<std::string, pinocchio::FrameIndex> EndEffectorIdMap

typedef std::map<pinocchio::FrameIndex, MatrixX3, std::less<pinocchio::FrameIndex>, Eigen::aligned_allocator<std::pair<const pinocchio::FrameIndex, MatrixX3>>> MatrixX3Map

typedef std::map<pinocchio::FrameIndex, pinocchio::Data::Matrix6x, std::less<pinocchio::FrameIndex>, Eigen::aligned_allocator<std::pair<const pinocchio::FrameIndex, pinocchio::Data::Matrix6x>>> Matrix6xMap

typedef std::map<pinocchio::FrameIndex, Eigen::Matrix<double, 6, 1>, std::less<pinocchio::FrameIndex>, Eigen::aligned_allocator<std::pair<const pinocchio::FrameIndex, Eigen::Matrix<double, 6, 1>>>> EndEffectorForceMap

Public Functions

EndEffectorForceEstimator()

Construct a new EndEffectorForceEstimator object.

No memory allocation,

See also

initialize. Use the default constructor for eigen and pinocchio objects.

~EndEffectorForceEstimator()

Destroy the EndEffectorForceEstimator object.

void initialize(const std::string &urdf_path, const std::vector<std::string> &frame_name = std::vector<std::string>())

void add_contact_frame(const std::string &frame_name)

void add_contact_frame(const std::vector<std::string> &frame_names)

void run(const Eigen::VectorXd &joint_positions, const Eigen::VectorXd &joint_torques)

const Eigen::Matrix<double, 6, 1> &get_force(const std::string &frame_name)

inline std::string to_string()

Public Members

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef Eigen::Matrix< double, Eigen::Dynamic, 3 > MatrixX3

class RobotStateEstimator

#include <robot_state_estimator.hpp>

This class has for purpose to estimate the state of the robot in the current environment.

It really on different classes implemented in this package and provide a uniforme way to interact with them.

For example there is an end-effector force estimation class that can be used for contact detection needed by an EKF. This class creates this pipeline in a transparent way for the user. The user simply need to pick which scientific method are to be used among the available ones.

Public Types

typedef std::map<std::string, Eigen::Vector3d, std::less<std::string>, Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector3d>>> ForceInWorldMap

Public Functions

RobotStateEstimator()

Construct a new Base State Estimator object.

~RobotStateEstimator()

Destroy by default the Base State Estimator object.

void initialize(const RobotStateEstimatorSettings &settings)

void set_initial_state(Eigen::Ref<const Eigen::VectorXd> initial_robot_configuration, Eigen::Ref<const Eigen::VectorXd> initial_robot_velocity)

void run(const std::vector<bool> &contact_schedule, Eigen::Ref<const Eigen::Vector3d> imu_accelerometer, Eigen::Ref<const Eigen::Vector3d> imu_gyroscope, Eigen::Ref<const Eigen::VectorXd> joint_position, Eigen::Ref<const Eigen::VectorXd> joint_velocity)

Estimate the robot base velocity and postion assuming the contact schedule is known.

Parameters:

• contact_schedule –

• imu_accelerometer –

• imu_gyroscope –

• joint_position –

• joint_velocity –

• joint_torque –

void run(Eigen::Ref<const Eigen::Vector3d> imu_accelerometer, Eigen::Ref<const Eigen::Vector3d> imu_gyroscope, Eigen::Ref<const Eigen::VectorXd> joint_position, Eigen::Ref<const Eigen::VectorXd> joint_velocity, Eigen::Ref<const Eigen::VectorXd> joint_torque)

Estimate the robot base velocity and postion and the contact states.

Parameters:

• imu_accelerometer –

• imu_gyroscope –

• joint_position –

• joint_velocity –

• joint_torque –

void get_state(Eigen::Ref<Eigen::VectorXd> robot_configuration, Eigen::Ref<Eigen::VectorXd> robot_velocity)

const Eigen::VectorXd &get_robot_configuration() const

const Eigen::VectorXd &get_robot_velocity() const

const std::vector<bool> &get_detected_contact() const

const Eigen::Vector3d &get_force(const std::string &frame_name)

void set_settings(const RobotStateEstimatorSettings &settings)

const RobotStateEstimatorSettings &get_settings() const

struct RobotStateEstimatorSettings : public mim_estimation::BaseEkfWithImuKinSettings

#include <robot_state_estimator.hpp>

Public Functions

inline virtual std::string to_string()

Convert the current object in human readable string.

Public Members

std::string urdf_path

Path to th robot URDF file.

double force_threshold_up = 10

Threshold on the rising force norm.

double force_threshold_down = 5

Threshold on the decreasing force norm.

struct SystemState

#include <base_ekf_with_imu_kin.hpp>

System state data.

Public Members

Eigen::Vector3d position = Eigen::Vector3d::Zero()

Base/Imu position.

Eigen::Vector3d linear_velocity = Eigen::Vector3d::Zero()

Base/Imu linear velocity.

Eigen::Quaterniond attitude = Eigen::Quaterniond::Identity()

Base/Imu linear attitude.

Eigen::Vector3d bias_accelerometer = Eigen::Vector3d::Zero()

Accelerometer bias.

Eigen::Vector3d bias_gyroscope = Eigen::Vector3d::Zero()

Gyroscope bias.

Eigen::MatrixXd covariance = Eigen::MatrixXd::Zero(state_dim, state_dim)

Process covariance.

Public Static Attributes

static const int state_dim = 15

State dimension.

static const int noise_dim = 12

Process noise dimension.

namespace dynamic_graph

Typedefs

typedef dynamicgraph::SignalTimeDependent<int, int> SignalTrigger

Simple shortcut for code writing convenience.

typedef dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> SignalOut

Simple shortcut for code writing convenience.

typedef dynamicgraph::SignalPtr<dynamicgraph::Vector, int> SignalIn

Simple shortcut for code writing convenience.

Functions

DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (RobotStateEstimator, "RobotStateEstimator")

std::string make_signal_string(const bool &is_input_signal, const std::string &class_name, const std::string &object_name, const std::string &signal_type, const std::string &signal_name)

class RobotStateEstimator : public Entity

#include <robot_state_estimator.hpp>

This class define a dynamic graph wrapper around the vicon client.

Public Functions

DYNAMIC_GRAPH_ENTITY_DECL()

RobotStateEstimator(const std::string &name)

Construct a new Vicon Client Entity object.

Parameters:

name – Entity name.

~RobotStateEstimator()

Destroy the Vicon Client Entity object.

void initialize(const RobotStateEstimatorSettings &settings)

Connect to the vicon system and start a.

Parameters:

host_name –

void set_initial_state(Eigen::Ref<const Eigen::VectorXd> initial_robot_configuration, Eigen::Ref<const Eigen::VectorXd> initial_robot_velocity)

Add a signal that contains the pose of the desired object.

namespace io_tools

class DataCollector

#include <data_collector.hpp>

Subclassed by mim_estimation::io_tools::NonRtDataCollector

Public Functions

inline DataCollector()

inline virtual ~DataCollector()

virtual void addVariable(const double *data, const std::string &name, const std::string &units) = 0

virtual functions to add variables to data collection for different types

virtual void addVariable(const float *data, const std::string &name, const std::string &units) = 0

virtual void addVariable(const int *data, const std::string &name, const std::string &units) = 0

virtual void updateDataCollection() = 0

virtual function to update the collection with the recently added variables

virtual void stopDataCollection() = 0

virtual void startDataCollection() = 0

virtual bool isDataCollectionDone() = 0

virtual function to check whether data collection has completed:

void addVector(const Eigen::Ref<const Eigen::VectorXd> &data, const std::vector<std::string> &name, const std::vector<std::string> &units)

helper functions to unroll eigen vectors into simpler types that can be handled by the addVariable functions

arbitrary Eigen Vectors

void addVector(const Eigen::Ref<const Eigen::VectorXd> &data, const std::string &name, const std::vector<std::string> &extension, const std::vector<std::string> &units)

extension contains the list of strings to be appended to name for each elem of data

void addVector(const Eigen::Ref<const Eigen::VectorXd> &data, const std::string &name, const std::string &unit)

will just add 0,1,2..etc as extension to name

void addVector3d(const Eigen::Ref<const Eigen::Vector3d> &data, const std::string &name, const std::string &units)

the actual recorded names will be name_x, name_y, name_z

void addVector3d(const Eigen::Ref<const Eigen::Vector3d> &data, const std::string &name, const std::string &units, const std::string &extension)

the actual recorded names will be name_x, name_y, name_z plus extension

void addQuaternion(const Eigen::Ref<const Eigen::Vector4d> &data, const std::string &name)

the actual recorded names will be name_q0, name_q1, name_q2, name_q3 with units “-”

void addQuaternion(const Eigen::Quaterniond &data, const std::string &name)

the actual recorded names will be name_q0, name_q1, name_q2, name_q3 with units “-”

void addVector6d(const Eigen::Ref<const Eigen::Matrix<double, 6, 1>> &data, const std::string &name, const std::string &units)

the actual recorded names will have extension x,y,z,a,b,g

void addMatrix(const Eigen::Ref<const Eigen::MatrixXd> &data, const std::string &name, const std::string &unit)

arbitrary Eigen matrices (will just add matrix indes as extension to name, eg 11, 12, 21, 22

class DataReader

#include <data_reader.hpp>

Public Functions

inline DataReader()

inline ~DataReader()

void read(const std::string &fname)

read, reads a clmcplot file and fills the internal data structure.

Parameters:

fname – name of the d-file to read.

int getIndex(const std::string &stream_name)

findIndex, find the column index corresponding to the field name.

Parameters:

name – name of the field.

Returns:

the column index corresponding to the field name.

void getIndexes(const std::vector<std::string> &stream_names, std::vector<int> &indexes)

getIndexes:, find stream column indexes corresponding to the names.

Parameters:

• names – list of the stream name.

• indexes – the output list of indexes.

void fillPose(int row, const std::vector<int> &indexes, Eigen::Vector3d &pose, Eigen::Quaterniond &orientation)

fillPose, fill an existing pose object with the data.

Parameters:

• row – row index homogenous to time/sampling_period.

• indexes – the indexes of the data streams to copy in order: [x, y, z, qw, qx, qy, qz].

• pose – the object to fill.

void fillVector(int row, const std::vector<int> &indexes, Eigen::Ref<Eigen::VectorXd> vec)

fillVector, fill an existing vector from the data

Parameters:

• row – row index homogenous to time/sampling_period.

• indexes – the indexes of the data streams to copy.

• vec – the vector with the correct dimension that is filled

inline int getNbRows()

getNbRows, get the number of rows equivalent to the number of time step saved in the d-file

Returns:

the number of time step saved

inline double getValue(int row, int index)

getValue, get the value of a specific stream at a specific time

Parameters:

• row – row index homogenous to time/sampling_period.

• index – the index of the data streams to copy from.

Returns:

the value

inline double getFrequency()

getFrequence, get the getFrequence of the read data

Returns:

the frequence

class NonRtDataCollector : public mim_estimation::io_tools::DataCollector

#include <non_rt_data_collector.hpp>

Public Functions

inline NonRtDataCollector()

inline virtual ~NonRtDataCollector()

virtual void addVariable(const double *data, const std::string &name, const std::string &units)

virtual functions to add variables to data collection for different types

virtual void addVariable(const float *data, const std::string &name, const std::string &units)

virtual void addVariable(const int *data, const std::string &name, const std::string &units)

virtual void addVariable(const bool *data, const std::string &name, const std::string &units)

virtual void updateDataCollection()

virtual function to update the collection with the recently added variables

virtual void stopDataCollection()

virtual void startDataCollection()

virtual bool isDataCollectionDone()

virtual function to check whether data collection has completed:

float reverseFloat(const float inFloat)

convertion big endian and little endian

void dump(std::string path)

dump the data in SL format the

namespace standard_filters

template<class classType, int _Order>
class ButterworthFilter

#include <butterworth_filter.hpp>

General filter template

template<typename _Scalar, int _Rows, int _Order> ColMajor, _Rows, 1 >, _Order >

#include <butterworth_filter.hpp>

Specialization to an Eigen::Vector. This Filter class implements a butterworth low pass filter able to filter a vector of measurements. Only Eigen::Vectors or Eigen::Matrices with column dimension 1 are valid. Usage example: typedef Filter<Eigen::Matrix<double,6,1> > FilterType; INSTANTIATE FilterType myfilter; INITIALIZE myfilter.initialize(initial_state, filter_order, cuttoff_frequency); UPDATE myfilter.update(measurement); RETRIEVE EST myfilter.getData(estimate);

Public Types

typedef Eigen::Matrix<_Scalar, _Rows, 1, Eigen::ColMajor, _Rows, 1> VecToFilter

Input and Output type definition (VecToFilter)

Public Functions

ButterworthFilter()

~ButterworthFilter()

void initialize(const VecToFilter &current_state, double cutoff_frequency)

Function to initialize several parameters of the filter

Parameters:

• current_state – [in] initial state of the system of type VecToFilter

• filter_order – [in] order of the butterworth filter (integer >= 1)

• cutoff_frequency – [in] cuttoff frequency for the filter as percentage of the nyquist frequency. For example, if sampling frequency is 200Hz; then the Nyquist frequency is half of the sampling frequency, namely 100Hz. cut_off frequency is a double element of [0-1], such that 0 corresponds to 0*Nyquist frequency and 1 corresponds to a cutoff_frequency equal to the Nyquist frequency.

void update(const VecToFilter &new_measurement)

Function to update the filtered state based on the new raw measurement

Parameters:

new_measurement – [in] new raw measurement of type VecToFilter

void getData(VecToFilter &current_state)

Function to ask for the filtered estimate

Parameters:

current_state – [out] filtered estimate of state of type VecToFilter

void getData(Eigen::Ref<ButterworthFilter::VecToFilter> current_state)

template<class S, class M>
class EKF

#include <ekf.hpp>

This class implements an extended Kalman Filter (EKF).

This class implements a templated Extended Kalman Filter that receives a State class “S” and a measurement class “M”. Both templated class must have this following operator: -, +, =, <<. This operator must be eigen compatible. To achieve this in a compact way I strongly suggest inheriting directly from the class Eigen::Vector<N> (see Eigen documentation https://eigen.tuxfamily.org/dox/TopicCustomizing_InheritingMatrix.html).

The main sources of reading comes from

• [1] “S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics, 2000” (Chapter 3)

• [2] “http://wolfweb.unr.edu/~fadali/EE782/NumEvalQphi.pdf”

• [3] “https://en.wikipedia.org/wiki/Extended_Kalman_filter”

The notation used in this documentation is the one from “http://wolfweb.unr.edu/~fadali/EE782/NumEvalQphi.pdf”.

\[\begin{split}\begin{align} x_t = g(u_t, x_{t-1}) + \epsilon_t \\ z_t = h(x_t) + \delta_t \end{align}\end{split}\]

With \( x_t \) the system state, \( u_t \) the system control, \( g \) the process evolution, \( \epsilon_t \approx N(0, R_t) \) the process Gaussian noise, \( h \) the measurement evolution and \( \delta_t \approx N(0, Q_t) \) the measurement Gaussian noise. The first equation depicts the “process” in the code. The second equation represents the “measurement” in the code.

Using a taylor expansion of the previous equation we get:

\[\begin{split}\begin{align} x_t = g(u_t, \mu_{t-1}) + G_t \; * \; (x_{t-1} - \mu_{t-1}) + \epsilon_t \\ z_t = h(\mu_t) + H_t \; * \; (x_{t} - \mu_{t}) + \delta_t \end{align}\end{split}\]

With \( \mu_{t} \) the average of the estimated state of covariance \( \Sigma_t \). The equation of the EKF routine are defined with the “update_filter” function.

The mapping between the variable names and this piece of documentation is done next to each variable.

Public Functions

EKF(bool numerical_jac, bool is_discrete, double dt, unsigned lin_order = 2)

EKF, constructor of the class.

Parameters:

• numerical_jac – do we compute the Jacobians analytically or not?

• is_discrete – is the process discrete?

• dt – the sampling period of the EKF.

inline ~EKF()

Default destructor.

void updateFilter(bool update_on)

updateFilter: compute a pass of the extended Kalman filter

From here on we only have access to \( \mu_{t-1} \) and \( \Sigma_{t-1} \) the previous estimated state along time. First of all we need to check if the system is a discrete one (see constructor). If the system is continuous we need to build a discrete approximation of it. In order to do so we use the Van Loan’s method (see [2] slide 15).

• 1/ Compute the dynamic matrix of the continuous process \( M \)

  \[\begin{align} M = \left[ \left. \frac{-G_t}{0} \right| \frac{E_t R_t E_t^T}{G_t^T} \right] \end{align}\]
  With \( E_t \) being the jacobian of the noise. In [2], \( E_t \) is also considered as the control jacobian in a way.

• 2/ Obtain the matrix M exponential

  \[\begin{align} e^{M \Delta t} = \left[ \left. \frac{ e^{-G_t \Delta t} }{0} \right| \frac{ e^{-G_t \Delta t} F_{t, disc} }{ e^{G_t^T \Delta t} } \right] \end{align}\]

• 3/ Transpose the lower right corner to compute the discretized process noise covariance.

  \[\begin{align} R_{t, disc} = e^{G_t \Delta t} = (e^{G_t^T \Delta t})^T \end{align}\]

• 4/ Compute the discretized process jacobian as follow:

  \[\begin{align} F_{t, disc} = R_{t, disc} \;\;\; e^{M \Delta t}[\text{upper right corner}] \end{align}\]
  In the following we will discard the \( {disc} \) subscript in the expression of the process Jacobian and noise covariance for sake of clarity. We assume that the system is either discrete or already discretized.

When the system has been discretized (or is discrete) we compute the EKF with the following procedure. First we need to predict what is supposed to be the current estimated state \( \tilde{\mu}_{t} \) and its Covariance \( \tilde{\Sigma}_{t} \) according to the model and the current control:

\[\begin{split}\begin{align} \tilde{\mu}_{t} &= g(u_t, \mu_{t-1}) \\ \tilde{\Sigma}_{t} &= G_t \Sigma_{t-1} G_t^T + R_t \end{align}\end{split}\]

The second step consist in introducing the actual measurement when one occur.

\[\begin{split}\begin{align} K_{t} &= \tilde{\Sigma}_{t} H_t^T (H_t \tilde{\Sigma}_t H_t^T + Q_t)^{-1} \\ \mu_{t} &= \tilde{\mu}_{t} + K_t (z_t - h(\tilde{\mu}_t)) \\ \Sigma_{t} &= (I - K_t H_t) \tilde{\Sigma}_{t} \end{align}\end{split}\]

Parameters:

update_on – is the measurement new?

virtual Eigen::Matrix<double, N, 1> getFilterState(void) = 0

getFilterState

Returns:

the current state estimate \( \mu_{t} \)

void printDebug(void)

printDebug, print all the protected method of the class for debugging purposes

Public Static Attributes

static const int N = S::state_dim

State dimension

static const int P = S::noise_dim

Noise dimension

static const int K = M::meas_dim

Measurement dimension

template<class S, class M>
class UKF

#include <ukf.hpp>

Public Functions

UKF(bool numerical_jac, bool is_discrete, bool update_on, double dt)

inline ~UKF()

virtual void initialize(void) = 0

inline virtual void updateControls(void)

void update(void)

virtual Eigen::Matrix<double, N, 1> get_filter_state(void) = 0

void print_debug(void)

Public Static Attributes

static const int N = S::state_dim

static const int P = S::noise_dim

static const int K = M::meas_dim

static const int L = 2 * N + 1

namespace test

class EstimatorViconImuTest

#include <ekf_vicon_imu_simple.hpp>

Public Functions

inline EstimatorViconImuTest(std::string yaml_file, std::string d_file)

This class tests the mim_estimation based on IMU and Vicon measurement.

inline void run()

run, simple method that execute the estimation of the base position from motion capture and IMU data saved from SL

inline void dump(std::string file_name)

inline void get_data_from_file(const int row, Eigen::Vector3d &gyroscope, Eigen::Vector3d &accelerometer, Eigen::Matrix4d &vicon_base_pose, Eigen::Quaterniond &input_vicon_base_quat, bool &vicon_is_new_frame)

get_data_from_file

Parameters:

• row – row index homogenous to time/sampling_period.

• gyroscope – output imu data copied from the data set

• accelerometer – output imu data copied from the data set

• unfiltered_joint_data – joint position copied from the data set

• filtered_joint_data – filtered joint position copied from the data set

• vicon_base_pose – position of the based measured form the Vicon system copied from the data set

• vicon_is_new_frame – True is the Vicon data if actually a new one.

inline void subscribe_to_data_collector()

subscribe_to_data_collector, collect the address of all variables to save

inline void get_data_indexes()

get_data_indexes In order to parse the SL data file one need the indexes associated to the data.

This method computes the different names and indexes.

inline void display_one_statistics(double avg, double min, double max, std::string name)

inline void display_all_statistics()

inline void compute_all_statistics()

inline void compute_one_statistics(Eigen::Ref<Eigen::MatrixXd> mat, double &avg, double &min, double &max)

Public Members

double avg_err_base_pose

double avg_err_base_pose_x

double avg_err_base_pose_y

double avg_err_base_pose_z

double max_err_base_pose

double max_err_base_pose_x

double max_err_base_pose_y

double max_err_base_pose_z

double min_err_base_pose

double min_err_base_pose_x

double min_err_base_pose_y

double min_err_base_pose_z

double avg_err_base_quat_x

double avg_err_base_quat_y

double avg_err_base_quat_z

double max_err_base_quat_x

double max_err_base_quat_y

double max_err_base_quat_z

double min_err_base_quat_x

double min_err_base_quat_y

double min_err_base_quat_z