test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Walk/Walk.h@34:89d701e15cdf, 2019-03-01 (annotated)
- Committer:
- shimizuta
- Date:
- Fri Mar 01 12:07:23 2019 +0000
- Revision:
- 34:89d701e15cdf
- Parent:
- 32:dc684a0b8448
- Child:
- 35:b4e1b8f25cd7
over come and rope
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 | 34:89d701e15cdf | 4 | enum LegNum //足とシリアルサーボの番号 |
shimizuta | 34:89d701e15cdf | 5 | { |
shimizuta | 34:89d701e15cdf | 6 | RIGHT_B, |
shimizuta | 34:89d701e15cdf | 7 | RIGHT_F, |
shimizuta | 34:89d701e15cdf | 8 | LEFT_B, |
shimizuta | 34:89d701e15cdf | 9 | LEFT_F, |
shimizuta | 34:89d701e15cdf | 10 | }; |
shimizuta | 32:dc684a0b8448 | 11 | enum TriangleParams |
shimizuta | 27:79b4b932a6dd | 12 | { |
shimizuta | 32:dc684a0b8448 | 13 | OFFSET_X_M, |
shimizuta | 32:dc684a0b8448 | 14 | OFFSET_Y_M, |
shimizuta | 32:dc684a0b8448 | 15 | STRIDE_M, |
shimizuta | 32:dc684a0b8448 | 16 | HEIGHT_M, |
shimizuta | 32:dc684a0b8448 | 17 | BUFFER_HEIGHT_M, |
shimizuta | 27:79b4b932a6dd | 18 | }; |
shimizuta | 29:7d8b8011a88d | 19 | //下記の論文を参照 |
shimizuta | 29:7d8b8011a88d | 20 | //https://static1.squarespace.com/static/542ddec8e4b0158794bd1036/t/5a861d1cc83025f3d460dfc3/1518738728029/gait-design-optimization-Final.pdf |
shimizuta | 29:7d8b8011a88d | 21 | class TriangleOrbit |
shimizuta | 27:79b4b932a6dd | 22 | { |
shimizuta | 29:7d8b8011a88d | 23 | float reverse_tanbeta_; //論文のβのtan |
shimizuta | 32:dc684a0b8448 | 24 | float offset_x_m_; |
shimizuta | 32:dc684a0b8448 | 25 | float offset_y_m_; |
shimizuta | 29:7d8b8011a88d | 26 | float stride_m_; |
shimizuta | 29:7d8b8011a88d | 27 | float height_m_; //足上げ幅 |
shimizuta | 29:7d8b8011a88d | 28 | float buffer_height_m_; //着地直前で止める高さ |
shimizuta | 29:7d8b8011a88d | 29 | float stridetime_s_; |
shimizuta | 29:7d8b8011a88d | 30 | float toptime_s_; //頂点に行くまでの時間 |
shimizuta | 29:7d8b8011a88d | 31 | float buffer_time_s_; //一時停止点から着地するまでの時間. |
shimizuta | 29:7d8b8011a88d | 32 | |
shimizuta | 29:7d8b8011a88d | 33 | float top_x_m_, top_y_m_, buffer_x_m_, buffer_y_m_; //事前に計算して保存しておく |
shimizuta | 29:7d8b8011a88d | 34 | int StrideLine_(OneLeg &leg, float phasetime_s); //着地している間の軌道 |
shimizuta | 29:7d8b8011a88d | 35 | int StrideLineAccel_(OneLeg &leg, float phasetime_s); //着地中に慣性力を考慮して加減速 |
shimizuta | 11:e81425872740 | 36 | |
shimizuta | 29:7d8b8011a88d | 37 | public: |
shimizuta | 34:89d701e15cdf | 38 | void CalOtherParam(); |
shimizuta | 32:dc684a0b8448 | 39 | void SetTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m, |
shimizuta | 29:7d8b8011a88d | 40 | float stridetime_s, float toptime_s, float buffer_time_s); |
shimizuta | 29:7d8b8011a88d | 41 | int GetOrbit(OneLeg &leg, float phasetime_s); |
shimizuta | 29:7d8b8011a88d | 42 | float GetOneWalkTime(); //足一周の時間 |
shimizuta | 32:dc684a0b8448 | 43 | void ChangeOneParam(TriangleParams param, float val); |
shimizuta | 29:7d8b8011a88d | 44 | }; |
shimizuta | 29:7d8b8011a88d | 45 | |
shimizuta | 32:dc684a0b8448 | 46 | struct LineParam |
shimizuta | 32:dc684a0b8448 | 47 | { |
shimizuta | 34:89d701e15cdf | 48 | float time_s; //x_m,y_mにいる時間 |
shimizuta | 32:dc684a0b8448 | 49 | float x_m; |
shimizuta | 32:dc684a0b8448 | 50 | float y_m; |
shimizuta | 32:dc684a0b8448 | 51 | }; |
shimizuta | 32:dc684a0b8448 | 52 | class FreeLineOrbit |
shimizuta | 32:dc684a0b8448 | 53 | { |
shimizuta | 34:89d701e15cdf | 54 | LineParam lineparams_[30]; //pointの数は多めにとっている。 |
shimizuta | 32:dc684a0b8448 | 55 | int point_num_; //pointの数 |
shimizuta | 32:dc684a0b8448 | 56 | public: |
shimizuta | 32:dc684a0b8448 | 57 | void SetFreeLinesParam(LineParam lineparams[], int point_num); //任意の直線の軌道を設定 |
shimizuta | 32:dc684a0b8448 | 58 | int GetOrbit(OneLeg &leg, float phasetime_s); |
shimizuta | 34:89d701e15cdf | 59 | float GetOneWalkTime(); //足一周の時間 |
shimizuta | 34:89d701e15cdf | 60 | void SetStandParam(float x_m, float y_m, float time_s); //Standupのparam設定 |
shimizuta | 32:dc684a0b8448 | 61 | }; |
shimizuta | 32:dc684a0b8448 | 62 | enum OrbitPattern |
shimizuta | 32:dc684a0b8448 | 63 | { |
shimizuta | 32:dc684a0b8448 | 64 | TRIANGLE, |
shimizuta | 32:dc684a0b8448 | 65 | FREELINES, |
shimizuta | 32:dc684a0b8448 | 66 | }; |
shimizuta | 29:7d8b8011a88d | 67 | //足の軌道を設定するクラスの皮。全ての軌道クラスを継承して一つにまとめる。patternで使い分ける |
shimizuta | 32:dc684a0b8448 | 68 | //関数は継承元のものも使えるので、継承元のクラスも見てください |
shimizuta | 32:dc684a0b8448 | 69 | class Orbit : public TriangleOrbit, public FreeLineOrbit |
shimizuta | 29:7d8b8011a88d | 70 | { |
shimizuta | 29:7d8b8011a88d | 71 | OrbitPattern pattern_; |
shimizuta | 29:7d8b8011a88d | 72 | |
shimizuta | 29:7d8b8011a88d | 73 | public: |
shimizuta | 29:7d8b8011a88d | 74 | Orbit(){}; //ただ宣言する用 |
shimizuta | 29:7d8b8011a88d | 75 | Orbit(OrbitPattern pattern); |
shimizuta | 29:7d8b8011a88d | 76 | float GetOneWalkTime(); //足一周の時間 |
shimizuta | 29:7d8b8011a88d | 77 | int GetOrbit(OneLeg &leg, float phasetime_s); //legに目標x,yを代入 |
shimizuta | 27:79b4b932a6dd | 78 | }; |
shimizuta | 11:e81425872740 | 79 | class Walk |
shimizuta | 11:e81425872740 | 80 | { |
shimizuta | 34:89d701e15cdf | 81 | Orbit orbit_[4]; |
shimizuta | 29:7d8b8011a88d | 82 | float offset_multi_[4]; |
shimizuta | 11:e81425872740 | 83 | |
shimizuta | 29:7d8b8011a88d | 84 | public: |
shimizuta | 29:7d8b8011a88d | 85 | Walk(); |
shimizuta | 29:7d8b8011a88d | 86 | static float calctime_s_; //計算周期 |
shimizuta | 29:7d8b8011a88d | 87 | float phasetime_s_[4]; |
shimizuta | 29:7d8b8011a88d | 88 | //参照渡しされたonelegそれぞれにorbit_::GetOrbit関数を代入 |
shimizuta | 29:7d8b8011a88d | 89 | //GetOrbit関数にて軌道, 時間をもとに位置を決める |
shimizuta | 29:7d8b8011a88d | 90 | //直接参照しているのはこの関数だけ |
shimizuta | 29:7d8b8011a88d | 91 | int Cal4LegsPosi(OneLeg leg[4]); //失敗したら1を返す。成功なら0 |
shimizuta | 29:7d8b8011a88d | 92 | //足一周の時間 |
shimizuta | 29:7d8b8011a88d | 93 | float GetOneWalkTime(); |
shimizuta | 29:7d8b8011a88d | 94 | //位相ずれの程度(値域[0,1])を入れる。 |
shimizuta | 29:7d8b8011a88d | 95 | void SetOffsetTime(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3); |
shimizuta | 29:7d8b8011a88d | 96 | //軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out |
shimizuta | 29:7d8b8011a88d | 97 | // またoffsetの設定はここで行うので必ず呼び出す。 |
shimizuta | 29:7d8b8011a88d | 98 | int CheckOrbit(OneLeg templateleg); |
shimizuta | 29:7d8b8011a88d | 99 | //一つの足の軌道を設定.軌道の代入にはこの関数を必ず通るようにする。 |
shimizuta | 29:7d8b8011a88d | 100 | void SetOneOrbit(int legnum, Orbit orbit); |
shimizuta | 29:7d8b8011a88d | 101 | void SetAllOrbit(Orbit orbit); //全足の軌道を一括で設定. |
shimizuta | 29:7d8b8011a88d | 102 | //stand用 |
shimizuta | 29:7d8b8011a88d | 103 | void SetOneLegStandParam(int legnum, float x_m, float y_m, float time_s); |
shimizuta | 29:7d8b8011a88d | 104 | void SetAllLegStandParam(float x_m, float y_m, float time_s); // 4足一括で設定できる |
shimizuta | 29:7d8b8011a88d | 105 | //triangle |
shimizuta | 32:dc684a0b8448 | 106 | 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 | 107 | float stridetime_s, float toptime_s, float buffer_time_s); |
shimizuta | 32:dc684a0b8448 | 108 | void SetAllLegTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m, |
shimizuta | 29:7d8b8011a88d | 109 | float stridetime_s, float toptime_s, float buffer_time_s); |
shimizuta | 32:dc684a0b8448 | 110 | void ChangeOneParam(int legnum, TriangleParams param, float val); |
shimizuta | 32:dc684a0b8448 | 111 | //freeline |
shimizuta | 32:dc684a0b8448 | 112 | void SetOneLegFreeLinesParam(int legnum, LineParam lineparams[], int point_num); |
shimizuta | 32:dc684a0b8448 | 113 | void SetAllLegFreeLinesParam(LineParam lineparams[], int point_num); |
shimizuta | 11:e81425872740 | 114 | }; |
shimizuta | 32:dc684a0b8448 | 115 | |
shimizuta | 34:89d701e15cdf | 116 | class OverCome |
shimizuta | 34:89d701e15cdf | 117 | { |
shimizuta | 34:89d701e15cdf | 118 | float start_x_m_[4]; //足のスタートx |
shimizuta | 34:89d701e15cdf | 119 | float start_y_m_[4]; //足のスタートy |
shimizuta | 34:89d701e15cdf | 120 | float d_x_m_; //目標地点までのx |
shimizuta | 34:89d701e15cdf | 121 | float goal_y_m_[4]; //目標地点までのy |
shimizuta | 34:89d701e15cdf | 122 | float height_m_[4]; |
shimizuta | 34:89d701e15cdf | 123 | float gravity_dist_[4]; //重心ずらし距離 |
shimizuta | 34:89d701e15cdf | 124 | float d_time_; //各動きの時間 |
shimizuta | 34:89d701e15cdf | 125 | float d_time_slow_; |
shimizuta | 34:89d701e15cdf | 126 | int next_point_; //次のparamのindex |
shimizuta | 34:89d701e15cdf | 127 | LineParam legs_[4][30]; //多めに |
shimizuta | 34:89d701e15cdf | 128 | void Rise(int legnum); |
shimizuta | 34:89d701e15cdf | 129 | void Land(int legnum); |
shimizuta | 34:89d701e15cdf | 130 | void Forward(int legnum); |
shimizuta | 34:89d701e15cdf | 131 | void Step(int legnum); |
shimizuta | 34:89d701e15cdf | 132 | void GravityMove(int legnum); |
shimizuta | 34:89d701e15cdf | 133 | void StartPoint(); |
shimizuta | 34:89d701e15cdf | 134 | void GoalPoint(); |
shimizuta | 34:89d701e15cdf | 135 | void GetLine(); |
shimizuta | 34:89d701e15cdf | 136 | |
shimizuta | 34:89d701e15cdf | 137 | public: |
shimizuta | 34:89d701e15cdf | 138 | Walk walk; |
shimizuta | 34:89d701e15cdf | 139 | OverCome(float start_x_m[4], float start_y_m[4], |
shimizuta | 34:89d701e15cdf | 140 | float d_x_m, float goal_y_m[4], float height_m[4], float gravity_dist[4]); |
shimizuta | 34:89d701e15cdf | 141 | }; |
shimizuta | 11:e81425872740 | 142 | #endif |