phantomx_mk2_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_PHANTOMX_MK2_CONST_H_
#define DESIGNLAB_PHANTOMX_MK2_CONST_H_
 
#include <array>
 
#include "cassert_define.h"
#include "math_util.h"
 
 
namespace designlab
{
 
class PhantomXMkIIConst final
{
private:
    constexpr static  int kPhantomXLegNum = 6;
 
public:
    // コンストラクタとコピーコンストラクタを削除し，実体を生成できないようにする．
    PhantomXMkIIConst() = delete;
    PhantomXMkIIConst(const PhantomXMkIIConst& other) = delete;
    PhantomXMkIIConst& operator=(const PhantomXMkIIConst& other) = delete;
    PhantomXMkIIConst(PhantomXMkIIConst&& 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(-81.0f);
 
    constexpr static float kCoxaAngleMax = math_util::ConvertDegToRad(81.0f);
 
 
    constexpr static float kFemurAngleMin = math_util::ConvertDegToRad(-105.0f);
 
    constexpr static float kFemurAngleMax = math_util::ConvertDegToRad(99.0f);
 
 
    constexpr static float kTibiaAngleMin = math_util::ConvertDegToRad(-145.0f);
 
    constexpr static float kTibiaAngleMax = math_util::ConvertDegToRad(25.5f);
 
 
    constexpr static float kCoxaLength = 52.0f;     
    constexpr static float kFemurLength = 66.0f;    
    constexpr static float kTibiaLength = 130.0f;   
 
    constexpr static float kCoxaBaseOffsetY = 61.f;
 
    constexpr static float kCenterCoxaBaseOffsetY = 103.4f;
 
    constexpr static float kCoxaBaseOffsetX = 122.f;
 
    constexpr static float kCoxaBaseOffsetZ = 1.116f;
 
    constexpr static float kBodyHeight = 40.0f;
 
 
    constexpr static bool IsValidCoxaAngle(const int leg_index, const float angle)
    {
        // 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)
    {
        return kFemurAngleMin <= angle && angle <= kFemurAngleMax;
    };
 
    constexpr static bool IsValidTibiaAngle(const float angle)
    {
        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_PHANTOMX_MK2_CONST_H_