node_creator_body_rot.h

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#ifndef DESIGNLAB_NODE_CREATOR_BODY_ROT_H_
#define DESIGNLAB_NODE_CREATOR_BODY_ROT_H_
 
#include <memory>
#include <vector>
 
#include "divided_map_state.h"
#include "interface_hexapod_coordinate_converter.h"
#include "interface_hexapod_posture_validator.h"
#include "interface_node_creator.h"
#include "math_util.h"
 
namespace designlab {
 
class NodeCreatorBodyRot final : public INodeCreator {
 public:
  NodeCreatorBodyRot(
      const DividedMapState& devide_map,
      const std::shared_ptr<const IHexapodCoordinateConverter>& converter_ptr,
      const std::shared_ptr<const IHexapodPostureValidator>& checker_ptr,
      const Vector3& rot_axis, HexapodMove next_move);
 
  ~NodeCreatorBodyRot() = default;
 
  void Create(const RobotStateNode& current_node, int current_num,
              std::vector<RobotStateNode>* output_graph) const override;
 
 private:
  static constexpr float kBodyYawRotAngleMax = math_util::ConvertDegToRad(20.f);
 
  static constexpr float kBodyYawRotAngleMin =
      math_util::ConvertDegToRad(-20.f);
 
  static constexpr int kBodyYawRotAngleDivNum = 21;
 
  constexpr std::array<float, kBodyYawRotAngleDivNum> CreateCandidateAngle()
      const {
    std::array<float, kBodyYawRotAngleDivNum> res{};
    int count = 0;
 
    res[count++] = (kBodyYawRotAngleMax + kBodyYawRotAngleMin) / 2;
 
    // 絶対値の小さい順に並べる.
    const float dif = (kBodyYawRotAngleMax - kBodyYawRotAngleMin) /
                      (kBodyYawRotAngleDivNum - 1);
 
    for (int i = 0; i < kBodyYawRotAngleDivNum / 2; ++i) {
      res[count++] = res[0] + dif * (i + 1);
      res[count++] = res[0] - dif * (i + 1);
    }
 
    return res;
  }
 
  const std::array<float, kBodyYawRotAngleDivNum> candidate_angle_ =
      CreateCandidateAngle();
 
  const DividedMapState map_;    
  const HexapodMove next_move_;  
 
  const std::shared_ptr<const IHexapodCoordinateConverter> converter_ptr_;
  const std::shared_ptr<const IHexapodPostureValidator> checker_ptr_;
 
  Vector3 rot_axis_;  
 
  static_assert(
      kBodyYawRotAngleMax > kBodyYawRotAngleMin,
      "kBodyYawRotAngleMax は kBodyYawRotAngleMinよりも大きい必要があります.");
  static_assert(kBodyYawRotAngleDivNum % 2 == 1,
                "kBodyYawRotAngleDivNum は奇数である必要があります.");
};
 
}  // namespace designlab
 
#endif  // DESIGNLAB_NODE_CREATOR_BODY_ROT_H_