test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: Walk/Walk.h
- Revision:
- 29:7d8b8011a88d
- Parent:
- 27:79b4b932a6dd
- Child:
- 32:dc684a0b8448
--- a/Walk/Walk.h Wed Feb 27 01:25:01 2019 +0000 +++ b/Walk/Walk.h Wed Feb 27 12:16:18 2019 +0000 @@ -1,99 +1,76 @@ #ifndef INCLUDED_WALK_H #define INCLUDED_WALK_H #include "OneLeg.h" -enum EllipseParams -{ - STRIDE_M, - HEIGHT_M, - GROUND_M, - ELLIPSE_CENTER_X_M, - ELLIPSE_CENTER_Y_M, -}; -class EllipseOrbit -{ - float stride_m_; - float height_m_; //足の上げ幅 - float ellipsecenter_y_m_; //楕円軌道の中心高さ - float ellipsecenter_x_m_; //楕円軌道の中心x座標 - float ground_m_; //地面の高さ - int StrideLine_(OneLeg &leg, float phasetime_s); //着地している間の軌道 - int StrideLineAccel_(OneLeg &leg, float phasetime_s); //着地中に慣性力を考慮して加減速 - int RiseEllipse_(OneLeg &leg, float phasetime_s); //空中を楕円軌道の半分の軌道で動く - int RiseEllipse2_(OneLeg &leg, float phasetime_s); //空中を楕円軌道の一部欠けの形で動く - - public: - int GetOrbit(OneLeg &leg, float phasetime_s); - //paramセット用。普段はこれを使う - void SetParam(float stridetime_s, float risetime_s, float stride_m, float height_m, float ground_m, float ellipsecenter_x_m, float ellipsecenter_y_m); - // ひとつのparamだけ変更 - void ChangeOneParam(EllipseParams param, float val); - float GetOneWalkTime(); //足一周の時間 - float stridetime_s_; - float risetime_s_; -}; -struct LineParam -{ - float time_s; //次の点に到達するまでにかける時間 - float x_m; - float y_m; -}; -class FreeLineOrbit -{ - LineParam lineparams_[10]; //pointの数はmax10としている。多めにとっているだけ。 - int point_num_; //pointの数 - public: - void SetFreeLinesParam(LineParam lineparams[], int point_num); //任意の直線の軌道を設定 - int GetOrbit(OneLeg &leg, float phasetime_s); - float GetOneWalkTime(); //足一周の時間 - //Standupのparam設定 - void SetStandParam(float x_m, float y_m, float time_s); -}; enum OrbitPattern { - ELLIPSE, - FREELINES, + TRIANGLE, }; -//足の軌道を設定するクラスの皮。全ての軌道クラスを継承して一つにまとめる。patternで使い分ける -class Orbit : public EllipseOrbit, public FreeLineOrbit +//下記の論文を参照 +//https://static1.squarespace.com/static/542ddec8e4b0158794bd1036/t/5a861d1cc83025f3d460dfc3/1518738728029/gait-design-optimization-Final.pdf +class TriangleOrbit { - OrbitPattern pattern_; + float reverse_tanbeta_; //論文のβのtan + float start_x_m_; + float start_y_m_; + float stride_m_; + float height_m_; //足上げ幅 + float buffer_height_m_; //着地直前で止める高さ + float stridetime_s_; + float toptime_s_; //頂点に行くまでの時間 + float buffer_time_s_; //一時停止点から着地するまでの時間. + + float top_x_m_, top_y_m_, buffer_x_m_, buffer_y_m_; //事前に計算して保存しておく + int StrideLine_(OneLeg &leg, float phasetime_s); //着地している間の軌道 + int StrideLineAccel_(OneLeg &leg, float phasetime_s); //着地中に慣性力を考慮して加減速 - public: - Orbit(){}; //ただ宣言する用 - Orbit(OrbitPattern pattern); - float GetOneWalkTime(); //足一周の時間 - int GetOrbit(OneLeg &leg, float phasetime_s); //legに目標x,yを代入 +public: + void SetTriangleParam(float start_x_m, float start_y_m, float stride_m, float height_m, float buffer_height_m, + float stridetime_s, float toptime_s, float buffer_time_s); + int GetOrbit(OneLeg &leg, float phasetime_s); + float GetOneWalkTime(); //足一周の時間 +}; + +//足の軌道を設定するクラスの皮。全ての軌道クラスを継承して一つにまとめる。patternで使い分ける +class Orbit : public TriangleOrbit +{ + OrbitPattern pattern_; + +public: + Orbit(){}; //ただ宣言する用 + Orbit(OrbitPattern pattern); + float GetOneWalkTime(); //足一周の時間 + int GetOrbit(OneLeg &leg, float phasetime_s); //legに目標x,yを代入 }; class Walk { - Orbit orbit_[4]; - float offset_multi_[4]; + Orbit orbit_[4]; + float offset_multi_[4]; - public: - Walk(); - static float calctime_s_; //計算周期 - float phasetime_s_[4]; - //参照渡しされたonelegそれぞれにorbit_::GetOrbit関数を代入 - //GetOrbit関数にて軌道, 時間をもとに位置を決める - //直接参照しているのはこの関数だけ - void Cal4LegsPosi(OneLeg leg[4]); - //足一周の時間 - float GetOneWalkTime(); - //位相ずれの程度(値域[0,1])を入れる。 - void SetOffset(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3); - //軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out - // またoffsetの設定はここで行うので必ず呼び出す。 - int CheckOrbit(OneLeg templateleg); - //一つの足の軌道を設定.軌道の代入にはこの関数を必ず通るようにする。 - void SetOneOrbit(int legnum, Orbit orbit); - //全足の軌道を一括で設定. - void SetAllOrbit(Orbit orbit); - void ChangeOneParam(int legnum, EllipseParams param, float val); - //standのparamを4足一括で設定できる - void SetAllLegStandParam(float x_m, float y_m, float time_s); - //ellipseのparamを4足一括で設定できる - void SetAllLegEllipseParam(float stridetime_s, float risetime_s, - float stride_m, float height_m, float ground_m, - float ellipsecenter_x_m, float ellipsecenter_y_m); +public: + Walk(); + static float calctime_s_; //計算周期 + float phasetime_s_[4]; + //参照渡しされたonelegそれぞれにorbit_::GetOrbit関数を代入 + //GetOrbit関数にて軌道, 時間をもとに位置を決める + //直接参照しているのはこの関数だけ + int Cal4LegsPosi(OneLeg leg[4]); //失敗したら1を返す。成功なら0 + //足一周の時間 + float GetOneWalkTime(); + //位相ずれの程度(値域[0,1])を入れる。 + void SetOffsetTime(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3); + //軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out + // またoffsetの設定はここで行うので必ず呼び出す。 + int CheckOrbit(OneLeg templateleg); + //一つの足の軌道を設定.軌道の代入にはこの関数を必ず通るようにする。 + void SetOneOrbit(int legnum, Orbit orbit); + void SetAllOrbit(Orbit orbit); //全足の軌道を一括で設定. + //stand用 + void SetOneLegStandParam(int legnum, float x_m, float y_m, float time_s); + void SetAllLegStandParam(float x_m, float y_m, float time_s); // 4足一括で設定できる + //triangle + void SetOneLegTriangleParam(int legnum, float start_x_m, float start_y_m, float stride_m, float height_m, float buffer_height_m, + float stridetime_s, float toptime_s, float buffer_time_s); + void SetAllLegTriangleParam(float start_x_m, float start_y_m, float stride_m, float height_m, float buffer_height_m, + float stridetime_s, float toptime_s, float buffer_time_s); }; #endif \ No newline at end of file