GaitGeneration by Graph Search
読み取り中…
検索中…
一致する文字列を見つけられません
com_type.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 "com_type.h"
9
10#include <magic_enum.hpp>
11#include <utility>
12
13#include "cassert_define.h"
14
15// com_func 内の関数は実際に処理を行う際に計算を行うと遅くなるため,
16// 初期化時に一度だけ呼び出して,結果を保存しておく.
17// その値を呼び出すことで速度を上げている.
18// このように無名名前空間の中に変数を宣言することで,
19// このファイル内でのみ使用可能になる.こうしてできた変数に結果を保存する.
20// アクセスするには,先頭に::をつける.
22namespace {
28
31LegGroundedMap MakeLegGroundedMap() {
33 int counter = 0;
34
35 // 脚が接地している場合1,遊脚の場合0として,6bitの数値を作成する.
36 // 0番脚が遊脚,残り接地の場合 111 110 .
37 // そしてそれに 0 から始まる番号を割り振る.(管理しやすくするため.)
38 // 全パターンを総当りで書いてあるけど,本当はこのリストを作成する関数を作りたい.
39
40 // トライポット歩容に使用するパターン.
41 res.insert(LegGroundedMapValue(LegGroundedBit("010101"), counter++));
42 res.insert(LegGroundedMapValue(LegGroundedBit("101010"), counter++));
43
44 // 6脚全て接地している場合.
45 res.insert(LegGroundedMapValue(LegGroundedBit("111111"), counter++));
46
47 // 5脚接地している場合.
48
49 res.insert(LegGroundedMapValue(LegGroundedBit("011111"), counter++));
50 res.insert(LegGroundedMapValue(LegGroundedBit("101111"), counter++));
51 res.insert(LegGroundedMapValue(LegGroundedBit("110111"), counter++));
52 res.insert(LegGroundedMapValue(LegGroundedBit("111011"), counter++));
53 res.insert(LegGroundedMapValue(LegGroundedBit("111101"), counter++));
54 res.insert(LegGroundedMapValue(LegGroundedBit("111110"), counter++));
55
56 // 4脚接地している場合
57
58 res.insert(LegGroundedMapValue(LegGroundedBit("001111"), counter++));
59 res.insert(LegGroundedMapValue(LegGroundedBit("010111"), counter++));
60 res.insert(LegGroundedMapValue(LegGroundedBit("011011"), counter++));
61 res.insert(LegGroundedMapValue(LegGroundedBit("011101"), counter++));
62 res.insert(LegGroundedMapValue(LegGroundedBit("011110"), counter++));
63 res.insert(LegGroundedMapValue(LegGroundedBit("100111"), counter++));
64 res.insert(LegGroundedMapValue(LegGroundedBit("101011"), counter++));
65 res.insert(LegGroundedMapValue(LegGroundedBit("101101"), counter++));
66 res.insert(LegGroundedMapValue(LegGroundedBit("101110"), counter++));
67 res.insert(LegGroundedMapValue(LegGroundedBit("110011"), counter++));
68 res.insert(LegGroundedMapValue(LegGroundedBit("110101"), counter++));
69 res.insert(LegGroundedMapValue(LegGroundedBit("110110"), counter++));
70 res.insert(LegGroundedMapValue(LegGroundedBit("111001"), counter++));
71 res.insert(LegGroundedMapValue(LegGroundedBit("111010"), counter++));
72 res.insert(LegGroundedMapValue(LegGroundedBit("111100"), counter++));
73
74 // 3脚接地している場合.隣り合う3脚が遊脚している場合は除く(転倒してしまうため).
75
76 // res.insert(LegGroundedMapValue(LegGroundedBit("000111"), counter++));
77 res.insert(LegGroundedMapValue(LegGroundedBit("001011"), counter++));
78 res.insert(LegGroundedMapValue(LegGroundedBit("001101"), counter++));
79 // res.insert(LegGroundedMapValue(LegGroundedBit("001110"), counter++));
80 res.insert(LegGroundedMapValue(LegGroundedBit("010011"), counter++));
81 res.insert(LegGroundedMapValue(LegGroundedBit("010110"), counter++));
82 res.insert(LegGroundedMapValue(LegGroundedBit("011001"), counter++));
83 res.insert(LegGroundedMapValue(LegGroundedBit("011010"), counter++));
84 // res.insert(LegGroundedMapValue(LegGroundedBit("011100"), counter++));
85 // res.insert(LegGroundedMapValue(LegGroundedBit("100011"), counter++));
86 res.insert(LegGroundedMapValue(LegGroundedBit("100101"), counter++));
87 res.insert(LegGroundedMapValue(LegGroundedBit("100110"), counter++));
88 res.insert(LegGroundedMapValue(LegGroundedBit("101001"), counter++));
89 res.insert(LegGroundedMapValue(LegGroundedBit("101100"), counter++));
90 // res.insert(LegGroundedMapValue(LegGroundedBit("110001"), counter++));
91 res.insert(LegGroundedMapValue(LegGroundedBit("110010"), counter++));
92 res.insert(LegGroundedMapValue(LegGroundedBit("110100"), counter++));
93 // res.insert(LegGroundedMapValue(LegGroundedBit("111000"), counter++));
94
95 return std::move(res);
96}
97
100const LegGroundedMap kLegGroundedPatternMap = MakeLegGroundedMap();
101
103const int kLegGroundedPatternNum =
104 static_cast<int>(kLegGroundedPatternMap.size());
105
111bool IsLegPairFree(int leg_index, int leg_ground_pattern_index) {
112 LegGroundedBit leg_ground_pattern;
113
114 // indexから遊脚のパターンを取得する.
115 try {
116 leg_ground_pattern =
117 ::kLegGroundedPatternMap.right.at(leg_ground_pattern_index);
118 } catch (...) {
119 return false;
120 }
121
122 // 両隣が遊脚の場合は true を返す.
123 if (!leg_ground_pattern[leg_index % HexapodConst::kLegNum] &&
124 !leg_ground_pattern[(leg_index + 1) % HexapodConst::kLegNum]) {
125 return true;
126 } else {
127 return false;
128 }
129}
130
133std::unordered_map<DiscreteComPos, std::vector<int>>
134MakeLegGroundedPatternBanList() {
135 std::unordered_map<DiscreteComPos, std::vector<int>> res;
136
137 // ロボットの体が前に寄っている時に前足が両方とも遊脚だと転倒してしまう.
138 // そのため,離散化された重心から,とることができない,
139 // 連続する脚が両方とも遊脚になるパターンを禁止するのがこの関数の目的である.
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};
149
150 // DiscreteComPosの要素数だけループする.
151 // magic_enum::enum_values<DiscreteComPos>() は,
152 // DiscreteComPos の要素を列挙した array を返す.
153 for (const auto& i : magic_enum::enum_values<DiscreteComPos>()) {
154 if (ban_leg_index_list.count(i) == 0) {
155 continue;
156 }
157
158 for (auto& j : ban_leg_index_list[i]) {
159 for (int k = 0; k < ::kLegGroundedPatternNum; ++k) {
160 if (IsLegPairFree(j, k)) {
161 res[i].push_back(k);
162 }
163 }
164 }
165 }
166
167 return std::move(res);
168}
169
172std::vector<std::vector<int>> MakeLegGroundedPatternBanListFromLeg() {
173 std::vector<std::vector<int>> res;
174
175 res.resize(
176 designlab::HexapodConst::kLegNum); // 脚の数だけ vector を確保する.
177
178 // i 番脚を接地しなければ,取ることができないものを保存する.
179 for (int i = 0; i < designlab::HexapodConst::kLegNum; i++) {
180 for (int j = 0; j < ::kLegGroundedPatternNum; ++j) {
181 // i番目のビットを確認し,立っているならば
182 // (つまり,その脚を接地しなければいけないなら),そのパターンを禁止する.
183 if (::kLegGroundedPatternMap.right.at(j)[i]) {
184 res[i].push_back(j);
185 }
186 }
187 }
188
189 return std::move(res);
190}
191
193const std::unordered_map<::designlab::enums::DiscreteComPos, std::vector<int>>
194 kLegGroundedPatternBanList = MakeLegGroundedPatternBanList();
195
197const std::vector<std::vector<int>> kLegGroundedPatternBanListFromLeg =
198 MakeLegGroundedPatternBanListFromLeg();
199
200} // namespace
201
203
204int GetLegGroundPatternNum() { return kLegGroundedPatternNum; }
205
207 const int leg_ground_pattern_index) {
209
210 // indexから遊脚のパターンを取得する.
211 res = ::kLegGroundedPatternMap.right.at(leg_ground_pattern_index);
212
213 return res;
214}
215
217 boost::dynamic_bitset<>* output) {
218 assert(output != nullptr);
219 assert((*output).size() == GetLegGroundPatternNum());
220
221 // kLegGroundedPatternBanListFromLeg にキーが存在していないことや,
222 // 値が GetLegGroundPatternNum() を超えてないことを確認していない.
223 // エラーが出たら,そこが原因の可能性がある
224 for (auto& i : kLegGroundedPatternBanList.at(discrete_com_pos)) {
225 (*output)[i] = false;
226 }
227}
228
230 int not_groundable_leg_index, boost::dynamic_bitset<>* output) {
231 assert(output != nullptr);
232 assert((*output).size() == GetLegGroundPatternNum());
233
234 // kLegGroundedPatternBanListFromLeg にキーが存在していないことや,
235 // 値が GetLegGroundPatternNum() を超えてないことを確認していない.
236 // エラーが出たら,そこが原因の可能性がある.
237
238 for (auto& i : kLegGroundedPatternBanListFromLeg[not_groundable_leg_index]) {
239 (*output)[i] = false;
240 }
241}
242
243void RemoveLegGroundPatternFromNotFreeLeg(int not_lift_leg_index,
244 boost::dynamic_bitset<>* output) {
245 assert(output != nullptr);
246 assert((*output).size() == GetLegGroundPatternNum());
247
248 // kLegGroundedPatternBanListFromLeg にキーが存在していないことや,
249 // 値が GetLegGroundPatternNum() を超えてないことを確認していない.
250 // エラーが出たら,そこが原因の可能性がある.
251 boost::dynamic_bitset<> inverse_output(GetLegGroundPatternNum());
252
253 for (auto& i : kLegGroundedPatternBanListFromLeg[not_lift_leg_index]) {
254 inverse_output[i] = true;
255 }
256
257 (*output) &= inverse_output;
258}
259
260} // namespace designlab::com_func
Hexapodの定数をまとめたクラス.
static constexpr int kLegNum
重心タイプに関する名前空間.Center of Mass Function の略.
Definition com_type.cpp:202
void RemoveLegGroundPatternFromCom(enums::DiscreteComPos discrete_com_pos, boost::dynamic_bitset<> *output)
離散化された重心位置から,その重心位置では取り得ない脚接地パターンを falseにする.
Definition com_type.cpp:216
LegGroundedMap::value_type LegGroundedMapValue
脚の接地パターンを表すマップの値の型.
Definition com_type.h:59
int GetLegGroundPatternNum()
脚の接地パターンの総数を返す.
Definition com_type.cpp:204
leg_func::LegGroundedBit GetLegGroundedBitFromLegGroundPatternIndex(const int leg_ground_pattern_index)
脚の接地パターンの番号から,その番号に該当する接地パターンを返す.
Definition com_type.cpp:206
void RemoveLegGroundPatternFromNotGroundableLeg(int not_groundable_leg_index, boost::dynamic_bitset<> *output)
接地できない脚番号から, その脚が接地できない場合に取り得ない接地パターンを falseにする.
Definition com_type.cpp:229
boost::bimaps::bimap< leg_func::LegGroundedBit, int > LegGroundedMap
脚の接地パターンを表す型.leftがビットのデータ,rightが int型の番号. bimaps::bimap は,左右の型の両方からアクセスできる map. key→value,...
Definition com_type.h:56
void RemoveLegGroundPatternFromNotFreeLeg(int not_lift_leg_index, boost::dynamic_bitset<> *output)
遊脚できない脚番号から, その脚が遊脚できない場合に取り得ない接地パターンを falseにする.
Definition com_type.cpp:243
std::bitset< HexapodConst::kLegNum > LegGroundedBit
脚の遊脚・接地を表す型.6bitのビット型.接地が 1 遊脚が 0.
Definition leg_state.h:58