GaitGeneration by Graph Search
読み取り中…
検索中…
一致する文字列を見つけられません
leg_state.cpp
[詳解]
1
3
4// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
5// Released under the MIT license
6// https://opensource.org/licenses/mit-license.php
7
8#include "leg_state.h"
9
10#include "cassert_define.h"
11
13
15 const enums::DiscreteComPos discrete_com_pos,
16 const std::array<bool, HexapodConst::kLegNum>& is_ground,
17 const std::array<DiscreteLegPos, HexapodConst::kLegNum>&
18 discretized_leg_pos) {
19 LegStateBit res = 0;
20
21 // bitに変換する.
22 LegStateBit discrete_com_pos_bit =
23 static_cast<unsigned int>(discrete_com_pos);
24 res |= discrete_com_pos_bit
25 << kShiftToComNum; // 重心パターンの数値だけビットを立てる.
26
27 for (int i = 0; i < HexapodConst::kLegNum; i++) {
28 // 接地しているならば上位ビットを立てる.
29 size_t ground_bit_index = static_cast<size_t>(i + 1) * 4 - 1;
30 if (is_ground[i]) {
31 res[ground_bit_index] = true;
32 }
33
34 // 脚のビットを立てる.
35 LegStateBit discrete_leg_pos_bit =
36 static_cast<unsigned int>(discretized_leg_pos[i]);
37 size_t shift_num = static_cast<size_t>(i) * 4; // 4bitずつずらす
38
39 res |= discrete_leg_pos_bit
40 << shift_num; // 脚の位置の数値だけビットを立てる
41 }
42
43 return res;
44}
45
46bool IsGrounded(const LegStateBit& leg_state, const int leg_index) {
47 // leg_indexは0~5の範囲にある必要がある.
48 assert(0 <= leg_index);
49 assert(leg_index < HexapodConst::kLegNum);
50
51 // 指定された脚の接地脚のビットが立っているか調べる.
52 size_t ground_bit_index = static_cast<size_t>(leg_index + 1) * 4 - 1;
53
54 if (leg_state[ground_bit_index]) {
55 return true;
56 } else {
57 return false;
58 }
59}
60
63
64 for (int i = 0; i < HexapodConst::kLegNum; i++) {
65 if (IsGrounded(leg_state, i)) {
66 res[i] = true;
67 } else {
68 res[i] = false;
69 }
70 }
71
72 return res;
73}
74
75int GetGroundedLegNum(const LegStateBit& leg_state) {
76 int res = 0;
77
78 // 脚の本数分ループする.
79 for (int i = 0; i < HexapodConst::kLegNum; i++) {
80 if (IsGrounded(leg_state, i)) {
81 // 接地している脚があればカウントアップする.
82 res++;
83 }
84 }
85
86 return res;
87}
88
89int GetLiftedLegNum(const LegStateBit& leg_state) {
90 return HexapodConst::kLegNum - GetGroundedLegNum(leg_state);
91}
92
94 std::vector<int>* res_index) {
95 // res_index は nullptr でないこと,かつ空である必要がある.
96 assert(res_index != nullptr);
97 assert((*res_index).size() == 0);
98
99 // 脚は6本あるので6回ループする.
100 for (int i = 0; i < HexapodConst::kLegNum; i++) {
101 if (IsGrounded(leg_state, i)) {
102 // 接地している脚の脚番号を vector に代入.
103 (*res_index).push_back(i);
104 }
105 }
106}
107
109 std::vector<int>* res_index) {
110 // res_index は nullptr でないこと,かつ空である必要がある.
111 assert(res_index != nullptr);
112 assert((*res_index).size() == 0);
113
114 // 脚は6本あるので6回ループする.
115 for (int i = 0; i < HexapodConst::kLegNum; i++) {
116 if (!IsGrounded(leg_state, i)) {
117 // 浮いている脚の脚番号を vector に代入.
118 (*res_index).push_back(i);
119 }
120 }
121}
122
124 const int leg_index) {
125 // leg_indexは0~5の範囲にある必要がある.
126 assert(0 <= leg_index);
127 assert(leg_index < HexapodConst::kLegNum);
128
129 const int shift_num = 4 * leg_index; // 4bitずつずらす
130
131 const int res = static_cast<int>(
132 ((leg_state & (kLegPosMaskBit << shift_num)) >> shift_num).to_ulong());
133
134 return static_cast<DiscreteLegPos>(res);
135}
136
138 // 重心パターンを保存するビットをマスクし,その値だけ取得できるように右へシフトする.
139 const int res = static_cast<int>(
140 ((leg_state & kComStateMaskBit) >> kShiftToComNum).to_ulong());
141
142 return static_cast<enums::DiscreteComPos>(res);
143}
144
145void ChangeLegState(const int leg_index,
146 const DiscreteLegPos new_discretized_leg_pos,
147 const bool is_ground, LegStateBit* leg_state) {
148 // leg_indexは0~5の範囲にある必要がある.
149 assert(0 <= leg_index);
150 assert(leg_index < HexapodConst::kLegNum);
151
152 // leg_state は nullptrではない
153 assert(leg_state != nullptr);
154
155 ChangeDiscreteLegPos(leg_index, new_discretized_leg_pos, leg_state);
156 ChangeGround(leg_index, is_ground, leg_state);
157}
158
159void ChangeDiscreteLegPos(const int leg_index,
160 const DiscreteLegPos new_discretized_leg_pos,
161 LegStateBit* leg_state) {
162 // leg_indexは0~5の範囲にある必要がある.
163 assert(0 <= leg_index);
164 assert(leg_index < HexapodConst::kLegNum);
165
166 // leg_state は nullptrではない.
167 assert(leg_state != nullptr);
168
169 // 新しい脚状態を生成する.
170 const size_t shift_num =
171 static_cast<size_t>(leg_index) * 4; // 4bitずつずらす.
172
173 // 4bitのデータを変更する地点までマスクをずらす.
174 const LegStateBit mask = kLegPosMaskBit << shift_num;
175
176 // bitに変換する.
177 const LegStateBit discreate_leg_pos_bit =
178 static_cast<unsigned int>(new_discretized_leg_pos);
179
180 // 脚位置のデータは4bitづつ配置されているのでその位置まで移動する.
181 const LegStateBit state = discreate_leg_pos_bit << shift_num;
182
183 // 浮いている脚の脚位置のみを変更(排他的論理和による特定ビットの交換.
184 // 参考 : https://qiita.com/vivisuke/items/bc707190e008551ca07f (アクセス日
185 // 2023/12/27)
186 LegStateBit res = ((*leg_state) ^ state) & mask;
187 (*leg_state) ^= res;
188}
189
190void ChangeGround(const int leg_index, const bool is_ground,
191 LegStateBit* leg_state) {
192 // leg_indexは0~5の範囲にある必要がある.
193 assert(0 <= leg_index);
194 assert(leg_index < HexapodConst::kLegNum);
195
196 // leg_state は nullptrではない
197 assert(leg_state != nullptr);
198
199 // 指定された脚の接地脚のビットを立てるか消すかする.
200 const size_t ground_bit_index = static_cast<size_t>(leg_index + 1) * 4 - 1;
201
202 if (is_ground) {
203 (*leg_state)[ground_bit_index] = true;
204 } else {
205 (*leg_state)[ground_bit_index] = false;
206 }
207}
208
209void ChangeAllLegGround(const LegGroundedBit& is_ground_list,
210 LegStateBit* leg_state) {
211 // leg_state は nullptrではない.
212 assert(leg_state != nullptr);
213
214 for (int i = 0; i < HexapodConst::kLegNum; i++) {
215 ChangeGround(i, is_ground_list[i], leg_state);
216 }
217}
218
220 LegStateBit* leg_state) {
221 // leg_state は nullptrではない.
222 assert(leg_state != nullptr);
223
224 const LegStateBit state = static_cast<unsigned int>(new_com_pattern)
226 LegStateBit sub = ((*leg_state) ^ state) & kComStateMaskBit;
227 (*leg_state) ^= sub;
228}
229
230} // namespace designlab::leg_func
static constexpr int kLegNum
このプログラムでは脚状態をビット(28bit)の情報で表す. そのデータを処理するための関数.
Definition leg_state.cpp:12
void ChangeLegState(const int leg_index, const DiscreteLegPos new_discretized_leg_pos, const bool is_ground, LegStateBit *leg_state)
脚の情報を変更する.
constexpr LegStateBit kLegPosMaskBit(0b0111)
脚位置は4bitの下位三桁で管理されるので,そこをマスクする.
int GetGroundedLegNum(const LegStateBit &leg_state)
接地している脚の本数を返す関数.
Definition leg_state.cpp:75
std::bitset< HexapodConst::kLegNum > LegGroundedBit
脚の遊脚・接地を表す型.6bitのビット型.接地が 1 遊脚が 0.
Definition leg_state.h:58
bool IsGrounded(const LegStateBit &leg_state, const int leg_index)
脚番号 leg_index 0 ~ 5 に応じて,その脚が接地しているかを調べる. 脚は右前脚を0番として,時計回りに0,1,2,3,4,5となる.左前足が5番.
Definition leg_state.cpp:46
DiscreteLegPos GetDiscreteLegPos(const LegStateBit &leg_state, const int leg_index)
脚状態を取得する.
constexpr int kShiftToComNum
重心パターンを保存するビットまで行くために,どれだけビットをシフトするか.
Definition leg_state.h:69
int GetLiftedLegNum(const LegStateBit &leg_state)
遊脚している脚の本数を返す関数.
Definition leg_state.cpp:89
constexpr LegStateBit kComStateMaskBit
重心パターンを保存するビットをマスクするビット.
Definition leg_state.h:72
void ChangeDiscreteLegPos(const int leg_index, const DiscreteLegPos new_discretized_leg_pos, LegStateBit *leg_state)
脚の状態を変更する.遊脚を表すビットはそのまま.
enums::DiscreteComPos GetDiscreteComPos(const LegStateBit &leg_state)
現在の脚状態から重心パターンを取得する.
void ChangeDiscreteComPos(const enums::DiscreteComPos new_com_pattern, LegStateBit *leg_state)
重心のデータを変更する.
LegStateBit MakeLegStateBit(const enums::DiscreteComPos discrete_com_pos, const std::array< bool, HexapodConst::kLegNum > &is_ground, const std::array< DiscreteLegPos, HexapodConst::kLegNum > &discretized_leg_pos)
脚状態を作成して返す関数.脚状態は重心パターン, 脚の接地・遊脚,離散化した脚位置のデータが含まれる.
Definition leg_state.cpp:14
void ChangeGround(const int leg_index, const bool is_ground, LegStateBit *leg_state)
脚の接地・遊脚情報を変更する.
void GetLiftedLegIndexByVector(const LegStateBit &leg_state, std::vector< int > *res_index)
遊脚している脚の脚番号0~5を,引数_res_numberで参照渡しする関数.
LegGroundedBit GetLegGroundedBit(const LegStateBit &leg_state)
脚が接地しているなら1,遊脚を0としたビット列で遊脚・接地脚の状態を返す. 例えば 0 番脚のみが遊脚しているなら 0b111 110 を返す.
Definition leg_state.cpp:61
void GetGroundedLegIndexByVector(const LegStateBit &leg_state, std::vector< int > *res_index)
接地している脚の脚番号0~5を,引数で参照渡しする関数.
Definition leg_state.cpp:93
void ChangeAllLegGround(const LegGroundedBit &is_ground_list, LegStateBit *leg_state)
全ての脚の接地・遊脚情報を変更する.
std::bitset< kLegStateBitNum > LegStateBit
脚状態を保存する型.28bitのビット型.
Definition leg_state.h:55
DiscreteLegPos
離散化された脚位置を表す列挙体. 先行研究では 1~7の int型の数値で表現されているが, 可読性を上げるために列挙体にした. 離散化された脚位置は 3bit (0 ~ 7)の範囲で表現される...