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