test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Walk/Walk.h@32:dc684a0b8448, 2019-02-28 (annotated)
- Committer:
- shimizuta
- Date:
- Thu Feb 28 08:48:21 2019 +0000
- Revision:
- 32:dc684a0b8448
- Parent:
- 29:7d8b8011a88d
- Child:
- 34:89d701e15cdf
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimizuta | 11:e81425872740 | 1 | #ifndef INCLUDED_WALK_H |
shimizuta | 11:e81425872740 | 2 | #define INCLUDED_WALK_H |
shimizuta | 11:e81425872740 | 3 | #include "OneLeg.h" |
shimizuta | 32:dc684a0b8448 | 4 | enum TriangleParams |
shimizuta | 27:79b4b932a6dd | 5 | { |
shimizuta | 32:dc684a0b8448 | 6 | OFFSET_X_M, |
shimizuta | 32:dc684a0b8448 | 7 | OFFSET_Y_M, |
shimizuta | 32:dc684a0b8448 | 8 | STRIDE_M, |
shimizuta | 32:dc684a0b8448 | 9 | HEIGHT_M, |
shimizuta | 32:dc684a0b8448 | 10 | BUFFER_HEIGHT_M, |
shimizuta | 27:79b4b932a6dd | 11 | }; |
shimizuta | 29:7d8b8011a88d | 12 | //下記の論文を参照 |
shimizuta | 29:7d8b8011a88d | 13 | //https://static1.squarespace.com/static/542ddec8e4b0158794bd1036/t/5a861d1cc83025f3d460dfc3/1518738728029/gait-design-optimization-Final.pdf |
shimizuta | 29:7d8b8011a88d | 14 | class TriangleOrbit |
shimizuta | 27:79b4b932a6dd | 15 | { |
shimizuta | 29:7d8b8011a88d | 16 | float reverse_tanbeta_; //論文のβのtan |
shimizuta | 32:dc684a0b8448 | 17 | float offset_x_m_; |
shimizuta | 32:dc684a0b8448 | 18 | float offset_y_m_; |
shimizuta | 29:7d8b8011a88d | 19 | float stride_m_; |
shimizuta | 29:7d8b8011a88d | 20 | float height_m_; //足上げ幅 |
shimizuta | 29:7d8b8011a88d | 21 | float buffer_height_m_; //着地直前で止める高さ |
shimizuta | 29:7d8b8011a88d | 22 | float stridetime_s_; |
shimizuta | 29:7d8b8011a88d | 23 | float toptime_s_; //頂点に行くまでの時間 |
shimizuta | 29:7d8b8011a88d | 24 | float buffer_time_s_; //一時停止点から着地するまでの時間. |
shimizuta | 29:7d8b8011a88d | 25 | |
shimizuta | 29:7d8b8011a88d | 26 | float top_x_m_, top_y_m_, buffer_x_m_, buffer_y_m_; //事前に計算して保存しておく |
shimizuta | 29:7d8b8011a88d | 27 | int StrideLine_(OneLeg &leg, float phasetime_s); //着地している間の軌道 |
shimizuta | 29:7d8b8011a88d | 28 | int StrideLineAccel_(OneLeg &leg, float phasetime_s); //着地中に慣性力を考慮して加減速 |
shimizuta | 11:e81425872740 | 29 | |
shimizuta | 29:7d8b8011a88d | 30 | public: |
shimizuta | 32:dc684a0b8448 | 31 | void SetTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m, |
shimizuta | 29:7d8b8011a88d | 32 | float stridetime_s, float toptime_s, float buffer_time_s); |
shimizuta | 29:7d8b8011a88d | 33 | int GetOrbit(OneLeg &leg, float phasetime_s); |
shimizuta | 29:7d8b8011a88d | 34 | float GetOneWalkTime(); //足一周の時間 |
shimizuta | 32:dc684a0b8448 | 35 | void ChangeOneParam(TriangleParams param, float val); |
shimizuta | 29:7d8b8011a88d | 36 | }; |
shimizuta | 29:7d8b8011a88d | 37 | |
shimizuta | 32:dc684a0b8448 | 38 | struct LineParam |
shimizuta | 32:dc684a0b8448 | 39 | { |
shimizuta | 32:dc684a0b8448 | 40 | float time_s; //次の点に到達するまでにかける時間 |
shimizuta | 32:dc684a0b8448 | 41 | float x_m; |
shimizuta | 32:dc684a0b8448 | 42 | float y_m; |
shimizuta | 32:dc684a0b8448 | 43 | }; |
shimizuta | 32:dc684a0b8448 | 44 | class FreeLineOrbit |
shimizuta | 32:dc684a0b8448 | 45 | { |
shimizuta | 32:dc684a0b8448 | 46 | LineParam lineparams_[10]; //pointの数はmax10としている。多めにとっているだけ。 |
shimizuta | 32:dc684a0b8448 | 47 | int point_num_; //pointの数 |
shimizuta | 32:dc684a0b8448 | 48 | public: |
shimizuta | 32:dc684a0b8448 | 49 | void SetFreeLinesParam(LineParam lineparams[], int point_num); //任意の直線の軌道を設定 |
shimizuta | 32:dc684a0b8448 | 50 | int GetOrbit(OneLeg &leg, float phasetime_s); |
shimizuta | 32:dc684a0b8448 | 51 | float GetOneWalkTime(); //足一周の時間 |
shimizuta | 32:dc684a0b8448 | 52 | //Standupのparam設定 |
shimizuta | 32:dc684a0b8448 | 53 | void SetStandParam(float x_m, float y_m, float time_s); |
shimizuta | 32:dc684a0b8448 | 54 | }; |
shimizuta | 32:dc684a0b8448 | 55 | enum OrbitPattern |
shimizuta | 32:dc684a0b8448 | 56 | { |
shimizuta | 32:dc684a0b8448 | 57 | TRIANGLE, |
shimizuta | 32:dc684a0b8448 | 58 | FREELINES, |
shimizuta | 32:dc684a0b8448 | 59 | }; |
shimizuta | 29:7d8b8011a88d | 60 | //足の軌道を設定するクラスの皮。全ての軌道クラスを継承して一つにまとめる。patternで使い分ける |
shimizuta | 32:dc684a0b8448 | 61 | //関数は継承元のものも使えるので、継承元のクラスも見てください |
shimizuta | 32:dc684a0b8448 | 62 | class Orbit : public TriangleOrbit, public FreeLineOrbit |
shimizuta | 29:7d8b8011a88d | 63 | { |
shimizuta | 29:7d8b8011a88d | 64 | OrbitPattern pattern_; |
shimizuta | 29:7d8b8011a88d | 65 | |
shimizuta | 29:7d8b8011a88d | 66 | public: |
shimizuta | 29:7d8b8011a88d | 67 | Orbit(){}; //ただ宣言する用 |
shimizuta | 29:7d8b8011a88d | 68 | Orbit(OrbitPattern pattern); |
shimizuta | 29:7d8b8011a88d | 69 | float GetOneWalkTime(); //足一周の時間 |
shimizuta | 29:7d8b8011a88d | 70 | int GetOrbit(OneLeg &leg, float phasetime_s); //legに目標x,yを代入 |
shimizuta | 27:79b4b932a6dd | 71 | }; |
shimizuta | 11:e81425872740 | 72 | class Walk |
shimizuta | 11:e81425872740 | 73 | { |
shimizuta | 29:7d8b8011a88d | 74 | float offset_multi_[4]; |
shimizuta | 11:e81425872740 | 75 | |
shimizuta | 29:7d8b8011a88d | 76 | public: |
shimizuta | 29:7d8b8011a88d | 77 | Walk(); |
shimizuta | 32:dc684a0b8448 | 78 | Orbit orbit_[4]; |
shimizuta | 29:7d8b8011a88d | 79 | static float calctime_s_; //計算周期 |
shimizuta | 29:7d8b8011a88d | 80 | float phasetime_s_[4]; |
shimizuta | 29:7d8b8011a88d | 81 | //参照渡しされたonelegそれぞれにorbit_::GetOrbit関数を代入 |
shimizuta | 29:7d8b8011a88d | 82 | //GetOrbit関数にて軌道, 時間をもとに位置を決める |
shimizuta | 29:7d8b8011a88d | 83 | //直接参照しているのはこの関数だけ |
shimizuta | 29:7d8b8011a88d | 84 | int Cal4LegsPosi(OneLeg leg[4]); //失敗したら1を返す。成功なら0 |
shimizuta | 29:7d8b8011a88d | 85 | //足一周の時間 |
shimizuta | 29:7d8b8011a88d | 86 | float GetOneWalkTime(); |
shimizuta | 29:7d8b8011a88d | 87 | //位相ずれの程度(値域[0,1])を入れる。 |
shimizuta | 29:7d8b8011a88d | 88 | void SetOffsetTime(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3); |
shimizuta | 29:7d8b8011a88d | 89 | //軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out |
shimizuta | 29:7d8b8011a88d | 90 | // またoffsetの設定はここで行うので必ず呼び出す。 |
shimizuta | 29:7d8b8011a88d | 91 | int CheckOrbit(OneLeg templateleg); |
shimizuta | 29:7d8b8011a88d | 92 | //一つの足の軌道を設定.軌道の代入にはこの関数を必ず通るようにする。 |
shimizuta | 29:7d8b8011a88d | 93 | void SetOneOrbit(int legnum, Orbit orbit); |
shimizuta | 29:7d8b8011a88d | 94 | void SetAllOrbit(Orbit orbit); //全足の軌道を一括で設定. |
shimizuta | 29:7d8b8011a88d | 95 | //stand用 |
shimizuta | 29:7d8b8011a88d | 96 | void SetOneLegStandParam(int legnum, float x_m, float y_m, float time_s); |
shimizuta | 29:7d8b8011a88d | 97 | void SetAllLegStandParam(float x_m, float y_m, float time_s); // 4足一括で設定できる |
shimizuta | 29:7d8b8011a88d | 98 | //triangle |
shimizuta | 32:dc684a0b8448 | 99 | void SetOneLegTriangleParam(int legnum, float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m, |
shimizuta | 32:dc684a0b8448 | 100 | float stridetime_s, float toptime_s, float buffer_time_s); |
shimizuta | 32:dc684a0b8448 | 101 | void SetAllLegTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m, |
shimizuta | 29:7d8b8011a88d | 102 | float stridetime_s, float toptime_s, float buffer_time_s); |
shimizuta | 32:dc684a0b8448 | 103 | void ChangeOneParam(int legnum, TriangleParams param, float val); |
shimizuta | 32:dc684a0b8448 | 104 | |
shimizuta | 32:dc684a0b8448 | 105 | //freeline |
shimizuta | 32:dc684a0b8448 | 106 | void SetOneLegFreeLinesParam(int legnum, LineParam lineparams[], int point_num); |
shimizuta | 32:dc684a0b8448 | 107 | void SetAllLegFreeLinesParam(LineParam lineparams[], int point_num); |
shimizuta | 11:e81425872740 | 108 | }; |
shimizuta | 32:dc684a0b8448 | 109 | |
shimizuta | 11:e81425872740 | 110 | #endif |