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