phantomx_mk2.h

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#ifndef DESIGNLAB_PHANTOMX_MK2_H_
#define DESIGNLAB_PHANTOMX_MK2_H_
 
#include "interface_hexapod_coordinate_converter.h"
#include "interface_hexapod_joint_calculator.h"
#include "interface_hexapod_state_presenter.h"
#include "interface_hexapod_posture_validator.h"
#include "phantomx_mk2_parameter_record.h"
 
 
namespace designlab
{
 
class PhantomXMkII :
    public IHexapodCoordinateConverter,
    public IHexapodJointCalculator,
    public IHexapodStatePresenter,
    public IHexapodPostureValidator
{
public:
    explicit PhantomXMkII(const PhantomXMkIIParameterRecord& parameter_record);
 
 
    HexapodJointState CalculateJointState(
        const int leg_index, const Vector3& leg_pos) const noexcept override;
 
    bool IsValidJointState(
        const int leg_index, const Vector3& leg_pos,
        const HexapodJointState& joint_state) const noexcept override;
 
 
    Vector3 ConvertGlobalToLegCoordinate(
        const Vector3& converted_position, int leg_index,
        const Vector3& center_of_mass_global, const Quaternion& robot_quat,
        const bool consider_rot) const override;
 
    Vector3 ConvertLegToGlobalCoordinate(
        const Vector3& converted_position, int leg_index,
        const Vector3& center_of_mass_global, const Quaternion& robot_quat,
        const bool consider_rot) const override;
 
    Vector3 ConvertRobotToGlobalCoordinate(
        const Vector3& converted_position,
        const Vector3& center_of_mass_global, const Quaternion& robot_quat,
        const bool consider_rot) const override;
 
    Vector3 ConvertRobotToLegCoordinate(
        const Vector3& converted_position, int leg_index) const;
 
    Vector3 ConvertLegToRobotCoordinate(
        const Vector3& converted_position, int leg_index) const;
 
 
    Vector3 GetFreeLegPosLegCoordinate(int leg_index) const noexcept override;
 
    Vector3 GetLegBasePosRobotCoordinate(int leg_index) const noexcept override;
 
    float GetGroundHeightMarginMin() const noexcept override;
 
    float GetGroundHeightMarginMax() const noexcept override;
 
 
    bool IsLegInRange(int leg_index, const Vector3& leg_pos) const override;
 
    bool IsLegInterfering(
        const std::array<Vector3, HexapodConst::kLegNum>& leg_pos) const override;
 
    float CalculateStabilityMargin(const leg_func::LegStateBit& leg_state,
      const std::array<Vector3, HexapodConst::kLegNum>& leg_pos) const override;
 
    bool IsStable(const leg_func::LegStateBit& leg_state,
      const std::array<Vector3, HexapodConst::kLegNum>& leg_pos) const override;
 
    bool IsBodyInterferingWithGround(
        const RobotStateNode& node,
        const DividedMapState& devide_map) const override;
 
private:
    static constexpr int kMaxLegRSize{ 200 };   
 
 
    std::array<float, kMaxLegRSize> InitMaxLegR() const;
    std::array<Vector2, HexapodConst::kLegNum> InitMinLegPosXY() const;
    std::array<Vector2, HexapodConst::kLegNum> InitMaxLegPosXY() const;
 
    const float kBodyLiftingHeightMin;
 
    const float kBodyLiftingHeightMax;
 
    const float kMovableCoxaAngleMin;   
    const float kMovableCoxaAngleMax;   
 
    const float kMinLegR;   
    const float kMaxLegR;   
 
    const float kFreeLegHeight;  
    const float kStableMargin;   
 
    const float kMinLegDistance;  
 
    std::array<float, kMaxLegRSize> kMaxLegRArray;
 
    std::array<Vector2, HexapodConst::kLegNum> kMinLegPosXY;
 
    std::array<Vector2, HexapodConst::kLegNum> kMaxLegPosXY;
 
 
    const std::array<Vector3, HexapodConst::kLegNum> leg_base_pos_robot_coordinate_;
 
    const std::array<Vector3, HexapodConst::kLegNum> free_leg_pos_leg_coordinate_;
};
 
}  // namespace designlab
 
 
#endif  // DESIGNLAB_PHANTOMX_MK2_H_