00001 #include <fstream>
00002 #include <iostream>
00003 #include <cmath>
00004 #include <cstring>
00005 using namespace std;
00006 #include "interpolator.h"
00007 #include <coil/Guard.h>
00008
00009 interpolator::interpolator(int dim_, double dt_, interpolation_mode imode_, double default_avg_vel_)
00010 {
00011 imode = imode_;
00012 dim = dim_;
00013 dt = dt_;
00014 length = 0;
00015 gx = new double[dim];
00016 gv = new double[dim];
00017 ga = new double[dim];
00018 a0 = new double[dim];
00019 a1 = new double[dim];
00020 a2 = new double[dim];
00021 a3 = new double[dim];
00022 a4 = new double[dim];
00023 a5 = new double[dim];
00024 x = new double[dim];
00025 v = new double[dim];
00026 a = new double[dim];
00027 for (int i=0; i<dim; i++){
00028 gx[i] = gv[i] = ga[i] = x[i] = v[i] = a[i] = 0.0;
00029 }
00030 remain_t = 0;
00031 target_t = 0;
00032 default_avg_vel = default_avg_vel_;
00033 }
00034
00035 interpolator::~interpolator()
00036 {
00037 clear();
00038 delete [] gx;
00039 delete [] gv;
00040 delete [] ga;
00041 delete [] a0;
00042 delete [] a1;
00043 delete [] a2;
00044 delete [] a3;
00045 delete [] a4;
00046 delete [] a5;
00047 delete [] x;
00048 delete [] v;
00049 delete [] a;
00050 }
00051
00052 void interpolator::clear()
00053 {
00054 while (!isEmpty()){
00055 pop();
00056 }
00057 }
00058
00059
00060 void interpolator::hoffarbib(double &remain_t_,
00061 double a0, double a1, double a2,
00062 double a3, double a4, double a5,
00063 double &xx, double &vv, double &aa)
00064 {
00065 #define EPS 1e-6
00066 if (remain_t_ > dt+EPS){
00067 remain_t_ -= dt;
00068 }else{
00069 remain_t_ = 0;
00070 }
00071 double t = target_t - remain_t_;
00072 xx=a0+a1*t+a2*t*t+a3*t*t*t+a4*t*t*t*t+a5*t*t*t*t*t;
00073 vv=a1+2*a2*t+3*a3*t*t+4*a4*t*t*t+5*a5*t*t*t*t;
00074 aa=2*a2+6*a3*t+12*a4*t*t+20*a5*t*t*t;
00075 }
00076
00077 void interpolator::linear_interpolation(double &remain_t_,
00078 double gx,
00079 double &xx, double &vv, double &aa)
00080 {
00081 if (remain_t_ > dt+EPS){
00082 aa = 0;
00083 vv = (gx-xx)/remain_t_;
00084 xx += vv*dt;
00085 remain_t_ -= dt;
00086 }else{
00087 aa = vv = 0;
00088 xx = gx;
00089 remain_t_ = 0;
00090 }
00091 }
00092
00093 void interpolator::sync()
00094 {
00095
00096 length = q.size();
00097 }
00098
00099 double interpolator::calc_interpolation_time(const double *newg)
00100 {
00101 double remain_t_;
00102 double max_diff = 0, diff;
00103 for (int i=0; i<dim; i++){
00104 diff = fabs(newg[i]-gx[i]);
00105 if (diff > max_diff) max_diff = diff;
00106 }
00107 remain_t_ = max_diff/default_avg_vel;
00108 #define MIN_INTERPOLATION_TIME (1.0)
00109 if (remain_t_ < MIN_INTERPOLATION_TIME) {
00110 std::cerr << "[interpolator][" << name << "] MIN_INTERPOLATION_TIME violated!! Limit remain_t (" << remain_t << ") by MIN_INTERPOLATION_TIME (" << MIN_INTERPOLATION_TIME << ")."
00111 << "(max_diff = " << max_diff << ", default_avg_vel = " << default_avg_vel << ")" << std::endl;;
00112 remain_t_ = MIN_INTERPOLATION_TIME;
00113 }
00114 return remain_t_;
00115 }
00116
00117 bool interpolator::setInterpolationMode (interpolation_mode i_mode_)
00118 {
00119 if (i_mode_ != LINEAR && i_mode_ != HOFFARBIB &&
00120 i_mode_ != QUINTICSPLINE && i_mode_ != CUBICSPLINE) return false;
00121 imode = i_mode_;
00122 return true;
00123 };
00124
00125 void interpolator::setGoal(const double *newg, double time,
00126 bool online)
00127 {
00128 setGoal(newg, NULL, time, online);
00129 }
00130
00131 void interpolator::setGoal(const double *newg, const double *newv, double time,
00132 bool online)
00133 {
00134 memcpy(gx, newg, sizeof(double)*dim);
00135 if ( newv != NULL ) memcpy(gv, newv, sizeof(double)*dim);
00136 else { for(int i = 0; i < dim; i++) { gv[i] = 0; } }
00137 target_t = time;
00138
00139 double A,B,C;
00140 for (int i=0; i<dim; i++){
00141 switch(imode){
00142 case HOFFARBIB:
00143 A=(gx[i]-(x[i]+v[i]*target_t+(a[i]/2.0)*target_t*target_t))/(target_t*target_t*target_t);
00144 B=(gv[i]-(v[i]+a[i]*target_t))/(target_t*target_t);
00145 C=(ga[i]-a[i])/target_t;
00146
00147 a0[i]=x[i];
00148 a1[i]=v[i];
00149 a2[i]=a[i]/2.0;
00150 a3[i]=10*A-4*B+0.5*C;
00151 a4[i]=(-15*A+7*B-C)/target_t;
00152 a5[i]=(6*A-3*B+0.5*C)/(target_t*target_t);
00153 break;
00154 case QUINTICSPLINE:
00155 a0[i]=x[i];
00156 a1[i]=v[i];
00157 a2[i]=0.5*a[i];
00158 a3[i]=(-20*x[i] + 20*gx[i] - 3*a[i]*target_t*target_t + ga[i]*target_t*target_t -
00159 12*v[i]*target_t - 8*gv[i]*target_t) / (2*target_t*target_t*target_t);
00160 a4[i]=(30*x[i] - 30*gx[i] + 3*a[i]*target_t*target_t - 2*ga[i]*target_t*target_t +
00161 16*v[i]*target_t + 14*gv[i]*target_t) / (2*target_t*target_t*target_t*target_t);
00162 a5[i]=(-12*x[i] + 12*gx[i] - a[i]*target_t*target_t + ga[i]*target_t*target_t -
00163 6*v[i]*target_t - 6*gv[i]*target_t) / (2*target_t*target_t*target_t*target_t*target_t);
00164 break;
00165 case CUBICSPLINE:
00166 a0[i]=x[i];
00167 a1[i]=v[i];
00168 a2[i]=(-3*x[i] + 3*gx[i] - 2*v[i]*target_t - gv[i]*target_t) / (target_t*target_t);
00169 a3[i]=( 2*x[i] - 2*gx[i] + v[i]*target_t + gv[i]*target_t) / (target_t*target_t*target_t);
00170 a4[i]=a5[i]=0;
00171 break;
00172 }
00173 }
00174 if (online) remain_t = time;
00175 }
00176
00177 void interpolator::interpolate(double& remain_t_)
00178 {
00179 if (remain_t_ <= 0) return;
00180
00181 double tm;
00182 for (int i=0; i<dim; i++){
00183 tm = remain_t_;
00184 switch(imode){
00185 case LINEAR:
00186 linear_interpolation(tm,
00187 gx[i],
00188 x[i], v[i], a[i]);
00189 break;
00190 case HOFFARBIB:
00191 case QUINTICSPLINE:
00192 case CUBICSPLINE:
00193 hoffarbib(tm,
00194 a0[i], a1[i], a2[i], a3[i], a4[i], a5[i],
00195 x[i], v[i], a[i]);
00196 break;
00197 }
00198 }
00199 push(x, v, a);
00200 remain_t_ = tm;
00201 }
00202
00203 void interpolator::go(const double *newg, double time, bool immediate)
00204 {
00205 go(newg, NULL, time, immediate);
00206 }
00207
00208 void interpolator::go(const double *newg, const double *newv, double time, bool immediate)
00209 {
00210 if (time == 0) time = calc_interpolation_time(newg);
00211 setGoal(newg, newv, time, false);
00212
00213 do{
00214 interpolate(time);
00215 }while(time>0);
00216 if (immediate) sync();
00217 }
00218
00219 void interpolator::load(const char *fname, double time_to_start, double scale,
00220 bool immediate, size_t offset1, size_t offset2)
00221 {
00222 ifstream strm(fname);
00223 if (!strm.is_open()) {
00224 cerr << "[interpolator " << name << "] file not found(" << fname << ")" << endl;
00225 return;
00226 }
00227 double *vs, ptime=-1,time, tmp;
00228 vs = new double[dim];
00229 strm >> time;
00230 while(strm.eof()==0){
00231 for (size_t i=0; i<offset1; i++){
00232 strm >> tmp;
00233 }
00234 for (int i=0; i<dim; i++){
00235 strm >> vs[i];
00236 }
00237 for (size_t i=0; i<offset2; i++){
00238 strm >> tmp;
00239 }
00240 if (ptime <0){
00241 go(vs, time_to_start, false);
00242 }else{
00243 go(vs, scale*(time-ptime), false);
00244 }
00245 ptime = time;
00246 strm >> time;
00247 }
00248 strm.close();
00249 delete [] vs;
00250 if (immediate) sync();
00251 }
00252
00253 void interpolator::load(string fname, double time_to_start, double scale,
00254 bool immediate, size_t offset1, size_t offset2)
00255 {
00256 load(fname.c_str(), time_to_start, scale, immediate, offset1, offset2);
00257 }
00258
00259 void interpolator::push(const double *x_, const double *v_, const double *a_, bool immediate)
00260 {
00261 double *p = new double[dim];
00262 double *dp = new double[dim];
00263 double *ddp = new double[dim];
00264 memcpy(p, x_, sizeof(double)*dim);
00265 memcpy(dp, v_, sizeof(double)*dim);
00266 memcpy(ddp, a_, sizeof(double)*dim);
00267 q.push_back(p);
00268 dq.push_back(dp);
00269 ddq.push_back(ddp);
00270 if (immediate) sync();
00271 }
00272
00273 void interpolator::pop()
00274 {
00275 coil::Guard<coil::Mutex> lock(pop_mutex_);
00276 if (length > 0){
00277 length--;
00278 double *&vs = q.front();
00279 delete [] vs;
00280 q.pop_front();
00281 double *&dvs = dq.front();
00282 delete [] dvs;
00283 dq.pop_front();
00284 double *&ddvs = ddq.front();
00285 delete [] ddvs;
00286 ddq.pop_front();
00287 }
00288 }
00289
00290 void interpolator::pop_back()
00291 {
00292 coil::Guard<coil::Mutex> lock(pop_mutex_);
00293 if (length > 0){
00294 length--;
00295 double *&vs = q.back();
00296 delete [] vs;
00297 q.pop_back();
00298 if (length > 0){
00299 memcpy(x, q.back(), sizeof(double)*dim);
00300 }else{
00301 memcpy(x, gx, sizeof(double)*dim);
00302 }
00303 double *&dvs = dq.back();
00304 delete [] dvs;
00305 dq.pop_back();
00306 if (length > 0){
00307 memcpy(v, dq.back(), sizeof(double)*dim);
00308 }else{
00309 memcpy(v, gv, sizeof(double)*dim);
00310 }
00311 double *&ddvs = ddq.back();
00312 delete [] ddvs;
00313 ddq.pop_back();
00314 if (length > 0){
00315 memcpy(a, ddq.back(), sizeof(double)*dim);
00316 }else{
00317 memcpy(a, ga, sizeof(double)*dim);
00318 }
00319 } else if (remain_t > 0) {
00320 remain_t = 0;
00321 }
00322 }
00323
00324 void interpolator::set(const double *x_, const double *v_)
00325 {
00326 for (int i=0; i<dim; i++){
00327 gx[i] = x[i] = x_[i];
00328 if (v_){
00329 gv[i] = v[i] = v_[i];
00330 }else{
00331 gv[i] = v[i] = 0;
00332 }
00333 ga[i] = a[i] = 0;
00334 }
00335 }
00336
00337 double *interpolator::front()
00338 {
00339 if (length!=0){
00340 return q.front();
00341 }else{
00342 return gx;
00343 }
00344 }
00345
00346 void interpolator::get(double *x_, bool popp)
00347 {
00348 get(x_, NULL, NULL, popp);
00349 }
00350
00351 void interpolator::get(double *x_, double *v_, bool popp)
00352 {
00353 get(x_, v_, NULL, popp);
00354 }
00355
00356 void interpolator::get(double *x_, double *v_, double *a_, bool popp)
00357 {
00358 interpolate(remain_t);
00359
00360 if (length!=0){
00361 double *&vs = q.front();
00362 if (vs == NULL) {
00363 cerr << "[interpolator " << name << "] interpolator::get vs = NULL, q.size() = " << q.size()
00364 << ", length = " << length << endl;
00365 }
00366 double *&dvs = dq.front();
00367 if (dvs == NULL) {
00368 cerr << "[interpolator " << name << "] interpolator::get dvs = NULL, dq.size() = " << dq.size()
00369 << ", length = " << length << endl;
00370 }
00371 double *&ddvs = ddq.front();
00372 if (ddvs == NULL) {
00373 cerr << "[interpolator " << name << "] interpolator::get ddvs = NULL, ddq.size() = " << ddq.size()
00374 << ", length = " << length << endl;
00375 }
00376 memcpy(x_, vs, sizeof(double)*dim);
00377 if ( v_ != NULL ) memcpy(v_, dvs, sizeof(double)*dim);
00378 if ( a_ != NULL ) memcpy(a_, ddvs, sizeof(double)*dim);
00379 if (popp) pop();
00380 }else{
00381 memcpy(x_, gx, sizeof(double)*dim);
00382 if ( v_ != NULL) memcpy(v_, gv, sizeof(double)*dim);
00383 if ( a_ != NULL) memcpy(a_, ga, sizeof(double)*dim);
00384 }
00385 }
00386
00387 bool interpolator::isEmpty()
00388 {
00389 return length==0 && remain_t <= 0;
00390 }
00391
00392 double interpolator::remain_time()
00393 {
00394 return dt*length;
00395 }