10#include <magic_enum.hpp>
95 return std::move(res);
100const LegGroundedMap kLegGroundedPatternMap = MakeLegGroundedMap();
103const int kLegGroundedPatternNum =
104 static_cast<int>(kLegGroundedPatternMap.size());
111bool IsLegPairFree(
int leg_index,
int leg_ground_pattern_index) {
117 ::kLegGroundedPatternMap.right.at(leg_ground_pattern_index);
123 if (!leg_ground_pattern[leg_index % HexapodConst::kLegNum] &&
124 !leg_ground_pattern[(leg_index + 1) % HexapodConst::kLegNum]) {
133std::unordered_map<DiscreteComPos, std::vector<int>>
134MakeLegGroundedPatternBanList() {
135 std::unordered_map<DiscreteComPos, std::vector<int>> res;
140 std::unordered_map<DiscreteComPos, std::vector<int>> ban_leg_index_list;
141 ban_leg_index_list[DiscreteComPos::kFront] = {0, 4, 5};
142 ban_leg_index_list[DiscreteComPos::kFrontRight] = {0, 1, 5};
143 ban_leg_index_list[DiscreteComPos::kFrontLeft] = {3, 4, 5};
144 ban_leg_index_list[DiscreteComPos::kBack] = {1, 2, 3};
145 ban_leg_index_list[DiscreteComPos::kBackRight] = {0, 1, 2};
146 ban_leg_index_list[DiscreteComPos::kBackLeft] = {2, 3, 4};
147 ban_leg_index_list[DiscreteComPos::kCenterBack] = {0, 2, 4};
148 ban_leg_index_list[DiscreteComPos::kCenterFront] = {1, 3, 5};
154 if (ban_leg_index_list.count(i) == 0) {
158 for (
auto& j : ban_leg_index_list[i]) {
159 for (
int k = 0; k < ::kLegGroundedPatternNum; ++k) {
160 if (IsLegPairFree(j, k)) {
167 return std::move(res);
172std::vector<std::vector<int>> MakeLegGroundedPatternBanListFromLeg() {
173 std::vector<std::vector<int>> res;
180 for (
int j = 0; j < ::kLegGroundedPatternNum; ++j) {
183 if (::kLegGroundedPatternMap.right.at(j)[i]) {
189 return std::move(res);
193const std::unordered_map<::designlab::enums::DiscreteComPos, std::vector<int>>
194 kLegGroundedPatternBanList = MakeLegGroundedPatternBanList();
197const std::vector<std::vector<int>> kLegGroundedPatternBanListFromLeg =
198 MakeLegGroundedPatternBanListFromLeg();
207 const int leg_ground_pattern_index) {
211 res = ::kLegGroundedPatternMap.right.at(leg_ground_pattern_index);
217 boost::dynamic_bitset<>* output) {
218 assert(output !=
nullptr);
224 for (
auto& i : kLegGroundedPatternBanList.at(discrete_com_pos)) {
225 (*output)[i] =
false;
230 int not_groundable_leg_index, boost::dynamic_bitset<>* output) {
231 assert(output !=
nullptr);
238 for (
auto& i : kLegGroundedPatternBanListFromLeg[not_groundable_leg_index]) {
239 (*output)[i] =
false;
244 boost::dynamic_bitset<>* output) {
245 assert(output !=
nullptr);
253 for (
auto& i : kLegGroundedPatternBanListFromLeg[not_lift_leg_index]) {
254 inverse_output[i] =
true;
257 (*output) &= inverse_output;
static constexpr int kLegNum
重心タイプに関する名前空間.Center of Mass Function の略.
void RemoveLegGroundPatternFromCom(enums::DiscreteComPos discrete_com_pos, boost::dynamic_bitset<> *output)
離散化された重心位置から,その重心位置では取り得ない脚接地パターンを falseにする.
LegGroundedMap::value_type LegGroundedMapValue
脚の接地パターンを表すマップの値の型.
int GetLegGroundPatternNum()
脚の接地パターンの総数を返す.
leg_func::LegGroundedBit GetLegGroundedBitFromLegGroundPatternIndex(const int leg_ground_pattern_index)
脚の接地パターンの番号から,その番号に該当する接地パターンを返す.
void RemoveLegGroundPatternFromNotGroundableLeg(int not_groundable_leg_index, boost::dynamic_bitset<> *output)
接地できない脚番号から, その脚が接地できない場合に取り得ない接地パターンを falseにする.
boost::bimaps::bimap< leg_func::LegGroundedBit, int > LegGroundedMap
脚の接地パターンを表す型.leftがビットのデータ,rightが int型の番号. bimaps::bimap は,左右の型の両方からアクセスできる map. key→value,...
void RemoveLegGroundPatternFromNotFreeLeg(int not_lift_leg_index, boost::dynamic_bitset<> *output)
遊脚できない脚番号から, その脚が遊脚できない場合に取り得ない接地パターンを falseにする.
std::bitset< HexapodConst::kLegNum > LegGroundedBit
脚の遊脚・接地を表す型.6bitのビット型.接地が 1 遊脚が 0.