16 const std::array<bool, HexapodConst::kLegNum>& is_ground,
17 const std::array<DiscreteLegPos, HexapodConst::kLegNum>&
18 discretized_leg_pos) {
23 static_cast<unsigned int>(discrete_com_pos);
24 res |= discrete_com_pos_bit
29 size_t ground_bit_index =
static_cast<size_t>(i + 1) * 4 - 1;
31 res[ground_bit_index] =
true;
36 static_cast<unsigned int>(discretized_leg_pos[i]);
37 size_t shift_num =
static_cast<size_t>(i) * 4;
39 res |= discrete_leg_pos_bit
48 assert(0 <= leg_index);
52 size_t ground_bit_index =
static_cast<size_t>(leg_index + 1) * 4 - 1;
54 if (leg_state[ground_bit_index]) {
94 std::vector<int>* res_index) {
96 assert(res_index !=
nullptr);
97 assert((*res_index).size() == 0);
103 (*res_index).push_back(i);
109 std::vector<int>* res_index) {
111 assert(res_index !=
nullptr);
112 assert((*res_index).size() == 0);
118 (*res_index).push_back(i);
124 const int leg_index) {
126 assert(0 <= leg_index);
129 const int shift_num = 4 * leg_index;
131 const int res =
static_cast<int>(
132 ((leg_state & (kLegPosMaskBit << shift_num)) >> shift_num).to_ulong());
139 const int res =
static_cast<int>(
149 assert(0 <= leg_index);
153 assert(leg_state !=
nullptr);
163 assert(0 <= leg_index);
167 assert(leg_state !=
nullptr);
170 const size_t shift_num =
171 static_cast<size_t>(leg_index) * 4;
178 static_cast<unsigned int>(new_discretized_leg_pos);
181 const LegStateBit state = discreate_leg_pos_bit << shift_num;
193 assert(0 <= leg_index);
197 assert(leg_state !=
nullptr);
200 const size_t ground_bit_index =
static_cast<size_t>(leg_index + 1) * 4 - 1;
203 (*leg_state)[ground_bit_index] =
true;
205 (*leg_state)[ground_bit_index] =
false;
212 assert(leg_state !=
nullptr);
222 assert(leg_state !=
nullptr);
224 const LegStateBit state =
static_cast<unsigned int>(new_com_pattern)
static constexpr int kLegNum
このプログラムでは脚状態をビット(28bit)の情報で表す. そのデータを処理するための関数.
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)
接地している脚の本数を返す関数.
std::bitset< HexapodConst::kLegNum > LegGroundedBit
脚の遊脚・接地を表す型.6bitのビット型.接地が 1 遊脚が 0.
bool IsGrounded(const LegStateBit &leg_state, const int leg_index)
脚番号 leg_index 0 ~ 5 に応じて,その脚が接地しているかを調べる. 脚は右前脚を0番として,時計回りに0,1,2,3,4,5となる.左前足が5番.
DiscreteLegPos GetDiscreteLegPos(const LegStateBit &leg_state, const int leg_index)
脚状態を取得する.
constexpr int kShiftToComNum
重心パターンを保存するビットまで行くために,どれだけビットをシフトするか.
int GetLiftedLegNum(const LegStateBit &leg_state)
遊脚している脚の本数を返す関数.
constexpr LegStateBit kComStateMaskBit
重心パターンを保存するビットをマスクするビット.
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)
脚状態を作成して返す関数.脚状態は重心パターン, 脚の接地・遊脚,離散化した脚位置のデータが含まれる.
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 を返す.
void GetGroundedLegIndexByVector(const LegStateBit &leg_state, std::vector< int > *res_index)
接地している脚の脚番号0~5を,引数で参照渡しする関数.
void ChangeAllLegGround(const LegGroundedBit &is_ground_list, LegStateBit *leg_state)
全ての脚の接地・遊脚情報を変更する.
std::bitset< kLegStateBitNum > LegStateBit
脚状態を保存する型.28bitのビット型.
DiscreteLegPos
離散化された脚位置を表す列挙体. 先行研究では 1~7の int型の数値で表現されているが, 可読性を上げるために列挙体にした. 離散化された脚位置は 3bit (0 ~ 7)の範囲で表現される...