robot_state_node.h

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#ifndef DESIGNLAB_ROBOT_STATE_NODE_H_
#define DESIGNLAB_ROBOT_STATE_NODE_H_
 
#include <array>
#include <magic_enum.hpp>
#include <memory>
#include <string>
 
#include "hexapod_next_move.h"
#include "interface_hexapod_coordinate_converter.h"
#include "leg_state.h"
#include "make_array.h"
#include "math_quaternion.h"
#include "math_vector3.h"
 
namespace designlab {
 
struct RobotStateNode final {
  static constexpr int kNoParentIndex = -1;  
 
  constexpr RobotStateNode()
      : leg_state(0),
        leg_pos{nostd::make_array<Vector3>(Vector3{}, Vector3{}, Vector3{},
                                           Vector3{}, Vector3{}, Vector3{})},
        leg_reference_pos{leg_pos},
        center_of_mass_global_coord{},
        posture{},
        next_move(HexapodMove::kComUpDown),
        parent_index(kNoParentIndex),
        depth(0) {}
 
  constexpr RobotStateNode(
      const leg_func::LegStateBit& state,
      const std::array<Vector3, HexapodConst::kLegNum>& pos,
      const std::array<Vector3, HexapodConst::kLegNum>& ref_pos,
      const Vector3& com, const Quaternion& q, const HexapodMove move,
      const int parent, const int d)
      : leg_state(state),
        leg_pos(pos),
        leg_reference_pos(ref_pos),
        center_of_mass_global_coord(com),
        posture(q),
        next_move(move),
        parent_index(parent),
        depth(d) {}
 
  RobotStateNode(const RobotStateNode& other) = default;
  RobotStateNode(RobotStateNode&& other) noexcept = default;
  RobotStateNode& operator=(const RobotStateNode& other) = default;
  ~RobotStateNode() = default;
 
  void ChangeGlobalCenterOfMass(const Vector3& new_com,
                                bool do_change_leg_base_pos);
 
  void ChangePosture(
      const std::shared_ptr<const IHexapodCoordinateConverter>& converter_ptr,
      const Quaternion& new_posture);
 
  inline void ChangeLootNode() {
    depth = 0;
    parent_index = kNoParentIndex;
    posture.Normalize();
  }
 
  constexpr void ChangeToNextNode(const int parent_index_,
                                  const HexapodMove next_move_) {
    ++depth;
    parent_index = parent_index_;
    next_move = next_move_;
  }
 
  constexpr bool IsLootNode() const {
    return (parent_index < 0) && (depth == 0);
  }
 
  std::string ToString() const;
 
  std::string ToCsvString() const;
 
  static RobotStateNode FromString(const std::string& str);
 
  constexpr bool operator==(const RobotStateNode& other) const {
    if (next_move != other.next_move) {
      return false;
    }
 
    if (leg_state != other.leg_state) {
      return false;
    }
 
    for (int i = 0; i < HexapodConst::kLegNum; i++) {
      if (leg_pos[i] != other.leg_pos[i]) {
        return false;
      }
 
      // if (leg_reference_pos[i] != other.leg_reference_pos[i])
      // {
      //  return false;
      // }
    }
 
    if (center_of_mass_global_coord != other.center_of_mass_global_coord) {
      return false;
    }
 
    if (posture != other.posture) {
      return false;
    }
 
    return true;
  }
 
  constexpr bool operator!=(const RobotStateNode& other) {
    return !(*this == other);
  }
 
  leg_func::LegStateBit leg_state;
 
  std::array<Vector3, HexapodConst::kLegNum> leg_pos;
 
  std::array<Vector3, HexapodConst::kLegNum> leg_reference_pos;
 
  Vector3 center_of_mass_global_coord;
 
  Quaternion posture;
 
  HexapodMove next_move;
 
  int parent_index;
 
  int depth;
};
 
template <class Char>
std::basic_ostream<Char>& operator<<(std::basic_ostream<Char>& os,
                                     const RobotStateNode& value) {
  os << value.leg_state.to_string() << ",";
 
  for (int i = 0; i < HexapodConst::kLegNum; i++) {
    os << value.leg_pos[i] << ",";
  }
 
  for (int i = 0; i < HexapodConst::kLegNum; i++) {
    os << value.leg_reference_pos[i] << ",";
  }
 
  os << value.center_of_mass_global_coord << ",";
  os << value.posture << ",";
  os << magic_enum::enum_name(value.next_move) << ",";
  os << value.parent_index << ",";
  os << value.depth;
 
  return os;
}
 
}  // namespace designlab
 
#endif  // DESIGNLAB_ROBOT_STATE_NODE_H_