node_creator_body_rot.cpp

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#include "node_creator_body_rot.h"
 
namespace designlab {
 
NodeCreatorBodyRot::NodeCreatorBodyRot(
    const DividedMapState& divided_map,
    const std::shared_ptr<const IHexapodCoordinateConverter>& converter_ptr,
    const std::shared_ptr<const IHexapodPostureValidator>& checker_ptr,
    const Vector3& rot_axis, HexapodMove next_move)
    : map_(divided_map),
      next_move_(next_move),
      converter_ptr_(converter_ptr),
      checker_ptr_(checker_ptr),
      rot_axis_(rot_axis) {}
 
void NodeCreatorBodyRot::Create(
    const RobotStateNode& current_node, int current_num,
    std::vector<RobotStateNode>* output_graph) const {
  for (int i = 0; i < kBodyYawRotAngleDivNum; ++i) {
    const Quaternion quat =
        Quaternion::MakeByAngleAxis(candidate_angle_[i], rot_axis_);
 
    RobotStateNode node = current_node;
    node.ChangePosture(converter_ptr_, current_node.posture * quat);
 
    if (checker_ptr_->IsBodyInterferingWithGround(node, map_)) {
      continue;
    }
 
    bool is_valid = true;
 
    for (int j = 0; j < HexapodConst::kLegNum; j++) {
      if (!checker_ptr_->IsLegInRange(j, node.leg_pos[j])) {
        is_valid = false;
        break;
      }
    }
 
    if (!is_valid) {
      continue;
    }
 
    node.ChangeToNextNode(current_num, next_move_);
 
    output_graph->push_back(node);
  }
 
  if (output_graph->empty()) {
    // 1つも回転動作を追加できない場合は,回転しないノードを追加する.
    RobotStateNode node = current_node;
    node.ChangeToNextNode(current_num, next_move_);
 
    output_graph->push_back(node);
  }
}
 
}  // namespace designlab