gait_pattern_graph_tree.h

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#ifndef DESIGNLAB_GAIT_PATTERN_GRAPH_TREE_H_
#define DESIGNLAB_GAIT_PATTERN_GRAPH_TREE_H_
 
#include <vector>
 
#include "cassert_define.h"
#include "robot_state_node.h"
 
namespace designlab {
 
class GaitPatternGraphTree final {
 public:
  GaitPatternGraphTree() = delete;
 
  explicit inline GaitPatternGraphTree(const int graph_max_size)
      : graph_size_(0), kGraphMaxSize(graph_max_size) {
    nodes_.resize(graph_max_size);
  }
 
  [[nodiscard]] constexpr int GetGraphSize() const { return graph_size_; }
 
  [[nodiscard]] constexpr bool IsEmpty() const { return graph_size_ == 0; }
 
  [[nodiscard]] constexpr bool HasRoot() const { return graph_size_ != 0; }
 
  [[nodiscard]] constexpr int GetRootIndex() const { return 0; }
 
  [[nodiscard]] inline const RobotStateNode& GetRootNode() const {
    return nodes_[0];
  }
 
  [[nodiscard]] inline const RobotStateNode& GetNode(const int index) const {
    assert(0 <= index);
    assert(index < graph_size_);
    return nodes_[index];
  }
 
  [[nodiscard]] const RobotStateNode& GetParentNode(const int index,
                                                    const int depth) const;
 
  [[nodiscard]] const int GetParentNodeIndex(const int index,
                                             const int depth) const;
 
  inline void AddNode(const RobotStateNode& node) {
    assert(graph_size_ < kGraphMaxSize);
 
    nodes_[graph_size_++] = node;
 
    assert(nodes_[graph_size_ - 1].IsLootNode() ||
           0 <= nodes_[graph_size_ - 1].parent_index);
 
    assert(nodes_[graph_size_ - 1].parent_index < graph_size_);
 
    assert(nodes_[graph_size_ - 1].IsLootNode() ||
           nodes_[graph_size_ - 1].depth - 1 ==
               nodes_[nodes_[graph_size_ - 1].parent_index].depth);
  }
 
  constexpr void Reset() { graph_size_ = 0; }
 
  [[nodiscard]] std::vector<float> GetCoMVerticalTrajectory(
      const int index) const {
    assert(0 <= index);
    assert(index < graph_size_);
    std::vector<float> com_vertical_trajectory_reverse;
 
    // 現在のノードから根ノードまでの重心の上下移動軌跡を取得する.
    for (int i = index; i != -1; i = nodes_[i].parent_index) {
      if (com_vertical_trajectory_reverse.empty() ||
          com_vertical_trajectory_reverse.back() !=
              nodes_[i].center_of_mass_global_coord.z) {
        com_vertical_trajectory_reverse.push_back(
            nodes_[i].center_of_mass_global_coord.z);
      }
    }
 
    // 根ノードから現在のノードまでの重心の上下移動軌跡を取得する.
    std::vector<float> com_vertical_trajectory(
        com_vertical_trajectory_reverse.size());
    for (int i = 0; i < com_vertical_trajectory_reverse.size(); ++i) {
      com_vertical_trajectory[i] = com_vertical_trajectory_reverse
          [com_vertical_trajectory_reverse.size() - 1 - i];
    }
 
    return com_vertical_trajectory;
  }
 
 private:
  std::vector<RobotStateNode> nodes_;  
 
  int graph_size_;
 
  const int kGraphMaxSize;  
};
 
}  // namespace designlab
 
#endif  // DESIGNLAB_GAIT_PATTERN_GRAPH_TREE_H_