[rtc/AutoBalancer/GaitGenerator.h] set initial state to modify bug of leg coords generator at walk with no double support phase

[Tnoriaki]

get_trajectory_point関数を使う脚軌道の時に両脚支持期を0にすると脚軌道の最初のpos等がsetされず値が飛ぶbugがあり，それを直すPRです．

[snozawa]

別途if文を追加するというよりは、今あるif文で条件をかえて対応できればと思ってます。

ところでこれは

diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h                                                                                                             
index 00c9c8a..59afe1c 100644                                                                                                                                                                
--- a/rtc/AutoBalancer/GaitGenerator.h                                                                                                                                                       
+++ b/rtc/AutoBalancer/GaitGenerator.h                                                                                                                                                       
@@ -443,7 +443,7 @@ namespace rats
       };                                                                                                                                                                                    
       void get_trajectory_point (hrp::Vector3& ret, const hrp::Vector3& start, const hrp::Vector3& goal, const double height)                                                               
       {                                                                                                                                                                                     
-        if ( double_support_count_before <= current_count && current_count < one_step_count - double_support_count_after ) { // swing phase                                                 
+        if ( double_support_count_before < current_count && current_count < one_step_count - double_support_count_after ) { // swing phase                                                  
           size_t swing_remain_count = one_step_count - current_count - double_support_count_after;                                                                                          
           size_t swing_one_step_count = one_step_count - double_support_count_before - double_support_count_after;                                                                          
           double final_path_distance_ratio = calc_antecedent_path(start, goal, height);                                                                                                     
@@ -470,7 +470,7 @@ namespace rats
           } else {                                                                                                                                                                          
             pos(2) = goal(2);                                                                                                                                                               
           }                                                                                                                                                                                 
-        } else if ( current_count < double_support_count_before ) { // first double support phase                                                                                           
+        } else if ( current_count <= double_support_count_before ) { // first double support phase                                                                                          
           pos = start;                                                                                                                                                                      
           vel = hrp::Vector3::Zero();                                                                                                                                                       
           acc = hrp::Vector3::Zero();       

みたいに、beforeのほうのswing期の境界がどっちに含まれるかをかえるのでも治ったりするのかな。
これで問題ないか（両脚０でとぶバグがなおる+今までの使い方でも問題ない）を確認してもらえるとたすかります。
（もしかしたら１カウント分、ゆう脚期がすくなくなってしまう？）

[Tnoriaki]

@snozawaさん
if文を変えるだけでも治りますが，１カウント分遊脚期間が少なくなるのでif文を付け足しました．
ただ1カウント分だけですし，コードが読みにくくなるのでif文の条件を変えるようにします．

[snozawa]

if文を変えるだけでも治りますが，１カウント分遊脚期間が少なくなるのでif文を付け足しました．

やはり１カウント文短くなるんだね。
@kyawawa , @YutaKojio, @Tnoriaki
的に問題ないのかな？

ただ1カウント分だけですし，コードが読みにくくなるのでif文の条件を変えるようにします．

もともとのPRはif文追加だったので、似たようなif文が２こでてしまうのと、
current_count==0な状況では、新たに追加したif文ともとからあるif文のうちの

        } else if ( current_count < double_support_count_before ) { // first double support phase                                                                                           

の２箇所にヒットしてしまうので、まとめられればと思いました。
まとめかたは
#1157 (comment)
がぱっと思いついたのでかいてみましたが、１カウント少なくなるのは問題かなあと思います。
他には、double_support_count_beforeが０かどうかをチェックするという方法もありそうかな。

[kyawawa]

1カウント少なくするこの方法だと
https://github.com/fkanehiro/hrpsys-base/pull/1157/files#diff-c0c1106962689a7d535262184ade16dbR448
ここも変更する必要があるかと思います．

if-elseのまとまりが増えますが，

else if ( current_count <= double_support_count_before )

の部分をまとめて一番上に持っていく方法もありそうです

[YutaKojio]

１制御周期なので１カウント少なくなるのは実用上問題ないと思いますが，
１カウント少なくならないほうが意図した挙動なのでできるなら減らないほうがいい気がします．

[Tnoriaki]

@kyawawa, @YutaKojio さんの意見を参考に
1カウント少なくならないようにしてみましたがどうでしょうか

[snozawa]

@Tnoriaki
内容確認しました。
travisが通ってないのでrestartするという意味もこめて、
追加でコミットが３つになってるところを１個にまとめて（コミットメッセージは元の内容と個々での議論の内容がいいかんじではいるようにしてもらうと良いかも）
git push -fしてもらえますでしょうか。

[Tnoriaki]

commitを一つにしてpushし，travisも通りました．

[snozawa]

@Tnoriaki
LGTM、ありがとう

@fkanehiro さん
お手すきの時で構いませんのでご確認いただけますと幸いです。

[snozawa]

ありがとうございます。