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 <memory>
#include <string>
 
#include <magic_enum.hpp>
 
#include "array_util.h"
#include "hexapod_next_move.h"
#include "interface_hexapod_coordinate_converter.h"
#include "leg_state.h"
#include "math_quaternion.h"
#include "math_vector3.h"
#include "robot_operation.h"
 
 
namespace designlab
{
 
struct RobotStateNode final
{
    static constexpr int kNoParentIndex = -1;  
 
    constexpr RobotStateNode() :
        leg_state(0),
        leg_pos{ MakeArray<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 designlab::Vector3& new_com,
                                  bool do_change_leg_base_pos);
 
    void ChangePosture(
        const std::shared_ptr<const IHexapodCoordinateConverter>& converter_ptr,
        const designlab::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_