GaitGeneration by Graph Search
読み取り中…
検索中…
一致する文字列を見つけられません
node_creator_leg_up_down_2d.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
9
10#include <algorithm>
11#include <boost/dynamic_bitset.hpp>
12
13#include "com_type.h"
14#include "leg_state.h"
15#include "math_util.h"
16
17namespace designlab {
18
20 const DividedMapState& divided_map,
21 const std::shared_ptr<const IHexapodCoordinateConverter>& converter_ptr,
22 const std::shared_ptr<const IHexapodStatePresenter>& presenter_ptr,
23 const std::shared_ptr<const IHexapodPostureValidator>& checker_ptr,
24 HexapodMove next_move)
25 : kLegMargin(20),
26 map_(divided_map),
27 converter_ptr_(converter_ptr),
28 presenter_ptr_(presenter_ptr),
29 checker_ptr_(checker_ptr),
30 next_move_(next_move) {
31 assert(converter_ptr_ != nullptr);
32 assert(presenter_ptr_ != nullptr);
33 assert(checker_ptr_ != nullptr);
34};
35
37 const RobotStateNode& current_node, int current_node_index,
38 std::vector<RobotStateNode>* output_graph) const {
39 // 脚の遊脚・接地によって生じるとりうる重心を com type として仕分けている.
40 // (詳しくは com_type.h を参照).
41 // vector<bool> を使用したいが,vector<bool> はテンプレートの特殊化で
42 // 通常の vector とは違う挙動をするので,boost::dynamic_bitset<> を使用する.
43 boost::dynamic_bitset<> is_able_leg_ground_pattern(
45
46 is_able_leg_ground_pattern.set(); // 全て true にする.
47
48 // まず離散化された重心位置から取り得ない接地パターンを除外する.
51 &is_able_leg_ground_pattern);
52
53 // 次に脚が地面に接地可能か調べる.
54
55 // 脚が設置可能ならば true になる.既に接地しているならば true になる.
56 bool is_groundable_leg[HexapodConst::kLegNum];
57 Vector3 ground_pos[HexapodConst::kLegNum]; // 脚が接地する座標.
58
59 for (int i = 0; i < HexapodConst::kLegNum; i++) {
60 ground_pos[i] = current_node.leg_pos[i];
61 }
62
63 for (int i = 0; i < HexapodConst::kLegNum; i++) {
64 if (leg_func::IsGrounded(current_node.leg_state, i)) {
65 // すでに接地している脚は接地可能に決まっているので true にする.
66 is_groundable_leg[i] = true;
67 ground_pos[i] = current_node.leg_pos[i];
68 } else {
69 // 現在遊脚中の脚は自身の脚状態で接地できるか検討する.
70 Vector3 res_ground_pos;
71
72 if (IsGroundableLeg(i, current_node, &res_ground_pos)) {
73 is_groundable_leg[i] = true; // 接地可能にする.
74 ground_pos[i] = res_ground_pos;
75 } else {
76 is_groundable_leg[i] = false; // 接地不可能にする.
78 i, &is_able_leg_ground_pattern);
79 }
80 }
81 }
82
83 // 子ノードを生成する.
84 for (int i = 0; i < com_func::GetLegGroundPatternNum(); i++) {
85 // その重心タイプが可能であれば,追加する.
86 if (is_able_leg_ground_pattern[i]) {
87 RobotStateNode res_node = current_node;
88
89 res_node.ChangeToNextNode(current_node_index, next_move_);
90
91 // 遊脚・接地を書き換える.
92 leg_func::LegGroundedBit new_is_ground =
94
95 leg_func::ChangeAllLegGround(new_is_ground, &res_node.leg_state);
96
97 // 脚位置を書き換える.
98 for (int j = 0; j < HexapodConst::kLegNum; j++) {
99 if (new_is_ground[j]) {
100 res_node.leg_pos[j] = ground_pos[j];
101
102 res_node.leg_reference_pos[j] = ground_pos[j];
103 } else {
104 res_node.leg_pos[j] = presenter_ptr_->GetFreeLegPosLegCoordinate(j);
105
106 res_node.leg_reference_pos[j].x = res_node.leg_pos[j].x;
107 res_node.leg_reference_pos[j].y = res_node.leg_pos[j].y;
108 }
109 }
110
111 if (checker_ptr_->IsStable(res_node.leg_state, res_node.leg_pos)) {
112 (*output_graph).push_back(res_node);
113 }
114 }
115 } // for i
116}
117
118bool NodeCreatorLegUpDown2d::IsGroundableLeg(int now_leg_num,
119 const RobotStateNode& current_node,
120 Vector3* output_ground_pos) const {
121 // for 文の中の continue については以下を参照.
122 // http://www9.plala.or.jp/sgwr-t/c/sec06-7.html (アクセス日 2023/12/27)
123
124 // 脚座標がマップのどこに当たるか調べて,そのマスの2つ上と2つ下の範囲内を全て探索する.
125 const Vector3 global_leg_base_pos =
126 converter_ptr_->ConvertLegToGlobalCoordinate(
127 current_node.leg_reference_pos[now_leg_num], now_leg_num,
128 current_node.center_of_mass_global_coord, current_node.posture, true);
129
130 int max_x_dev = map_.GetDividedMapIndexX(global_leg_base_pos.x) + 2;
131 int min_x_dev = map_.GetDividedMapIndexX(global_leg_base_pos.x) - 2;
132 int max_y_dev = map_.GetDividedMapIndexY(global_leg_base_pos.y) + 2;
133 int min_y_dev = map_.GetDividedMapIndexY(global_leg_base_pos.y) - 2;
134
135 // 値がマップの範囲外にあるときは丸める.
136 max_x_dev = DividedMapState::ClampDividedMapIndex(max_x_dev);
137 min_x_dev = DividedMapState::ClampDividedMapIndex(min_x_dev);
138 max_y_dev = DividedMapState::ClampDividedMapIndex(max_y_dev);
139 min_y_dev = DividedMapState::ClampDividedMapIndex(min_y_dev);
140
141 // マップ内を全探索して,現在の脚位置(離散化した物)に適した脚設置可能点が,存在するか調べる.
142 Vector3 candidate_pos; // 現在の脚位置に合致する候補座標群.
143 bool is_candidate_pos = false; // 候補座標が存在するかどうか.
144
145 // 範囲内の点を全て調べる.
146 for (int x = min_x_dev; x < max_x_dev; x++) {
147 for (int y = min_y_dev; y < max_y_dev; y++) {
148 const auto pos_num = map_.GetPointNum(x, y);
149 if (!pos_num) {
150 continue; // このマスに脚設置可能点が存在しない場合はスキップ.
151 }
152
153 for (int n = 0; n < *pos_num; n++) {
154 // 脚設置可能点の座標を取り出す.
155 const auto map_point_pos_exp = map_.GetPointPos(x, y, n);
156 if (!map_point_pos_exp) {
157 continue; // このマスに脚設置可能点が存在しない場合はスキップ.
158 }
159
160 const Vector3 map_point_pos =
161 converter_ptr_->ConvertGlobalToLegCoordinate(
162 *map_point_pos_exp, now_leg_num,
163 current_node.center_of_mass_global_coord, current_node.posture,
164 true);
165
166 // 脚位置を更新したノードを作成する.
167 RobotStateNode new_node = current_node;
168
169 new_node.leg_pos[now_leg_num] = map_point_pos;
170
171 // 前の候補地点と比較して,より良い候補地点の時のみ実行すする
172 if (is_candidate_pos) {
173 // 反対方向をむいている場合は候補地点として採用しない.
174 if (new_node.leg_reference_pos[now_leg_num].ProjectedXY().Cross(
175 candidate_pos.ProjectedXY()) *
176 new_node.leg_reference_pos[now_leg_num].ProjectedXY().Cross(
177 map_point_pos.ProjectedXY()) <
178 0) {
179 continue;
180 }
181
182 // 現在の脚位置と候補地点の間に障害物がある場合は候補地点として採用しない.
183 if (map_point_pos.ProjectedXY().Cross(candidate_pos.ProjectedXY()) *
184 map_point_pos.ProjectedXY().Cross(
185 new_node.leg_reference_pos[now_leg_num].ProjectedXY()) <
186 0) {
187 continue;
188 }
189 }
190
191 leg_func::ChangeGround(now_leg_num, true, &new_node.leg_state);
192
193 if (!checker_ptr_->IsLegInRange(now_leg_num,
194 new_node.leg_pos[now_leg_num])) {
195 // 脚が範囲外ならば追加せずに続行.
196 continue;
197 }
198
199 if (!IsAbleLegPos(new_node, now_leg_num)) {
200 // 候補座標として,適していないならば追加せずに続行.
201 continue;
202 }
203
204 is_candidate_pos = true;
205 candidate_pos = map_point_pos;
206 }
207 } // for y
208 } // for x
209
210 // 候補点を全列挙したのち,候補点が一つもなければ falseを返す.
211 if (!is_candidate_pos) {
212 return false;
213 }
214
215 // 存在するなら,その中で最も適したものを結果として返し,trueを返す.
216 (*output_ground_pos) = candidate_pos;
217
218 return true;
219}
220
221bool NodeCreatorLegUpDown2d::IsAbleLegPos(const RobotStateNode& node,
222 const int leg_index) const {
223 const DiscreteLegPos discrete_leg_pos = ConvertTo2D(
224 leg_func::GetDiscreteLegPos(node.leg_state, leg_index)); // 脚位置を取得.
225
226 // まず最初に脚位置4のところにないか確かめる.
227 if ((node.leg_reference_pos[leg_index].ProjectedXY() -
228 node.leg_pos[leg_index].ProjectedXY())
229 .GetSquaredLength() < math_util::Squared(kLegMargin)) {
230 if (discrete_leg_pos == DiscreteLegPos::kCenter) {
231 return true;
232 } else {
233 return false;
234 }
235 } else {
236 if (discrete_leg_pos == DiscreteLegPos::kCenter) {
237 return false;
238 }
239 }
240
241 // 脚位置4と比較して前か後ろか.
242 if (node.leg_reference_pos[leg_index].ProjectedXY().Cross(
243 node.leg_pos[leg_index].ProjectedXY()) *
244 node.leg_pos[leg_index].ProjectedXY().Cross({1, 0}) >
245 0) {
246 // 前.
247 if (discrete_leg_pos == DiscreteLegPos::kBack) {
248 return false;
249 }
250 } else {
251 // 後ろ.
252 if (discrete_leg_pos == DiscreteLegPos::kFront) {
253 return false;
254 }
255 }
256
257 return true;
258}
259
260DiscreteLegPos NodeCreatorLegUpDown2d::ConvertTo2D(
261 DiscreteLegPos leg_pos) const {
262 switch (leg_pos) {
266 // 2Dの場合はそのまま返す.
267 return leg_pos;
268 }
269
272 // 3Dの場合は2Dに変換する.
274 }
275
278 // 同様に,3Dの場合は2Dに変換する.
280 }
281
282 default: {
283 // ここに来た場合はアサートを発生させる.
284 assert(false);
286 }
287 }
288}
289
290} // namespace designlab
マップを格子状に分割して管理するクラス.
constexpr int GetDividedMapIndexX(const float pos_x) const noexcept
指定した座標を DividedMap のインデックスに変換する. 範囲外の値を指定した場合でも,値を丸めずに返す. そのため,IsInMap で範囲内に存在するかどうかを確認する必要がある.
nostd::expected< Vector3, std::string > GetPointPos(int x_index, int y_index, int divided_map_index) const
格子状に切り分けられたマップから,脚設置可能点の実際の座標を取得する. 範囲外の値を指定した場合は, unexpected を返す.
constexpr int GetDividedMapIndexY(const float pos_y) const noexcept
指定した座標を DividedMap のインデックスに変換する. 範囲外の値を指定した場合でも,値を丸めずに返す. そのため,IsInMap で範囲内に存在するかどうかを確認する必要がある.
nostd::expected< int, std::string > GetPointNum(int x_index, int y_index) const
格子状に切り分けられたマップから,脚設置可能点の数を取得する. 範囲外の値を指定した場合は, unexpected を返す.
static constexpr int ClampDividedMapIndex(const int index) noexcept
指定した座標がマップのインデックスの範囲内になるように丸める.
static constexpr int kLegNum
void Create(const RobotStateNode &current_node, int current_node_index, std::vector< RobotStateNode > *output_graph) const override
現在のノードから次のノード群を生成する.
NodeCreatorLegUpDown2d(const DividedMapState &devide_map, const std::shared_ptr< const IHexapodCoordinateConverter > &converter_ptr, const std::shared_ptr< const IHexapodStatePresenter > &presenter_ptr, const std::shared_ptr< const IHexapodPostureValidator > &checker_ptr, HexapodMove next_move)
void RemoveLegGroundPatternFromCom(enums::DiscreteComPos discrete_com_pos, boost::dynamic_bitset<> *output)
離散化された重心位置から,その重心位置では取り得ない脚接地パターンを falseにする.
Definition com_type.cpp:216
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
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)
脚状態を取得する.
enums::DiscreteComPos GetDiscreteComPos(const LegStateBit &leg_state)
現在の脚状態から重心パターンを取得する.
void ChangeGround(const int leg_index, const bool is_ground, LegStateBit *leg_state)
脚の接地・遊脚情報を変更する.
void ChangeAllLegGround(const LegGroundedBit &is_ground_list, LegStateBit *leg_state)
全ての脚の接地・遊脚情報を変更する.
constexpr T Squared(const T num) noexcept
2乗した値を返す関数.
Definition math_util.h:55
DiscreteLegPos
離散化された脚位置を表す列挙体. 先行研究では 1~7の int型の数値で表現されているが, 可読性を上げるために列挙体にした. 離散化された脚位置は 3bit (0 ~ 7)の範囲で表現される...
@ kUpperBack
現在の位置より後方かつ上方にある.
@ kBack
現在の位置より後方にある.
@ kCenter
現在の位置にある.
@ kUpperFront
現在の位置より前方かつ上方にある.
@ kFront
現在の位置より前方にある.
@ kLowerBack
現在の位置より後方かつ下方にある.
@ kLowerFront
現在の位置より前方かつ下方にある.
HexapodMove
ロボットが次にどの動作をするのかを表す列挙体.
グラフ構造のためのノード(頂点).
std::array< Vector3, HexapodConst::kLegNum > leg_pos
[4 * 3 * 6 = 72 byte] 脚先の座標.(coxa(脚の付け根)を原点とする)
leg_func::LegStateBit leg_state
[4 byte] 脚状態,重心パターンを bitで表す.旧名 leg_con.
constexpr void ChangeToNextNode(const int parent_index_, const HexapodMove next_move_)
次の動作を設定する関数. 深さを一つ深くして,親と次の動作を設定する.
Vector3 center_of_mass_global_coord
[4 * 3 = 12byte] グローバル座標系における重心の位置.旧名 GCOM
Quaternion posture
[4 * 4 = 16byte] 姿勢を表すクォータニオン.
std::array< Vector3, HexapodConst::kLegNum > leg_reference_pos
3次元の位置ベクトルを表す構造体.
float x
ロボットの正面方向に正.
constexpr Vector2 ProjectedXY() const noexcept
XY平面に射影したベクトルを返す.
float y
ロボットの左向きに正.