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_