graph_searcher_straight_move.cpp

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#include "graph_searcher_straight_move.h"
 
#include <functional>
#include <vector>
 
#include "cassert_define.h"
#include "cmdio_util.h"
#include "leg_state.h"
#include "math_util.h"
 
namespace designlab {
 
GraphSearcherStraightMove::GraphSearcherStraightMove(
    const std::shared_ptr<const IHexapodPostureValidator>& checker_ptr)
    : checker_ptr_(checker_ptr), evaluator_(InitializeEvaluator()) {}
 
std::tuple<GraphSearchResult, GraphSearchEvaluationValue, RobotStateNode>
GraphSearcherStraightMove::SearchGraphTree(
    const GaitPatternGraphTree& graph, const RobotOperation& operation,
    const DividedMapState& divided_map_state, const int max_depth) const {
  // ターゲットモードは直進である.
  assert(operation.operation_type ==
             RobotOperationType::kStraightMovePosition ||
         operation.operation_type == RobotOperationType::kStraightMoveVector);
 
  if (!graph.HasRoot()) {
    const GraphSearchResult result = {enums::Result::kFailure,
                                      "ルートノードがありません."};
    return {result, GraphSearchEvaluationValue{}, RobotStateNode{}};
  }
 
  // 初期化.
  Vector3 normalized_move_direction;
 
  if (operation.operation_type == RobotOperationType::kStraightMovePosition) {
    normalized_move_direction =
        (operation.straight_move_position -
         graph.GetRootNode().center_of_mass_global_coord);
    normalized_move_direction.z = 0.0f;
    normalized_move_direction = normalized_move_direction.GetNormalized();
  } else {
    normalized_move_direction = operation.straight_move_vector;
    normalized_move_direction.z = 0.0f;
    normalized_move_direction = normalized_move_direction.GetNormalized();
    normalized_move_direction = normalized_move_direction.GetLength() == 0.0f
                                    ? Vector3::GetFrontVec()
                                    : normalized_move_direction;
  }
 
  const float target_z_value = InitTargetZValue(
      graph.GetRootNode(), divided_map_state, normalized_move_direction);
 
  cmdio::DebugOutputF("target_z_value = {}", target_z_value);
 
  GraphSearchEvaluationValue max_evaluation_value =
      evaluator_.InitializeEvaluationValue();
  int max_evaluation_value_index = -1;
  int log_depth = 0;
  HexapodMove log_move = HexapodMove::kNone;
 
  GraphSearchEvaluationValue candidate_evaluation_value =
      evaluator_.InitializeEvaluationValue();
 
  for (int i = 0; i < graph.GetGraphSize(); i++) {
    log_depth =
        log_depth < graph.GetNode(i).depth ? graph.GetNode(i).depth : log_depth;
    log_move = log_depth == graph.GetNode(i).depth ? graph.GetNode(i).next_move
                                                   : log_move;
 
    // 最大深さのノードのみを評価する.
    if (graph.GetNode(i).depth != max_depth) {
      continue;
    }
 
    // 評価値を計算する.
    candidate_evaluation_value.value.at(kTagMoveForward) =
        GetMoveForwardEvaluationValue(graph.GetNode(i), graph.GetRootNode(),
                                      normalized_move_direction);
    // if (!evaluator_.LeftIsBetterWithTag(candidate_evaluation_value,
    // max_evaluation_value, kTagMoveForward)) { continue; }
 
    candidate_evaluation_value.value.at(kTagLegRot) =
        GetLegRotEvaluationValue(graph.GetNode(i), graph.GetRootNode());
    // if (!evaluator_.LeftIsBetterWithTag(candidate_evaluation_value,
    // max_evaluation_value, kTagLegRot)) { continue; }
    candidate_evaluation_value.value.at(kTagZDiff) = GetZDiffEvaluationValue(
        graph.GetCoMVerticalTrajectory(i), target_z_value);
 
    // candidate_evaluation_value.value.at(kTagZDiff) =
    // GetZDiffEvaluationValue({ graph.GetNode(i).center_of_mass_global_coord.z
    // }, target_z_value);
 
    // 評価値を比較する.
    if (evaluator_.LeftIsBetter(candidate_evaluation_value,
                                max_evaluation_value)) {
      // 上回っている場合は更新する.
      cmdio::OutputF(OutputDetail::kDebug, "max_evaluation_value = {}",
                     max_evaluation_value.value[kTagZDiff]);
      max_evaluation_value = candidate_evaluation_value;
      max_evaluation_value_index = i;
    }
  }
 
  // インデックスが範囲外ならば失敗.
  if (graph.GetGraphSize() <= max_evaluation_value_index) {
    const GraphSearchResult result = {enums::Result::kFailure,
                                      "最大評価値のインデックスが範囲外です."};
    return {result, GraphSearchEvaluationValue{}, RobotStateNode{}};
  }
 
  if (max_evaluation_value_index < 0) {
    const GraphSearchResult result = {
        enums::Result::kFailure,
        std::format("葉ノードが存在しません.最大深さ : {},動作 : {}", log_depth,
                    string_util::EnumToStringRemoveTopK(log_move))};
 
    return {result, GraphSearchEvaluationValue{}, RobotStateNode{}};
  }
 
  const GraphSearchResult result = {enums::Result::kSuccess, ""};
 
  return {result, max_evaluation_value,
          graph.GetParentNode(max_evaluation_value_index, 1)};
}
 
std::tuple<GraphSearchResult, GraphSearchEvaluationValue, RobotStateNode>
GraphSearcherStraightMove::SearchGraphTreeVector(
    const std::vector<GaitPatternGraphTree>& graph_vector,
    const RobotOperation& operation, const DividedMapState& divided_map_state,
    int max_depth) const {
  std::vector<
      std::tuple<GraphSearchResult, GraphSearchEvaluationValue, RobotStateNode>>
      result_vector;
  result_vector.resize(graph_vector.size());
 
  for (size_t i = 0; i < graph_vector.size(); i++) {
    // グラフ探索の結果を格納する.
    result_vector[i] = SearchGraphTree(graph_vector[i], operation,
                                       divided_map_state, max_depth);
  }
 
  // 最大評価値を持つものを探す.
  GraphSearchEvaluationValue max_evaluation_value =
      evaluator_.InitializeEvaluationValue();
  int max_evaluation_value_index = -1;
 
  for (int i = 0; i < result_vector.size(); i++) {
    const auto& [result, evaluation_value, _] = result_vector[i];
 
    // 失敗しているものは無視する.
    if (result.result != enums::Result::kSuccess) {
      continue;
    }
 
    // 評価値を比較する.
    if (evaluator_.LeftIsBetter(evaluation_value, max_evaluation_value)) {
      // 上回っている場合は更新する.
      max_evaluation_value = evaluation_value;
      max_evaluation_value_index = i;
    }
  }
 
  // インデックスが範囲外ならば失敗.
  if (max_evaluation_value_index < 0 ||
      result_vector.size() <= max_evaluation_value_index) {
    const GraphSearchResult result = {enums::Result::kFailure,
                                      "最大評価値のインデックスが範囲外です."};
    return {result, GraphSearchEvaluationValue{}, RobotStateNode{}};
  }
 
  return result_vector[max_evaluation_value_index];
}
 
GraphSearchEvaluator GraphSearcherStraightMove::InitializeEvaluator() const {
  GraphSearchEvaluator::EvaluationMethod move_forward_method = {
      .is_lower_better = false,
      .margin = 0.0f,
  };
 
  GraphSearchEvaluator::EvaluationMethod leg_rot_method = {
      .is_lower_better = false,
      .margin = 0.0f,
  };
 
  GraphSearchEvaluator::EvaluationMethod z_diff_method = {
      .is_lower_better = true,
      .margin = 0.f,
  };
 
  GraphSearchEvaluator ret({{kTagMoveForward, move_forward_method},
                            {kTagLegRot, leg_rot_method},
                            {kTagZDiff, z_diff_method}},
                           {kTagZDiff, kTagMoveForward, kTagLegRot});
 
  return ret;
}
 
float GraphSearcherStraightMove::InitTargetZValue(
    const RobotStateNode& node, const DividedMapState& divided_map_state,
    const Vector3& move_direction) const {
  const float move_length = 100.0f;
 
  const Vector3 target_position = move_direction * move_length;
 
  const int div = 300;
  const float min_z = -150.0f;
  const float max_z = 150.0f;
 
  for (int i = 0; i < div; i++) {
    const float z = min_z + (max_z - min_z) / static_cast<float>(div) *
                                static_cast<float>(i);
 
    Vector3 pos = node.center_of_mass_global_coord;
    pos += target_position;
    pos.z += z;
 
    RobotStateNode temp_node = node;
    temp_node.ChangeGlobalCenterOfMass(pos, false);
 
    if (!checker_ptr_->IsBodyInterferingWithGround(temp_node,
                                                   divided_map_state)) {
      return node.center_of_mass_global_coord.z + z;
    }
  }
 
  return node.center_of_mass_global_coord.z;
}
 
float GraphSearcherStraightMove::GetMoveForwardEvaluationValue(
    const RobotStateNode& node, const RobotStateNode& root_node,
    const Vector3& normalized_move_direction) const {
  // 正規化されていることを確認する.
  assert(math_util::IsEqual(normalized_move_direction.GetSquaredLength(), 1.f));
 
  const Vector3 root_to_current =
      node.center_of_mass_global_coord - root_node.center_of_mass_global_coord;
 
  // root_to_current の normalized_move_direction 方向の成分を取り出す.
  const float result = root_to_current.Dot(normalized_move_direction);
 
  return result;
 
  // const float margin = 7.5f;
}
 
float GraphSearcherStraightMove::GetLegRotEvaluationValue(
    const RobotStateNode& node, const RobotStateNode& root_node) const {
  float result = 0.0f;
 
  for (int i = 0; i < HexapodConst::kLegNum; i++) {
    if (leg_func::IsGrounded(node.leg_state, i)) {
      result +=
          (node.leg_pos[i].ProjectedXY() - root_node.leg_pos[i].ProjectedXY())
              .GetLength();
    } else {
      result += (node.leg_pos[i] - root_node.leg_pos[i]).GetLength();
    }
  }
 
  return result;
 
  // const float margin = 10.0f;
}
 
float GraphSearcherStraightMove::GetZDiffEvaluationValue(
    const std::vector<float>& com_trajectory,
    const float target_z_value) const {
  float result = abs(com_trajectory.back() - target_z_value);
 
  if (com_trajectory.size() == 3) {
    if ((com_trajectory[0] - com_trajectory[1]) *
            (com_trajectory[0] - com_trajectory[2]) <=
        0) {
      result += abs(com_trajectory[0] - com_trajectory[1]);
    }
  }
 
  return result;
}
 
}  // namespace designlab