graph_searcher_spot_turn.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_spot_turn.h"
 
#include <vector>
 
#include "cassert_define.h"
 
namespace designlab {
 
GraphSearcherSpotTurn::GraphSearcherSpotTurn(
    const std::shared_ptr<const IHexapodCoordinateConverter>& converter_ptr,
    const std::shared_ptr<const IHexapodPostureValidator>& checker_ptr)
    : converter_ptr_(converter_ptr),
      checker_ptr_(checker_ptr),
      evaluator_(InitializeEvaluator()) {}
 
std::tuple<GraphSearchResult, GraphSearchEvaluationValue, RobotStateNode>
GraphSearcherSpotTurn::SearchGraphTree(const GaitPatternGraphTree& graph,
                                       const RobotOperation& operation,
                                       const DividedMapState& divided_map_state,
                                       const int max_depth) const {
  assert(operation.operation_type == RobotOperationType::kSpotTurnLastPosture ||
         operation.operation_type == RobotOperationType::kSpotTurnRotAxis);
 
  if (!graph.HasRoot()) {
    const GraphSearchResult result = {enums::Result::kFailure,
                                      "ルートノードがありません."};
    return {result, GraphSearchEvaluationValue{}, RobotStateNode{}};
  }
 
  // 初期化.
  Quaternion target_quaternion;
 
  if (operation.operation_type == RobotOperationType::kSpotTurnLastPosture) {
    target_quaternion = operation.spot_turn_last_posture;
  } else {
    // 回転軸周りに20deg回転クォータニオンを作成する.
    const auto rot_quaternion = Quaternion::MakeByAngleAxis(
        math_util::ConvertDegToRad(20.0f), operation.spot_turn_rot_axis);
    target_quaternion = rot_quaternion * graph.GetRootNode().posture;
  }
 
  const float target_z_value = InitTargetZValue(
      graph.GetRootNode(), divided_map_state, target_quaternion);
 
  GraphSearchEvaluationValue max_evaluation_value =
      evaluator_.InitializeEvaluationValue();
  int max_evaluation_value_index = -1;
 
  for (int i = 0; i < graph.GetGraphSize(); i++) {
    // 最大深さのノードのみを評価する.
    if (graph.GetNode(i).depth != max_depth) {
      continue;
    }
 
    // 評価値を計算する.
    GraphSearchEvaluationValue candidate_evaluation_value =
        evaluator_.InitializeEvaluationValue();
 
    candidate_evaluation_value.value.at(kTagAmountOfTurn) =
        GetAmountOfTurnEvaluationValue(graph.GetNode(i), target_quaternion);
    candidate_evaluation_value.value.at(kTagLegRot) =
        GetLegRotEvaluationValue(graph.GetNode(i), graph.GetRootNode());
    candidate_evaluation_value.value.at(kTagZDiff) =
        GetZDiffEvaluationValue(graph.GetNode(i), target_z_value);
 
    // 評価値を比較する.
    if (evaluator_.LeftIsBetter(candidate_evaluation_value,
                                max_evaluation_value)) {
      max_evaluation_value = candidate_evaluation_value;
      max_evaluation_value_index = i;
    }
  }
 
  // インデックスが範囲外ならば失敗.
  if (max_evaluation_value_index < 0 ||
      graph.GetGraphSize() <= max_evaluation_value_index) {
    const GraphSearchResult result = {enums::Result::kFailure,
                                      "最大評価値のインデックスが範囲外です."};
    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>
GraphSearcherSpotTurn::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;
 
  for (const auto& graph : graph_vector) {
    const auto result =
        SearchGraphTree(graph, operation, divided_map_state, max_depth);
 
    result_vector.push_back(result);
  }
 
  // 最大評価値を持つものを探す.
  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.at(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 ||
      max_evaluation_value_index >= result_vector.size()) {
    const GraphSearchResult result = {enums::Result::kFailure,
                                      "最大評価値のインデックスが範囲外です."};
    return {result, GraphSearchEvaluationValue{}, RobotStateNode{}};
  }
 
  return result_vector[max_evaluation_value_index];
}
 
GraphSearchEvaluator GraphSearcherSpotTurn::InitializeEvaluator() const {
  GraphSearchEvaluator::EvaluationMethod amount_of_turn_method = {
      .is_lower_better = false,
      .margin = 0.0f,
  };
 
  GraphSearchEvaluator::EvaluationMethod leg_rot_method = {
      .is_lower_better = false,
      .margin = 50.0f,
  };
 
  GraphSearchEvaluator::EvaluationMethod z_diff_method = {
      .is_lower_better = true,
      .margin = 1.0f,
  };
 
  GraphSearchEvaluator ret({{kTagAmountOfTurn, amount_of_turn_method},
                            {kTagLegRot, leg_rot_method},
                            {kTagZDiff, z_diff_method}},
                           {
                               kTagAmountOfTurn,
                               kTagLegRot,
                               kTagZDiff,
                           });
 
  return ret;
}
 
float GraphSearcherSpotTurn::InitTargetZValue(
    const RobotStateNode& node, const DividedMapState& divided_map_state,
    [[maybe_unused]] const Quaternion& target_quat) const {
  const int div = 60;
  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.z += z;
 
    RobotStateNode temp_node = node;
    temp_node.ChangeGlobalCenterOfMass(pos, false);
    temp_node.ChangePosture(converter_ptr_, target_quat);
 
    if (!checker_ptr_->IsBodyInterferingWithGround(temp_node,
                                                   divided_map_state)) {
      // std::cout << "z = " << node.center_of_mass_global_coord.z + z <<
      // std::endl;
      return node.center_of_mass_global_coord.z + z;
    }
  }
 
  return node.center_of_mass_global_coord.z;
}
 
float GraphSearcherSpotTurn::GetAmountOfTurnEvaluationValue(
    const RobotStateNode& node, const Quaternion& target_quat) const {
  // 目標姿勢を Qt,現在の姿勢を Qc とする.
  // Qt = Qc * Qd となるような Qd を求める.
  // Qd = Qc^-1 * Qt となる.
 
  // 最終姿勢を表すクォータニオンとの差分を計算する.
  const Quaternion target_to_current =
      node.posture.GetConjugate() * target_quat;
 
  return target_to_current.w;
}
 
float GraphSearcherSpotTurn::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 = 50.0f;
}
 
float GraphSearcherSpotTurn::GetZDiffEvaluationValue(
    const RobotStateNode& node, const float target_z_value) const {
  return abs(node.center_of_mass_global_coord.z - target_z_value);
}
 
}  // namespace designlab