interpolator.h
Go to the documentation of this file.
00001 #ifndef __INTERPOLATOR_H__
00002 #define __INTERPOLATOR_H__
00003 
00004 #include <deque>
00005 #include <string>
00006 #include <coil/Mutex.h>
00007 
00008 using namespace std;
00009 
00010 class interpolator
00011 {
00012   // interpolator class is to interpolate from current value to goal value considering position, velocities, and accelerations.
00013   //   Two status : empty or not
00014   //                Interpolator interpolates based on remaining time (remain_t) and pushes value to queue (q, dq, ddq).
00015   //                Users can get interpolated results from queue (q, dq, ddq).
00016   //                If remain_t <= 0 and queue is empty, interpolator is "empty", otherwise "not empty".
00017   //                This is related with isEmpty() function.
00018   //   Setting goal value : setGoal(), go(), and load()
00019   //   Getting current value : get()
00020   //   Resetting current value : set()
00021   //   Interpolate : interpolate()
00022 public:
00023   typedef enum {LINEAR, HOFFARBIB,QUINTICSPLINE,CUBICSPLINE} interpolation_mode;
00024   interpolator(int dim_, double dt_, interpolation_mode imode_=HOFFARBIB, double default_avg_vel_=0.5); // default_avg_vel = [rad/s]
00025   ~interpolator();
00026   void push(const double *x_, const double *v_, const double *a_, bool immediate=true);
00027   double *front();
00028   // Getter function.
00029   //   1. Interpolate value if remain_t > 0 (time to goal is remaining).
00030   //   2. Get value.
00031   //   3. Pop value queue (q, dq, ddq) if popp = true.
00032   void get(double *x_, bool popp=true);
00033   void get(double *x_, double *v_, bool popp=true);
00034   void get(double *x_, double *v_, double *a_, bool popp=true);
00035   // Reset current value.
00036   void set(const double *x, const double *v=NULL);
00037   // Set goal and complete all interpolation.
00038   //   After calling of go(), value queue (q, dq, ddq) is full and remain_t = 0.
00039   void go(const double *gx, const double *gv, double time, bool immediate=true);
00040   void go(const double *gx, double time, bool immediate=true);
00041   void pop();
00042   void pop_back();
00043   void clear();
00044   void sync();
00045   void load(string fname, double time_to_start=1.0, double scale=1.0,
00046             bool immediate=true, size_t offset1 = 0, size_t offset2 = 0);
00047   void load(const char *fname, double time_to_start=1.0, double scale=1.0,
00048             bool immediate=true, size_t offset1 = 0, size_t offset2 = 0);
00049   bool isEmpty();
00050   double remain_time();
00051   double calc_interpolation_time(const double *g);
00052   bool setInterpolationMode (interpolation_mode i_mode_);
00053   // Set goal
00054   //   If online=true, user can get and interpolate value through get() function.
00055   void setGoal(const double *gx, const double *gv, double time,
00056                bool online=true);
00057   void setGoal(const double *gx, double time, bool online=true);
00058   // Interpolate value and push value to queue (q, dq, ddq).
00059   //   If remain_t <= 0, do nothing.
00060   void interpolate(double& remain_t_);
00061   double deltaT() const { return dt; }
00062   int dimension() const { return dim; }
00063   void setName (const std::string& _name) { name = _name; };
00064 private:
00065   // Current interpolation mode
00066   interpolation_mode imode;
00067   // Queue of positions, velocities, and accelerations ([q_t, q_t+1, ...., q_t+n]).
00068   deque<double *> q, dq, ddq;
00069   // Length of queue.
00070   int length;
00071   // Dimension of interpolated vector (dim of x, v, a, ... etc)
00072   int dim;
00073   // Control time [s]
00074   double dt;
00075   // Current positions, velocities, and accelerations.
00076   double *x, *v, *a;
00077   // Current goal positions, velocities, and accelerations.
00078   double *gx, *gv, *ga;
00079   // target_t : time to goal [s] at setGoal
00080   // remain_t : time to goal [s] from current time. remain_t is [0, target_t].
00081   double target_t, remain_t;
00082   // Coefficients for interpolation polynomials.
00083   double *a0, *a1, *a2, *a3, *a4, *a5;
00084   // Default average velocity for calc_interpolation_time
00085   double default_avg_vel;
00086   // Interpolator name
00087   std::string name;
00088 
00089   void hoffarbib(double &remain_t_,
00090                  double a0, double a1, double a2,
00091                  double a3, double a4, double a5,
00092                  double &xx, double &vv, double &aa);
00093   void linear_interpolation(double &remain_t_,
00094                             double gx,
00095                             double &xx, double &vv, double &aa);
00096   //Mutex to avoid poping twice the same element
00097   coil::Mutex pop_mutex_;
00098 };
00099 
00100 #endif


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:18