interpolator.cpp
Go to the documentation of this file.
1 #include <fstream>
2 #include <iostream>
3 #include <cmath>
4 #include <cstring>
5 using namespace std;
6 #include "interpolator.h"
7 #include <coil/Guard.h>
8 
9 interpolator::interpolator(int dim_, double dt_, interpolation_mode imode_, double default_avg_vel_)
10 {
11  imode = imode_;
12  dim = dim_;
13  dt = dt_;
14  length = 0;
15  gx = new double[dim];
16  gv = new double[dim];
17  ga = new double[dim];
18  a0 = new double[dim];
19  a1 = new double[dim];
20  a2 = new double[dim];
21  a3 = new double[dim];
22  a4 = new double[dim];
23  a5 = new double[dim];
24  x = new double[dim];
25  v = new double[dim];
26  a = new double[dim];
27  for (int i=0; i<dim; i++){
28  gx[i] = gv[i] = ga[i] = x[i] = v[i] = a[i] = 0.0;
29  }
30  remain_t = 0;
31  target_t = 0;
32  default_avg_vel = default_avg_vel_;
33 }
34 
36 {
37  clear();
38  delete [] gx;
39  delete [] gv;
40  delete [] ga;
41  delete [] a0;
42  delete [] a1;
43  delete [] a2;
44  delete [] a3;
45  delete [] a4;
46  delete [] a5;
47  delete [] x;
48  delete [] v;
49  delete [] a;
50 }
51 
53 {
54  while (!isEmpty()){
55  pop();
56  }
57 }
58 
59 // 1dof interpolator
60 void interpolator::hoffarbib(double &remain_t_,
61  double a0, double a1, double a2,
62  double a3, double a4, double a5,
63  double &xx, double &vv, double &aa)
64 {
65 #define EPS 1e-6
66  if (remain_t_ > dt+EPS){
67  remain_t_ -= dt;
68  }else{
69  remain_t_ = 0;
70  }
71  double t = target_t - remain_t_;
72  xx=a0+a1*t+a2*t*t+a3*t*t*t+a4*t*t*t*t+a5*t*t*t*t*t;
73  vv=a1+2*a2*t+3*a3*t*t+4*a4*t*t*t+5*a5*t*t*t*t;
74  aa=2*a2+6*a3*t+12*a4*t*t+20*a5*t*t*t;
75 }
76 
77 void interpolator::linear_interpolation(double &remain_t_,
78  double gx,
79  double &xx, double &vv, double &aa)
80 {
81  if (remain_t_ > dt+EPS){
82  aa = 0;
83  vv = (gx-xx)/remain_t_;
84  xx += vv*dt;
85  remain_t_ -= dt;
86  }else{
87  aa = vv = 0;
88  xx = gx;
89  remain_t_ = 0;
90  }
91 }
92 
94 {
95  //cout << "sync:" << length << "," << q.size() << endl;
96  length = q.size();
97 }
98 
99 double interpolator::calc_interpolation_time(const double *newg)
100 {
101  double remain_t_;
102  double max_diff = 0, diff;
103  for (int i=0; i<dim; i++){
104  diff = fabs(newg[i]-gx[i]);
105  if (diff > max_diff) max_diff = diff;
106  }
107  remain_t_ = max_diff/default_avg_vel;
108 #define MIN_INTERPOLATION_TIME (1.0)
109  if (remain_t_ < MIN_INTERPOLATION_TIME) {
110  std::cerr << "[interpolator][" << name << "] MIN_INTERPOLATION_TIME violated!! Limit remain_t (" << remain_t << ") by MIN_INTERPOLATION_TIME (" << MIN_INTERPOLATION_TIME << ")."
111  << "(max_diff = " << max_diff << ", default_avg_vel = " << default_avg_vel << ")" << std::endl;;
112  remain_t_ = MIN_INTERPOLATION_TIME;
113  }
114  return remain_t_;
115 }
116 
118 {
119  if (i_mode_ != LINEAR && i_mode_ != HOFFARBIB &&
120  i_mode_ != QUINTICSPLINE && i_mode_ != CUBICSPLINE) return false;
121  imode = i_mode_;
122  return true;
123 };
124 
125 void interpolator::setGoal(const double *newg, double time,
126  bool online)
127 {
128  setGoal(newg, NULL, time, online);
129 }
130 
131 void interpolator::setGoal(const double *newg, const double *newv, double time,
132  bool online)
133 {
134  memcpy(gx, newg, sizeof(double)*dim);
135  if ( newv != NULL ) memcpy(gv, newv, sizeof(double)*dim);
136  else { for(int i = 0; i < dim; i++) { gv[i] = 0; } }
137  target_t = time;
138 
139  double A,B,C;
140  for (int i=0; i<dim; i++){
141  switch(imode){
142  case HOFFARBIB:
143  A=(gx[i]-(x[i]+v[i]*target_t+(a[i]/2.0)*target_t*target_t))/(target_t*target_t*target_t);
144  B=(gv[i]-(v[i]+a[i]*target_t))/(target_t*target_t);
145  C=(ga[i]-a[i])/target_t;
146 
147  a0[i]=x[i];
148  a1[i]=v[i];
149  a2[i]=a[i]/2.0;
150  a3[i]=10*A-4*B+0.5*C;
151  a4[i]=(-15*A+7*B-C)/target_t;
152  a5[i]=(6*A-3*B+0.5*C)/(target_t*target_t);
153  break;
154  case QUINTICSPLINE:
155  a0[i]=x[i];
156  a1[i]=v[i];
157  a2[i]=0.5*a[i];
158  a3[i]=(-20*x[i] + 20*gx[i] - 3*a[i]*target_t*target_t + ga[i]*target_t*target_t -
159  12*v[i]*target_t - 8*gv[i]*target_t) / (2*target_t*target_t*target_t);
160  a4[i]=(30*x[i] - 30*gx[i] + 3*a[i]*target_t*target_t - 2*ga[i]*target_t*target_t +
161  16*v[i]*target_t + 14*gv[i]*target_t) / (2*target_t*target_t*target_t*target_t);
162  a5[i]=(-12*x[i] + 12*gx[i] - a[i]*target_t*target_t + ga[i]*target_t*target_t -
163  6*v[i]*target_t - 6*gv[i]*target_t) / (2*target_t*target_t*target_t*target_t*target_t);
164  break;
165  case CUBICSPLINE:
166  a0[i]=x[i];
167  a1[i]=v[i];
168  a2[i]=(-3*x[i] + 3*gx[i] - 2*v[i]*target_t - gv[i]*target_t) / (target_t*target_t);
169  a3[i]=( 2*x[i] - 2*gx[i] + v[i]*target_t + gv[i]*target_t) / (target_t*target_t*target_t);
170  a4[i]=a5[i]=0;
171  break;
172  }
173  }
174  if (online) remain_t = time; // interpolation will start
175 }
176 
177 void interpolator::interpolate(double& remain_t_)
178 {
179  if (remain_t_ <= 0) return;
180 
181  double tm;
182  for (int i=0; i<dim; i++){
183  tm = remain_t_;
184  switch(imode){
185  case LINEAR:
186  linear_interpolation(tm,
187  gx[i],
188  x[i], v[i], a[i]);
189  break;
190  case HOFFARBIB:
191  case QUINTICSPLINE:
192  case CUBICSPLINE:
193  hoffarbib(tm,
194  a0[i], a1[i], a2[i], a3[i], a4[i], a5[i],
195  x[i], v[i], a[i]);
196  break;
197  }
198  }
199  push(x, v, a);
200  remain_t_ = tm;
201 }
202 
203 void interpolator::go(const double *newg, double time, bool immediate)
204 {
205  go(newg, NULL, time, immediate);
206 }
207 
208 void interpolator::go(const double *newg, const double *newv, double time, bool immediate)
209 {
210  if (time == 0) time = calc_interpolation_time(newg);
211  setGoal(newg, newv, time, false);
212 
213  do{
214  interpolate(time);
215  }while(time>0);
216  if (immediate) sync();
217 }
218 
219 void interpolator::load(const char *fname, double time_to_start, double scale,
220  bool immediate, size_t offset1, size_t offset2)
221 {
222  ifstream strm(fname);
223  if (!strm.is_open()) {
224  cerr << "[interpolator " << name << "] file not found(" << fname << ")" << endl;
225  return;
226  }
227  double *vs, ptime=-1,time, tmp;
228  vs = new double[dim];
229  strm >> time;
230  while(strm.eof()==0){
231  for (size_t i=0; i<offset1; i++){
232  strm >> tmp;
233  }
234  for (int i=0; i<dim; i++){
235  strm >> vs[i];
236  }
237  for (size_t i=0; i<offset2; i++){
238  strm >> tmp;
239  }
240  if (ptime <0){
241  go(vs, time_to_start, false);
242  }else{
243  go(vs, scale*(time-ptime), false);
244  }
245  ptime = time;
246  strm >> time;
247  }
248  strm.close();
249  delete [] vs;
250  if (immediate) sync();
251 }
252 
253 void interpolator::load(string fname, double time_to_start, double scale,
254  bool immediate, size_t offset1, size_t offset2)
255 {
256  load(fname.c_str(), time_to_start, scale, immediate, offset1, offset2);
257 }
258 
259 void interpolator::push(const double *x_, const double *v_, const double *a_, bool immediate)
260 {
261  double *p = new double[dim];
262  double *dp = new double[dim];
263  double *ddp = new double[dim];
264  memcpy(p, x_, sizeof(double)*dim);
265  memcpy(dp, v_, sizeof(double)*dim);
266  memcpy(ddp, a_, sizeof(double)*dim);
267  q.push_back(p);
268  dq.push_back(dp);
269  ddq.push_back(ddp);
270  if (immediate) sync();
271 }
272 
274 {
275  coil::Guard<coil::Mutex> lock(pop_mutex_);
276  if (length > 0){
277  length--;
278  double *&vs = q.front();
279  delete [] vs;
280  q.pop_front();
281  double *&dvs = dq.front();
282  delete [] dvs;
283  dq.pop_front();
284  double *&ddvs = ddq.front();
285  delete [] ddvs;
286  ddq.pop_front();
287  }
288 }
289 
291 {
292  coil::Guard<coil::Mutex> lock(pop_mutex_);
293  if (length > 0){
294  length--;
295  double *&vs = q.back();
296  delete [] vs;
297  q.pop_back();
298  if (length > 0){
299  memcpy(x, q.back(), sizeof(double)*dim);
300  }else{
301  memcpy(x, gx, sizeof(double)*dim);
302  }
303  double *&dvs = dq.back();
304  delete [] dvs;
305  dq.pop_back();
306  if (length > 0){
307  memcpy(v, dq.back(), sizeof(double)*dim);
308  }else{
309  memcpy(v, gv, sizeof(double)*dim);
310  }
311  double *&ddvs = ddq.back();
312  delete [] ddvs;
313  ddq.pop_back();
314  if (length > 0){
315  memcpy(a, ddq.back(), sizeof(double)*dim);
316  }else{
317  memcpy(a, ga, sizeof(double)*dim);
318  }
319  } else if (remain_t > 0) {
320  remain_t = 0;
321  }
322 }
323 
324 void interpolator::set(const double *x_, const double *v_)
325 {
326  for (int i=0; i<dim; i++){
327  gx[i] = x[i] = x_[i];
328  if (v_){
329  gv[i] = v[i] = v_[i];
330  }else{
331  gv[i] = v[i] = 0;
332  }
333  ga[i] = a[i] = 0;
334  }
335 }
336 
338 {
339  if (length!=0){
340  return q.front();
341  }else{
342  return gx;
343  }
344 }
345 
346 void interpolator::get(double *x_, bool popp)
347 {
348  get(x_, NULL, NULL, popp);
349 }
350 
351 void interpolator::get(double *x_, double *v_, bool popp)
352 {
353  get(x_, v_, NULL, popp);
354 }
355 
356 void interpolator::get(double *x_, double *v_, double *a_, bool popp)
357 {
358  interpolate(remain_t);
359 
360  if (length!=0){
361  double *&vs = q.front();
362  if (vs == NULL) {
363  cerr << "[interpolator " << name << "] interpolator::get vs = NULL, q.size() = " << q.size()
364  << ", length = " << length << endl;
365  }
366  double *&dvs = dq.front();
367  if (dvs == NULL) {
368  cerr << "[interpolator " << name << "] interpolator::get dvs = NULL, dq.size() = " << dq.size()
369  << ", length = " << length << endl;
370  }
371  double *&ddvs = ddq.front();
372  if (ddvs == NULL) {
373  cerr << "[interpolator " << name << "] interpolator::get ddvs = NULL, ddq.size() = " << ddq.size()
374  << ", length = " << length << endl;
375  }
376  memcpy(x_, vs, sizeof(double)*dim);
377  if ( v_ != NULL ) memcpy(v_, dvs, sizeof(double)*dim);
378  if ( a_ != NULL ) memcpy(a_, ddvs, sizeof(double)*dim);
379  if (popp) pop();
380  }else{
381  memcpy(x_, gx, sizeof(double)*dim);
382  if ( v_ != NULL) memcpy(v_, gv, sizeof(double)*dim);
383  if ( a_ != NULL) memcpy(a_, ga, sizeof(double)*dim);
384  }
385 }
386 
388 {
389  return length==0 && remain_t <= 0;
390 }
391 
393 {
394  return dt*length;
395 }
void push(const double *x_, const double *v_, const double *a_, bool immediate=true)
bool setInterpolationMode(interpolation_mode i_mode_)
void interpolate(double &remain_t_)
void load(string fname, double time_to_start=1.0, double scale=1.0, bool immediate=true, size_t offset1=0, size_t offset2=0)
void clear(CorbaSequence &seq)
#define EPS
void linear_interpolation(double &remain_t_, double gx, double &xx, double &vv, double &aa)
png_infop png_charpp name
png_bytep png_bytep png_size_t length
png_uint_32 i
double * front()
void hoffarbib(double &remain_t_, double a0, double a1, double a2, double a3, double a4, double a5, double &xx, double &vv, double &aa)
png_timep ptime
a0
void go(const double *gx, const double *gv, double time, bool immediate=true)
t
double calc_interpolation_time(const double *g)
#define MIN_INTERPOLATION_TIME
interpolator(int dim_, double dt_, interpolation_mode imode_=HOFFARBIB, double default_avg_vel_=0.5)
Definition: interpolator.cpp:9
void get(double *x_, bool popp=true)
double remain_time()
void setGoal(const double *gx, const double *gv, double time, bool online=true)
void set(const double *x, const double *v=NULL)


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:50