sot-torque-control  1.5.3
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
poly-estimator.cpp
Go to the documentation of this file.
1 /*
2  Oscar Efrain RAMOS PONCE, LAAS-CNRS
3  Date: 28/10/2014
4  Object to estimate a polynomial that fits some data.
5 */
6 
8 
9 void pinv(const Eigen::MatrixXd& matrix_in, Eigen::MatrixXd& pseudo_inv, const double& pinvtoler) {
10  Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrix_in, Eigen::ComputeThinU | Eigen::ComputeThinV);
11  Eigen::VectorXd singular_values;
12  Eigen::VectorXd singular_values_inv;
13  singular_values = svd.singularValues();
14  singular_values_inv.setZero(singular_values.size());
15 
16  for (int w = 0; w < singular_values.size(); ++w)
17  if (singular_values(w) > pinvtoler) singular_values_inv(w) = 1 / singular_values(w);
18  pseudo_inv = svd.matrixV() * singular_values_inv.asDiagonal() * svd.matrixU().transpose();
19  return;
20 }
21 
22 PolyEstimator::PolyEstimator(const unsigned int& order, const unsigned int& N, const double& dt)
23  : order_(order), N_(N), dt_(dt), dt_zero_(true), first_run_(true), pt_(0) {
24  t_.resize(N_);
25  x_.resize(N_);
26  elem_list_.reserve(N_);
27  elem_list_.resize(N_);
28  time_list_.reserve(N_);
29  time_list_.resize(N_);
30  // Determine if the dt is valid
31  if (dt_ > 1e-10) dt_zero_ = false;
32  // Used only in the generic fit function
33  R_.resize(N_, order_ + 1);
34 }
35 
36 void PolyEstimator::estimate(std::vector<double>& esteem, const std::vector<double>& el, const double& time) {
37  /* Feed Data */
38  elem_list_.at(pt_) = el;
39  time_list_.at(pt_) = time;
40 
41  if (first_run_) {
42  if ((pt_ + 1) < N_) {
43  // Return input vector when not enough elements to compute
44  pt_++;
45  for (unsigned int i = 0; i < esteem.size(); ++i) esteem.at(i) = el[i];
46  return;
47  } else {
48  first_run_ = false;
49  }
50  }
51 
52  // Next pointer value
53  pt_ = (pt_ + 1) < N_ ? (pt_ + 1) : 0;
54 
55  // Get the time substracting the lowest one, so that the minimum time is
56  // always zero
57  unsigned int idx;
58  for (unsigned int j = 0; j < N_; ++j) {
59  idx = pt_ + j;
60  if (idx >= N_) idx = idx - N_;
61  t_[j] = time_list_[idx] - time_list_[pt_];
62  }
63 
64  // Get size of first element: N dof (assuming all elements have same size)
65  size_t dim = elem_list_.at(0).size();
66  // TODO: CHECK THAT SIZE OF ESTEEM = DIM
67 
68  // Cycle upon all elements
69  for (unsigned int i = 0; i < dim; ++i) {
70  // Retrieve the data vector
71  for (unsigned int j = 0; j < N_; ++j) {
72  idx = pt_ + j;
73  if (idx >= N_) idx = idx - N_;
74  x_[j] = elem_list_[idx][i];
75  }
76  // Fit the vector
77  fit();
78  esteem[i] = getEsteeme();
79  }
80 
81  return;
82 }
83 
85  for (unsigned int i = 0; i < N_; ++i) {
86  double xtemp = t_[i];
87  R_(i, 0) = 1.0;
88 
89  for (unsigned int j = 1; j <= order_; ++j) {
90  R_(i, j) = xtemp;
91  xtemp *= xtemp;
92  }
93  }
94  Eigen::Map<Eigen::VectorXd> ytemp(&x_[0], N_, 1);
95  coeff_ = R_.householderQr().solve(ytemp);
96 
97  return;
98 }
99 
100 void PolyEstimator::setWindowLength(const unsigned int& N) { N_ = N; }
101 
102 unsigned int PolyEstimator::getWindowLength() { return N_; }
PolyEstimator::fit
virtual void fit()
Definition: poly-estimator.cpp:84
PolyEstimator::first_run_
bool first_run_
Definition: poly-estimator.hh:116
PolyEstimator::dt_
double dt_
Sampling (control) time.
Definition: poly-estimator.hh:109
PolyEstimator::R_
Eigen::MatrixXd R_
Definition: poly-estimator.hh:140
poly-estimator.hh
PolyEstimator::estimate
void estimate(std::vector< double > &estimee, const std::vector< double > &data_element, const double &time)
Definition: poly-estimator.cpp:36
PolyEstimator::coeff_
Eigen::VectorXd coeff_
Coefficients for the least squares solution.
Definition: poly-estimator.hh:128
PolyEstimator::setWindowLength
void setWindowLength(const unsigned int &N)
Definition: poly-estimator.cpp:100
PolyEstimator::dt_zero_
bool dt_zero_
Indicate that dt is zero (dt is invalid)
Definition: poly-estimator.hh:112
PolyEstimator::t_
std::vector< double > t_
Time vector setting the lowest time to zero (for numerical stability).
Definition: poly-estimator.hh:131
PolyEstimator::N_
unsigned int N_
Window length.
Definition: poly-estimator.hh:106
PolyEstimator::getWindowLength
unsigned int getWindowLength()
Definition: poly-estimator.cpp:102
PolyEstimator::order_
unsigned int order_
Order of the polynomial estimator.
Definition: poly-estimator.hh:103
PolyEstimator::pt_
unsigned int pt_
Circular index to each data and time element.
Definition: poly-estimator.hh:125
PolyEstimator::x_
std::vector< double > x_
Definition: poly-estimator.hh:135
PolyEstimator::time_list_
std::vector< double > time_list_
Time vector corresponding to each element in elem_list_.
Definition: poly-estimator.hh:122
PolyEstimator::PolyEstimator
PolyEstimator(const unsigned int &order, const unsigned int &N, const double &dt)
Definition: poly-estimator.cpp:22
pinv
void pinv(const Eigen::MatrixXd &matrix_in, Eigen::MatrixXd &pseudo_inv, const double &pinvtoler)
Definition: poly-estimator.cpp:9
PolyEstimator::elem_list_
std::vector< std::vector< double > > elem_list_
All the data (N elements of size dim)
Definition: poly-estimator.hh:119
PolyEstimator::getEsteeme
virtual double getEsteeme()=0