sot-torque-control  1.5.3
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
lin-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 of first order that fits some data.
5 */
6 
7 #include <iostream>
8 
10 
11 LinEstimator::LinEstimator(const unsigned int& N, const unsigned int& dim, const double& dt)
12  : PolyEstimator(1, N, dt), dim_(dim), sum_ti_(0.0), sum_ti2_(0.0) {
13  /* Number of coefficients for a quadratic estimator: 2 */
14  coeff_.resize(2);
15 
16  /* Initialize sums for the recursive computation */
17  sum_xi_.resize(dim);
18  sum_tixi_.resize(dim);
19  for (unsigned int i = 0; i < dim; ++i) {
20  sum_xi_.at(i) = 0.0;
21  sum_tixi_.at(i) = 0.0;
22  }
23 
24  /* Initialize (pre-compute) pseudo inverse matrix that assumes that the sample
25  time is constant (only if dt is not zero) */
26  if (!dt_zero_) {
27  Eigen::MatrixXd Tmat(N_, 2);
28  Eigen::MatrixXd pinvTmat(2, N_);
29  double time = 0.0;
30  for (unsigned int i = 0; i < N_; ++i) {
31  Tmat(i, 1) = time;
32  Tmat(i, 0) = 1.0;
33  time += dt_;
34  }
35  /* Half time used to estimate the position */
36  tmed_ = time * 0.5;
37 
38  /* Pseudo inverse */
39  pinv(Tmat, pinvTmat);
40  pinv0_ = new double[N_];
41  pinv1_ = new double[N_];
42  c0_.resize(dim_);
43  c1_.resize(dim_);
44  c0_.assign(dim_, 0.0);
45  c1_.assign(dim_, 0.0);
46 
47  /* Copy the pseudo inverse to an array */
48  for (unsigned int i = 0; i < N_; ++i) {
49  pinv0_[i] = pinvTmat(0, i);
50  pinv1_[i] = pinvTmat(1, i);
51  }
52  }
53 }
54 
55 double LinEstimator::getEsteeme() { return coeff_(1); }
56 
57 void LinEstimator::estimateRecursive(std::vector<double>& esteem, const std::vector<double>& el, const double& time) {
58  /* Feed Data */
59  elem_list_.at(pt_) = el;
60  time_list_.at(pt_) = time;
61 
62  double t, x;
63 
64  if (first_run_) {
65  // Not enough elements in the window
66  if ((pt_ + 1) < N_) {
67  // t = time - time_list_.at(0);
68  t = time;
69  sum_ti_ += t;
70  sum_ti2_ += t * t;
71  for (unsigned int i = 0; i < esteem.size(); ++i) {
72  // Return a zero vector
73  esteem.at(i) = 0.0;
74  x = el.at(i);
75  // Fill the sumations
76  sum_xi_.at(i) += x;
77  sum_tixi_.at(i) += t * x;
78  }
79  pt_++;
80  return;
81  } else {
82  first_run_ = false;
83  }
84  }
85 
86  // Increase the 'circular' pointer
87  pt_ = (pt_ + 1) < N_ ? (pt_ + 1) : 0;
88  // The pointer now points to the "oldest" element in the vector
89 
90  // Get size of first element: N dof (assuming all elements have same size)
91  size_t dim = elem_list_.at(0).size();
92  // TODO: CHECK THAT SIZE OF ESTEEM = DIM
93 
94  // t = time - time_list_.at(pt_);
95  t = time;
96  sum_ti_ += t;
97  sum_ti2_ += t * t;
98  double den = N_ * sum_ti2_ - sum_ti_ * sum_ti_;
99 
100  // double t_old = time_list_.at(pt_);
101  double t_old = time_list_.at(pt_);
102  for (unsigned int i = 0; i < dim; ++i) {
103  x = el[i];
104  // Fill the sumations
105  sum_xi_[i] += x;
106  sum_tixi_[i] += t * x;
107 
108  // the bias
109  // coeff_(0) = (sum_xi*sum_titi - sum_ti*sum_tixi) / den;
110  // the linear coefficient
111  esteem[i] = (N_ * sum_tixi_[i] - sum_ti_ * sum_xi_[i]) / den;
112 
113  x = elem_list_[pt_][i];
114  sum_xi_[i] -= x;
115  sum_tixi_[i] -= t_old * x;
116  }
117  sum_ti_ -= t_old;
118  sum_ti2_ -= t_old * t_old;
119 
120  return;
121 }
122 
123 void LinEstimator::fit() {
124  double sum_ti = 0.0;
125  double sum_titi = 0.0;
126  double sum_xi = 0.0;
127  double sum_tixi = 0.0;
128 
129  for (unsigned int i = 0; i < N_; ++i) {
130  sum_ti += t_[i];
131  sum_titi += t_[i] * t_[i];
132  sum_xi += x_[i];
133  sum_tixi += t_[i] * x_[i];
134  }
135 
136  double den = N_ * sum_titi - sum_ti * sum_ti;
137 
138  // the bias
139  coeff_(0) = (sum_xi * sum_titi - sum_ti * sum_tixi) / den;
140 
141  // the linear coefficient
142  coeff_(1) = (N_ * sum_tixi - sum_ti * sum_xi) / den;
143 
144  return;
145 }
146 
147 void LinEstimator::estimate(std::vector<double>& esteem, const std::vector<double>& el) {
148  if (dt_zero_) {
149  std::cerr << "Error: dt cannot be zero" << std::endl;
150  // Return a zero vector
151  for (unsigned int i = 0; i < esteem.size(); ++i) esteem[i] = 0.0;
152  return;
153  }
154 
155  /* Feed Data. Note that the time is not completed since it is assumed to be
156  constant */
157  elem_list_[pt_] = el;
158 
159  if (first_run_) {
160  if ((pt_ + 1) < N_) {
161  // Return input vector when not enough elements to compute
162  pt_++;
163  for (unsigned int i = 0; i < esteem.size(); ++i) esteem[i] = el[i];
164  return;
165  } else
166  first_run_ = false;
167  }
168 
169  // Next pointer value
170  pt_ = (pt_ + 1) < N_ ? (pt_ + 1) : 0;
171 
172  unsigned int idx;
173  double x;
174  // Cycle all the elements in the vector
175  for (int i = 0; i < dim_; ++i) {
176  c0_[i] = 0.0;
177  c1_[i] = 0.0;
178  // Retrieve the data in the window
179  for (unsigned int j = 0; j < N_; ++j) {
180  idx = (pt_ + j);
181  if (idx >= N_) idx -= N_;
182  x = elem_list_[idx][i];
183  c0_[i] += x * pinv0_[j];
184  c1_[i] += x * pinv1_[j];
185  }
186 
187  // Polynomial (position)
188  esteem[i] = c1_[i] * tmed_ + c0_[i];
189  }
190 }
191 
192 void LinEstimator::getEstimateDerivative(std::vector<double>& estimateDerivative, const unsigned int order) {
193  switch (order) {
194  case 0:
195  for (int i = 0; i < dim_; ++i) estimateDerivative[i] = c1_[i] * tmed_ + c0_[i];
196  return;
197 
198  case 1:
199  for (int i = 0; i < dim_; ++i) estimateDerivative[i] = c1_[i];
200  return;
201 
202  default:
203  for (int i = 0; i < dim_; ++i) estimateDerivative[i] = 0.0;
204  }
205 }
LinEstimator::getEstimateDerivative
void getEstimateDerivative(std::vector< double > &estimeeDerivative, const unsigned int order)
Definition: lin-estimator.cpp:192
PolyEstimator::first_run_
bool first_run_
Definition: poly-estimator.hh:116
PolyEstimator::dt_
double dt_
Sampling (control) time.
Definition: poly-estimator.hh:109
PolyEstimator::coeff_
Eigen::VectorXd coeff_
Coefficients for the least squares solution.
Definition: poly-estimator.hh:128
lin-estimator.hh
PolyEstimator::dt_zero_
bool dt_zero_
Indicate that dt is zero (dt is invalid)
Definition: poly-estimator.hh:112
pinv
void pinv(const Eigen::MatrixXd &matrix_in, Eigen::MatrixXd &pseudo_inv, const double &pinvtoler=1.0e-6)
Definition: poly-estimator.cpp:9
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
LinEstimator::estimateRecursive
virtual void estimateRecursive(std::vector< double > &estimee, const std::vector< double > &el, const double &time)
Definition: lin-estimator.cpp:57
PolyEstimator
Definition: poly-estimator.hh:29
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
LinEstimator::estimate
virtual void estimate(std::vector< double > &estimee, const std::vector< double > &el)
Definition: lin-estimator.cpp:147
LinEstimator::LinEstimator
LinEstimator(const unsigned int &N, const unsigned int &dim, const double &dt=0.0)
Definition: lin-estimator.cpp:11
PolyEstimator::elem_list_
std::vector< std::vector< double > > elem_list_
All the data (N elements of size dim)
Definition: poly-estimator.hh:119