seqplay.h
Go to the documentation of this file.
00001 #ifndef __SEQPLAY_H__
00002 #define __SEQPLAY_H__
00003 
00004 #include <fstream>
00005 #include <vector>
00006 #include <map>
00007 #include <hrpUtil/EigenTypes.h>
00008 #include "interpolator.h"
00009 #include "timeUtil.h"
00010 
00011 using namespace hrp;
00012 
00013 class seqplay
00014 {
00015 public:
00016     seqplay(unsigned int i_dof, double i_dt, unsigned int i_fnum = 0, unsigned int optional_data_dim = 1);
00017     ~seqplay();
00018     //
00019     bool isEmpty() const;
00020     bool isEmpty(const char *gname);
00021     //
00022     void setJointAngles(const double *i_qRef, double i_tm=0.0);
00023     void getJointAngles(double *i_qRef);
00024     void setZmp(const double *i_zmp, double i_tm=0.0);
00025     void setBasePos(const double *i_pos, double i_tm=0.0);
00026     void setBaseRpy(const double *i_rpy, double i_tm=0.0);
00027     void setBaseAcc(const double *i_acc, double i_tm=0.0);
00028     void setWrenches(const double *i_wrenches, double i_tm=0.0);
00029     void playPattern(std::vector<const double*> pos, std::vector<const double*> zmp, std::vector<const double*> rpy, std::vector<double> tm, const double *qInit, unsigned int len);
00030     //
00031     bool addJointGroup(const char *gname, const std::vector<int>& indices);
00032     bool getJointGroup(const char *gname, std::vector<int>& indices);
00033     bool removeJointGroup(const char *gname, double time=2.5);
00034     bool setJointAnglesOfGroup(const char *gname, const double* i_qRef, const size_t i_qsize, double i_tm=0.0);
00035     void clearOfGroup(const char *gname, double i_timeLimit);
00036     bool playPatternOfGroup(const char *gname, std::vector<const double*> pos, std::vector<double> tm, const double *qInit, unsigned int len);
00037 
00038     bool resetJointGroup(const char *gname, const double *full);
00039     //
00040     bool setJointAnglesSequence(std::vector<const double*> pos, std::vector<double> tm);
00041     bool setJointAnglesSequenceOfGroup(const char *gname, std::vector<const double*> pos, std::vector<double> tm, const size_t pos_size);
00042     bool setJointAnglesSequenceFull(std::vector<const double*> pos, std::vector<const double*> vel, std::vector<const double*> torques, std::vector<const double*> bpos, std::vector<const double*> brpy, std::vector<const double*> bacc, std::vector<const double*> zmps, std::vector<const double*> wrenches, std::vector<const double*> optionals, std::vector<double> tm);
00043     bool clearJointAngles();
00044     bool clearJointAnglesOfGroup(const char *gname);
00045     //
00046     void setJointAngle(unsigned int i_rank, double jv, double tm);
00047     void loadPattern(const char *i_basename, double i_tm);
00048     void clear(double i_timeLimit=0);
00049     void get(double *o_q, double *o_zmp, double *o_accel,
00050              double *o_basePos, double *o_baseRpy, double *o_tq, double *o_wrenches, double *o_optional_data);
00051     void go(const double *i_q, const double *i_zmp, const double *i_acc,
00052             const double *i_p, const double *i_rpy, const double *i_tq, const double *i_wrenches, const double *i_optional_data, double i_time, 
00053             bool immediate=true);
00054     void go(const double *i_q, const double *i_zmp, const double *i_acc,
00055             const double *i_p, const double *i_rpy, const double *i_tq, const double *i_wrenches, const double *i_optional_data,
00056             const double *ii_q, const double *ii_zmp, const double *ii_acc,
00057             const double *ii_p, const double *ii_rpy, const double *ii_tq, const double *ii_wrenches, const double *ii_optional_data,
00058             double i_time, bool immediate=true);
00059     void sync();
00060     bool setInterpolationMode(interpolator::interpolation_mode i_mode_);
00061 private:
00062     class groupInterpolator{
00063     public:
00064         groupInterpolator(const std::vector<int>& i_indices, double i_dt)
00065             : indices(i_indices), state(created){
00066             inter = new interpolator(i_indices.size(), i_dt);
00067         }
00068         ~groupInterpolator(){
00069             delete inter;
00070         }
00071         void get(double *full, double *dfull = NULL, bool popp=true){
00072             if (state == created) return;
00073             if (state == removing){
00074                 double x[indices.size()];
00075                 double v[indices.size()];
00076                 for (size_t i=0; i<indices.size(); i++){
00077                     x[i] = full[indices[i]];
00078                     v[i] = dfull ? dfull[indices[i]] : 0;
00079                 }
00080                 inter->setGoal(x, v, time2remove);
00081                 time2remove -= inter->deltaT();
00082                 if (time2remove <= 0) state = removed;
00083             }
00084             double x[indices.size()], v[indices.size()];
00085             inter->get(x, v, popp);
00086             for (size_t i=0; i<indices.size(); i++){
00087                 full[indices[i]] = x[i];
00088                 if (dfull) dfull[indices[i]] = v[i];
00089             }
00090         }
00091         void set(const double *full, const double *dfull=NULL){
00092             double x[indices.size()], v[indices.size()];
00093             for (size_t i=0; i<indices.size(); i++){
00094                 x[i] = full[indices[i]];
00095                 v[i] = dfull ? dfull[indices[i]] : 0;
00096                 //std::cout << v[i] << " ";
00097             }
00098             //std::cout << std::endl;
00099             inter->set(x,v);
00100         }
00101         void extract(double *dst, const double *src){
00102             for (size_t i=0; i<indices.size(); i++){
00103                 dst[i] = src[indices[i]];
00104             }
00105         }
00106         bool isEmpty() { return inter->isEmpty() && state != removing; } 
00107         void go(const double *g, double tm){
00108             inter->go(g, tm);
00109             state = working;
00110         }
00111         void go(const double *g, const double *v, double tm){
00112             inter->go(g, v, tm);
00113             state = working;
00114         }
00115         void setGoal(const double *g, double tm){
00116             inter->setGoal(g, tm);
00117             inter->sync();
00118             state = working;
00119         }
00120         void setGoal(const double *g, const double *v, double tm){
00121             inter->setGoal(g, v, tm);
00122             inter->sync();
00123             state = working;
00124         }
00125         void remove(double time){
00126             state = removing;
00127             time2remove = time;
00128         }
00129         void clear(double i_timeLimit=0) {
00130             tick_t t1 = get_tick();
00131             while (!isEmpty()){
00132                 if (i_timeLimit > 0 
00133                         && tick2sec(get_tick()-t1)>=i_timeLimit) break;
00134                 inter->pop_back();
00135             }
00136         }
00137 
00138         interpolator *inter;
00139         std::vector<int> indices;
00140         typedef enum { created, working, removing, removed } gi_state;
00141         gi_state state;
00142         double time2remove;
00143     };
00144     void pop_back();
00145     enum {Q, ZMP, ACC, P, RPY, TQ, WRENCHES, OPTIONAL_DATA, NINTERPOLATOR};
00146     interpolator *interpolators[NINTERPOLATOR];
00147     std::map<std::string, groupInterpolator *> groupInterpolators; 
00148     int debug_level, m_dof;
00149 };
00150 
00151 #endif


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56