xr_r1_const.h

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#ifndef DESIGNLAB_XR_R1_CONST_H_
#define DESIGNLAB_XR_R1_CONST_H_
 
#include <array>
 
#include "cassert_define.h"
#include "math_util.h"
 
 
namespace designlab {
 
class XrR1Const final {
private:
    constexpr static  int kPhantomXLegNum = 6;
 
public:
    // コンストラクタとコピーコンストラクタを削除し,実体を生成できないようにする.
    XrR1Const() = delete;
    XrR1Const(const XrR1Const& other) = delete;
    XrR1Const& operator=(const XrR1Const& other) = delete;
    XrR1Const(XrR1Const&& other) = delete;
 
    constexpr static std::array<float, kPhantomXLegNum> kCoxaDefaultAngle = {
      math_util::ConvertDegToRad(-45.0f),
      math_util::ConvertDegToRad(-90.0f),
      math_util::ConvertDegToRad(-135.0f),
      math_util::ConvertDegToRad(135.0f),
      math_util::ConvertDegToRad(90.0f),
      math_util::ConvertDegToRad(45.0f)
    };
 
    constexpr static float kCoxaAngleMin = math_util::ConvertDegToRad(-90.0f);
 
    constexpr static float kCoxaAngleMax = math_util::ConvertDegToRad(90.0f);
 
 
    constexpr static float kFemurAngleMin = math_util::ConvertDegToRad(-90.0f);
 
    constexpr static float kFemurAngleMax = math_util::ConvertDegToRad(90.0f);
 
 
    constexpr static float kTibiaAngleMin = math_util::ConvertDegToRad(-170.0f);
 
    constexpr static float kTibiaAngleMax = math_util::ConvertDegToRad(0.0f);
 
 
    constexpr static float kCoxaLength = 45.0f;     
    constexpr static float kFemurLength = 75.0f;    
    constexpr static float kTibiaLength = 140.0f;   
 
    constexpr static float kCoxaBaseOffsetY = 59.f;
 
    constexpr static float kCenterCoxaBaseOffsetY = 91.5f;
 
    constexpr static float kCoxaBaseOffsetX = 120.f;
 
    constexpr static float kCoxaBaseOffsetZ = 0.f;
 
    constexpr static float kBodyHeight = 45.0f;
 
 
    constexpr static bool IsValidCoxaAngle(const int leg_index, const float angle) noexcept {
        // 0 <= leg_index < kPhantomXLegNum であることを保証する.
        assert(0 <= leg_index);
        assert(leg_index < kPhantomXLegNum);
 
        return (kCoxaAngleMin + kCoxaDefaultAngle[leg_index] <= angle &&
          angle <= kCoxaAngleMax + kCoxaDefaultAngle[leg_index]);
    };
 
    constexpr static bool IsValidFemurAngle(const float angle) noexcept {
        return kFemurAngleMin <= angle && angle <= kFemurAngleMax;
    };
 
    constexpr static bool IsValidTibiaAngle(const float angle) noexcept {
        return kTibiaAngleMin <= angle && angle <= kTibiaAngleMax;
    };
 
 
    static_assert(kCoxaAngleMin < kCoxaAngleMax,
                  "kCoxaAngleMax > Min is required.");
    static_assert(kFemurAngleMin < kFemurAngleMax,
                  "kFemurAngleMax > Min is required.");
    static_assert(kTibiaAngleMin < kTibiaAngleMax,
                  "kTibiaAngleMax > Min is required.");
 
    static_assert(kCoxaLength > 0.f,
                  "kCoxaLength and Coxa Link must be positive.");
    static_assert(kFemurLength > 0.f,
                  "kFemurLength and Femur Link must be positive.");
    static_assert(kTibiaLength > 0.f,
                  "kTibiaLength and Tibia Link must be positive.");
};
 
}  // namespace designlab
 
 
#endif  // DESIGNLAB_XR_R1_CONST_H_