SteppableRegionの受け取り方について

[Naoki-Hiraoka]

僕のソースコードの理解のどこかに間違えがあると思うのですが、指摘していただけますと幸いです。

1. l_rフラグには、現在の支持脚が入る(right: 0, left: 1). regionは、GaitGeneratorのworld座標系で表現される
2. 届いたdataのl_rフラグと、現在の支持脚が一致したら受け取る
   

   hrpsys-base/rtc/AutoBalancer/AutoBalancer.cpp

   Lines 806 to 809 in a6f1a9f

   	if ((gg->get_support_leg_names().front() == "rleg" && m_steppableRegion.data.l_r == 0) ||
   	(gg->get_support_leg_names().front() == "lleg" && m_steppableRegion.data.l_r == 1))
   	{
   	gg->set_steppable_region(m_steppableRegion);

3. lr_regionとsteppable_regionの、現在の遊脚のインデックスに格納する
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.h

   Lines 1812 to 1827 in a6f1a9f

   	void set_steppable_region (const OpenHRP::TimedSteppableRegion& _region) {
   	leg_type next_sup = (_region.data.l_r == 0 ? LLEG : RLEG);
   	steppable_region[next_sup].resize(_region.data.region.length());
   	steppable_height[next_sup].resize(_region.data.region.length(), 0.0);
   	for (size_t i = 0; i < steppable_region[next_sup].size(); i++) {
   	double height_sum = 0.0;
   	steppable_region[next_sup][i].resize(_region.data.region[i].length()/3);;
   	for (size_t j = 0; j < steppable_region[next_sup][i].size(); j++) {
   	steppable_region[next_sup][i][j](0) = _region.data.region[i][3*j];
   	steppable_region[next_sup][i][j](1) = _region.data.region[i][3*j+1];
   	height_sum += _region.data.region[i][3*j+2];
   	}
   	if (steppable_region[next_sup][i].size() > 0) steppable_height[next_sup][i] = height_sum / steppable_region[next_sup][i].size(); // average height
   	}
   	lr_region[next_sup] = true;
   	};

4. GaitGeneratorの処理中、cur_legに、現在の遊脚が入る
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.cpp

   Line 807 in a6f1a9f

   leg_type cur_leg = footstep_nodes_list[lcg.get_footstep_index()].front().l_r;

5. lr_legionの、現在の遊脚のインデックスを消す (3でせっかく格納したのに?)
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.cpp

   Line 808 in a6f1a9f

   lr_region[cur_leg] = false;

6. lr_regionの現在の支持脚のインデックスがtrueなら
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.cpp

   Line 809 in a6f1a9f

   if (lr_region[cur_leg == RLEG ? LLEG : RLEG]) {

   
   steppable_regionの、現在の支持脚のインデックスに現在の遊脚用のstride_limitationを施す
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.cpp

   Line 857 in a6f1a9f

   steppable_region[cur_leg == RLEG ? LLEG : RLEG][i] = calc_intersect_convex(steppable_region[cur_leg == RLEG ? LLEG : RLEG][i], stride_limitation_polygon);

7. steppable_regionの現在の支持脚のインデックスを用いて、現在の遊脚の着地位置をリミットする
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.cpp

   Line 1529 in a6f1a9f

   for (size_t i = 0; i < steppable_region[cur_sup].size(); i++) {

[YutaKojio]

自分で今見返してもなにやってるかすぐわからない処理になってました．．．

1. regionがどこの部分指しているかわからないけど，ROSから送るときは支持足相対で送って，hrpsys内では一歩遅れて一歩前の支持足相対（対象の遊脚の離床直前の足位置相対）で保持しています．
2. yes
3. yes. ただし，現在の遊脚と同じことだけど，6., 7. で使う場面を考えて next_support という名前にしています．
4. yes. 補足で，footstepのindexが変わる瞬間（両足支持期の真ん中）で行われます．
5. 4.でindexが変わっているので，対象にしたい足と逆の足のフラグを消していることになります．
6. yes
7. yes

タイミングを以下の手書きで示してみました．

[Naoki-Hiraoka]

ありがとうございます。

• ROSから送るときは支持足相対で送って，hrpsys内では一歩遅れて一歩前の支持足相対（対象の遊脚の離床直前の足位置相対）で保持している
• footstepのindexが変わる瞬間（両足支持期の真ん中）で行われる

の2点を見落としていました。

具体的にllegの着地位置のsteppable regionを対象に考えると、

1. llegが支持脚のときに、次のllegの一歩のsteppable regionをm_steppableRegionとして受け取る。l_rフラグには、1が入る(right: 0, left: 1).m_steppableRegion.data.regionは、現在のlleg相対の座標系で表現される
2. 届いたdataのl_rフラグと、現在の支持脚が一致したら受け取る
   

   hrpsys-base/rtc/AutoBalancer/AutoBalancer.cpp

   Lines 806 to 809 in a6f1a9f

   	if ((gg->get_support_leg_names().front() == "rleg" && m_steppableRegion.data.l_r == 0) ||
   	(gg->get_support_leg_names().front() == "lleg" && m_steppableRegion.data.l_r == 1))
   	{
   	gg->set_steppable_region(m_steppableRegion);

3. lr_regionとsteppable_regionの、rlegのインデックスに格納する(next_support という名前にしている)
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.h

   Lines 1812 to 1827 in a6f1a9f

   	void set_steppable_region (const OpenHRP::TimedSteppableRegion& _region) {
   	leg_type next_sup = (_region.data.l_r == 0 ? LLEG : RLEG);
   	steppable_region[next_sup].resize(_region.data.region.length());
   	steppable_height[next_sup].resize(_region.data.region.length(), 0.0);
   	for (size_t i = 0; i < steppable_region[next_sup].size(); i++) {
   	double height_sum = 0.0;
   	steppable_region[next_sup][i].resize(_region.data.region[i].length()/3);;
   	for (size_t j = 0; j < steppable_region[next_sup][i].size(); j++) {
   	steppable_region[next_sup][i][j](0) = _region.data.region[i][3*j];
   	steppable_region[next_sup][i][j](1) = _region.data.region[i][3*j+1];
   	height_sum += _region.data.region[i][3*j+2];
   	}
   	if (steppable_region[next_sup][i].size() > 0) steppable_height[next_sup][i] = height_sum / steppable_region[next_sup][i].size(); // average height
   	}
   	lr_region[next_sup] = true;
   	};

4. footstepのindexが変わる瞬間（両足支持期の真ん中）、4,5,6の処理が行われる.
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.cpp

   Line 806 in a6f1a9f

   if (lcg.get_lcg_count() == static_cast<size_t>(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 1.0) - 1 && lcg.get_footstep_index() > 0) {

   llegが遊脚になるので、cur_legにllegが入る
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.cpp

   Line 807 in a6f1a9f

   leg_type cur_leg = footstep_nodes_list[lcg.get_footstep_index()].front().l_r;

5. (削除)
6. lr_regionのrlegのインデックスがtrueなら
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.cpp

   Line 809 in a6f1a9f

   if (lr_region[cur_leg == RLEG ? LLEG : RLEG]) {

   
   steppable_regionの、rlegのインデックスにllegのstride_limitationを施す
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.cpp

   Line 857 in a6f1a9f

   steppable_region[cur_leg == RLEG ? LLEG : RLEG][i] = calc_intersect_convex(steppable_region[cur_leg == RLEG ? LLEG : RLEG][i], stride_limitation_polygon);

7. 以降、steppable_regionのrlegのインデックスを用いて、llegの着地位置をリミットする
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.cpp

   Line 1529 in a6f1a9f

   for (size_t i = 0; i < steppable_region[cur_sup].size(); i++) {

8. footstepのindexが変わる瞬間（両足支持期の真ん中）、llegが支持脚になり、lr_legionのrlegのインデックスを消す
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.cpp

   Line 808 in a6f1a9f

   lr_region[cur_leg] = false;