interface_hexapod_joint_calculator.h

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#ifndef DESIGNLAB_INTERFACE_HEXAPOD_JOINT_CALCULATOR_H_
#define DESIGNLAB_INTERFACE_HEXAPOD_JOINT_CALCULATOR_H_
 
#include <array>
#include <vector>
 
#include "robot_state_node.h"
 
 
namespace designlab
{
 
struct HexapodJointState
{
    std::vector<Vector3> joint_pos_leg_coordinate;
 
    std::vector<float> joint_angle;
 
    bool is_in_range{ false };
};
 
 
class IHexapodJointCalculator
{
public:
    virtual ~IHexapodJointCalculator() = default;
 
 
    [[nodiscard]]
    std::array<HexapodJointState, HexapodConst::kLegNum> CalculateAllJointState(
        const RobotStateNode& node) const noexcept
    {
        std::array<HexapodJointState, HexapodConst::kLegNum> joint_state;
 
        // 計算を行う．
        for (int i = 0; i < HexapodConst::kLegNum; i++)
        {
            joint_state[i] = CalculateJointState(i, node.leg_pos[i]);
        }
 
        return joint_state;
    }
 
    [[nodiscard]]
    virtual HexapodJointState CalculateJointState(
        const int leg_index, const Vector3& leg_pos) const noexcept = 0;
 
    [[nodiscard]]
    bool IsValidAllJointState(
        const RobotStateNode& node,
        const std::array<HexapodJointState, HexapodConst::kLegNum>& joint_state)
        const noexcept
    {
        for (int i = 0; i < HexapodConst::kLegNum; i++)
        {
            if (!IsValidJointState(i, node.leg_pos[i], joint_state[i]))
            {
                return false;
            }
        }
 
        return true;
    }
 
    [[nodiscard]]
    virtual bool IsValidJointState(
        const int leg_index, const Vector3& leg_pos,
        const HexapodJointState& joint_state) const noexcept = 0;
};
 
}  // namespace designlab
 
 
#endif  // DESIGNLAB_INTERFACE_HEXAPOD_JOINT_CALCULATOR_H_