11#include <boost/dynamic_bitset.hpp>
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,
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);
38 std::vector<RobotStateNode>* output_graph)
const {
43 boost::dynamic_bitset<> is_able_leg_ground_pattern(
46 is_able_leg_ground_pattern.set();
51 &is_able_leg_ground_pattern);
60 ground_pos[i] = current_node.
leg_pos[i];
66 is_groundable_leg[i] =
true;
67 ground_pos[i] = current_node.
leg_pos[i];
72 if (IsGroundableLeg(i, current_node, &res_ground_pos)) {
73 is_groundable_leg[i] =
true;
74 ground_pos[i] = res_ground_pos;
76 is_groundable_leg[i] =
false;
78 i, &is_able_leg_ground_pattern);
86 if (is_able_leg_ground_pattern[i]) {
99 if (new_is_ground[j]) {
100 res_node.
leg_pos[j] = ground_pos[j];
104 res_node.
leg_pos[j] = presenter_ptr_->GetFreeLegPosLegCoordinate(j);
112 (*output_graph).push_back(res_node);
118bool NodeCreatorLegUpDown2d::IsGroundableLeg(
int now_leg_num,
120 Vector3* output_ground_pos)
const {
125 const Vector3 global_leg_base_pos =
126 converter_ptr_->ConvertLegToGlobalCoordinate(
143 bool is_candidate_pos =
false;
146 for (
int x = min_x_dev; x < max_x_dev; x++) {
147 for (
int y = min_y_dev; y < max_y_dev; y++) {
153 for (
int n = 0; n < *pos_num; n++) {
155 const auto map_point_pos_exp = map_.
GetPointPos(x, y, n);
156 if (!map_point_pos_exp) {
160 const Vector3 map_point_pos =
161 converter_ptr_->ConvertGlobalToLegCoordinate(
162 *map_point_pos_exp, now_leg_num,
167 RobotStateNode new_node = current_node;
169 new_node.
leg_pos[now_leg_num] = map_point_pos;
172 if (is_candidate_pos) {
174 if (new_node.leg_reference_pos[now_leg_num].ProjectedXY().Cross(
176 new_node.leg_reference_pos[now_leg_num].ProjectedXY().Cross(
177 map_point_pos.ProjectedXY()) <
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()) <
193 if (!checker_ptr_->IsLegInRange(now_leg_num,
194 new_node.leg_pos[now_leg_num])) {
199 if (!IsAbleLegPos(new_node, now_leg_num)) {
204 is_candidate_pos =
true;
205 candidate_pos = map_point_pos;
211 if (!is_candidate_pos) {
216 (*output_ground_pos) = candidate_pos;
221bool NodeCreatorLegUpDown2d::IsAbleLegPos(
const RobotStateNode& node,
222 const int leg_index)
const {
227 if ((node.leg_reference_pos[leg_index].ProjectedXY() -
228 node.leg_pos[leg_index].ProjectedXY())
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}) >
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 ¤t_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にする.
int GetLegGroundPatternNum()
脚の接地パターンの総数を返す.
leg_func::LegGroundedBit GetLegGroundedBitFromLegGroundPatternIndex(const int leg_ground_pattern_index)
脚の接地パターンの番号から,その番号に該当する接地パターンを返す.
void RemoveLegGroundPatternFromNotGroundableLeg(int not_groundable_leg_index, boost::dynamic_bitset<> *output)
接地できない脚番号から, その脚が接地できない場合に取り得ない接地パターンを falseにする.
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)
脚状態を取得する.
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乗した値を返す関数.
DiscreteLegPos
離散化された脚位置を表す列挙体. 先行研究では 1~7の int型の数値で表現されているが, 可読性を上げるために列挙体にした. 離散化された脚位置は 3bit (0 ~ 7)の範囲で表現される...
@ kUpperBack
現在の位置より後方かつ上方にある.
@ kUpperFront
現在の位置より前方かつ上方にある.
@ 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
constexpr Vector2 ProjectedXY() const noexcept
XY平面に射影したベクトルを返す.