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