seqplay.h
Go to the documentation of this file.
1 #ifndef __SEQPLAY_H__
2 #define __SEQPLAY_H__
3 
4 #include <fstream>
5 #include <vector>
6 #include <map>
7 #include <hrpUtil/EigenTypes.h>
8 #include "interpolator.h"
9 #include "timeUtil.h"
10 
11 using namespace hrp;
12 
13 class seqplay
14 {
15 public:
16  seqplay(unsigned int i_dof, double i_dt, unsigned int i_fnum = 0, unsigned int optional_data_dim = 1);
17  ~seqplay();
18  //
19  bool isEmpty() const;
20  bool isEmpty(const char *gname);
21  //
22  void setJointAngles(const double *i_qRef, double i_tm=0.0);
23  void getJointAngles(double *i_qRef);
24  void setZmp(const double *i_zmp, double i_tm=0.0);
25  void setBasePos(const double *i_pos, double i_tm=0.0);
26  void setBaseRpy(const double *i_rpy, double i_tm=0.0);
27  void setBaseAcc(const double *i_acc, double i_tm=0.0);
28  void setWrenches(const double *i_wrenches, double i_tm=0.0);
29  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);
30  //
31  bool addJointGroup(const char *gname, const std::vector<int>& indices);
32  bool getJointGroup(const char *gname, std::vector<int>& indices);
33  bool removeJointGroup(const char *gname, double time=2.5);
34  bool setJointAnglesOfGroup(const char *gname, const double* i_qRef, const size_t i_qsize, double i_tm=0.0);
35  void clearOfGroup(const char *gname, double i_timeLimit);
36  bool playPatternOfGroup(const char *gname, std::vector<const double*> pos, std::vector<double> tm, const double *qInit, unsigned int len);
37 
38  bool resetJointGroup(const char *gname, const double *full);
39  //
40  bool setJointAnglesSequence(std::vector<const double*> pos, std::vector<double> tm);
41  bool setJointAnglesSequenceOfGroup(const char *gname, std::vector<const double*> pos, std::vector<double> tm, const size_t pos_size);
42  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);
43  bool clearJointAngles();
44  bool clearJointAnglesOfGroup(const char *gname);
45  //
46  void setJointAngle(unsigned int i_rank, double jv, double tm);
47  void loadPattern(const char *i_basename, double i_tm);
48  void clear(double i_timeLimit=0);
49  void get(double *o_q, double *o_zmp, double *o_accel,
50  double *o_basePos, double *o_baseRpy, double *o_tq, double *o_wrenches, double *o_optional_data);
51  void go(const double *i_q, const double *i_zmp, const double *i_acc,
52  const double *i_p, const double *i_rpy, const double *i_tq, const double *i_wrenches, const double *i_optional_data, double i_time,
53  bool immediate=true);
54  void go(const double *i_q, const double *i_zmp, const double *i_acc,
55  const double *i_p, const double *i_rpy, const double *i_tq, const double *i_wrenches, const double *i_optional_data,
56  const double *ii_q, const double *ii_zmp, const double *ii_acc,
57  const double *ii_p, const double *ii_rpy, const double *ii_tq, const double *ii_wrenches, const double *ii_optional_data,
58  double i_time, bool immediate=true);
59  void sync();
60  bool setInterpolationMode(interpolator::interpolation_mode i_mode_);
61 private:
63  public:
64  groupInterpolator(const std::vector<int>& i_indices, double i_dt)
65  : indices(i_indices), state(created){
66  inter = new interpolator(i_indices.size(), i_dt);
67  }
69  delete inter;
70  }
71  void get(double *full, double *dfull = NULL, bool popp=true){
72  if (state == created) return;
73  if (state == removing){
74  double x[indices.size()];
75  double v[indices.size()];
76  for (size_t i=0; i<indices.size(); i++){
77  x[i] = full[indices[i]];
78  v[i] = dfull ? dfull[indices[i]] : 0;
79  }
80  inter->setGoal(x, v, time2remove);
81  time2remove -= inter->deltaT();
82  if (time2remove <= 0) state = removed;
83  }
84  double x[indices.size()], v[indices.size()];
85  inter->get(x, v, popp);
86  for (size_t i=0; i<indices.size(); i++){
87  full[indices[i]] = x[i];
88  if (dfull) dfull[indices[i]] = v[i];
89  }
90  }
91  void set(const double *full, const double *dfull=NULL){
92  double x[indices.size()], v[indices.size()];
93  for (size_t i=0; i<indices.size(); i++){
94  x[i] = full[indices[i]];
95  v[i] = dfull ? dfull[indices[i]] : 0;
96  //std::cout << v[i] << " ";
97  }
98  //std::cout << std::endl;
99  inter->set(x,v);
100  }
101  void extract(double *dst, const double *src){
102  for (size_t i=0; i<indices.size(); i++){
103  dst[i] = src[indices[i]];
104  }
105  }
106  bool isEmpty() { return inter->isEmpty() && state != removing; }
107  void go(const double *g, double tm){
108  inter->go(g, tm);
109  state = working;
110  }
111  void go(const double *g, const double *v, double tm){
112  inter->go(g, v, tm);
113  state = working;
114  }
115  void setGoal(const double *g, double tm){
116  inter->setGoal(g, tm);
117  inter->sync();
118  state = working;
119  }
120  void setGoal(const double *g, const double *v, double tm){
121  inter->setGoal(g, v, tm);
122  inter->sync();
123  state = working;
124  }
125  void remove(double time){
126  state = removing;
127  time2remove = time;
128  }
129  void clear(double i_timeLimit=0) {
130  tick_t t1 = get_tick();
131  while (!isEmpty()){
132  if (i_timeLimit > 0
133  && tick2sec(get_tick()-t1)>=i_timeLimit) break;
134  inter->pop_back();
135  }
136  }
137 
139  std::vector<int> indices;
140  typedef enum { created, working, removing, removed } gi_state;
141  gi_state state;
142  double time2remove;
143  };
144  void pop_back();
145  enum {Q, ZMP, ACC, P, RPY, TQ, WRENCHES, OPTIONAL_DATA, NINTERPOLATOR};
146  interpolator *interpolators[NINTERPOLATOR];
147  std::map<std::string, groupInterpolator *> groupInterpolators;
148  int debug_level, m_dof;
149 };
150 
151 #endif
std::map< std::string, groupInterpolator * > groupInterpolators
Definition: seqplay.h:147
unsigned long long tick_t
void clear(CorbaSequence &seq)
state
void setGoal(const double *g, double tm)
Definition: seqplay.h:115
int m_dof
Definition: seqplay.h:148
def loadPattern(basename, tm=1.0)
Definition: HRP4C.py:42
groupInterpolator(const std::vector< int > &i_indices, double i_dt)
Definition: seqplay.h:64
png_uint_32 i
interpolator * inter
Definition: seqplay.h:138
void go(const double *g, const double *v, double tm)
Definition: seqplay.h:111
void extract(double *dst, const double *src)
Definition: seqplay.h:101
void clear(double i_timeLimit=0)
Definition: seqplay.h:129
void setGoal(const double *g, const double *v, double tm)
Definition: seqplay.h:120
#define tick2sec(t)
convert time stamp counter into sec
Definition: timeUtil.h:37
std::vector< int > indices
Definition: seqplay.h:139
void go(const double *g, double tm)
Definition: seqplay.h:107
tick_t get_tick()
get time stamp counter
Definition: timeUtil.cpp:7


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