Diff: Walk/Walk.h

Revision:

29:7d8b8011a88d

Parent:

27:79b4b932a6dd

Child:

32:dc684a0b8448

diff -r 8e1cbeffe6c2 -r 7d8b8011a88d Walk/Walk.h
--- 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