xr_r1.cpp

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#include "xr_r1.h"
 
#include <numbers>
 
#include <cmath>
 
#include "cassert_define.h"
#include "math_line_segment2.h"
#include "math_util.h"
#include "leg_state.h"
#include "xr_r1_const.h"
 
namespace designlab {
 
XrR1::XrR1(const XrR1ParameterRecord& parameter_record) :
    kBodyLiftingHeightMin(parameter_record.body_lifting_height_min),
    kBodyLiftingHeightMax(parameter_record.body_lifting_height_max),
    kMovableCoxaAngleMin(math_util::ConvertDegToRad(parameter_record.movable_coxa_angle_min_deg)),
    kMovableCoxaAngleMax(math_util::ConvertDegToRad(parameter_record.movable_coxa_angle_max_deg)),
    kMinLegR(parameter_record.min_leg_range),
    kMaxLegR(parameter_record.max_leg_range),
    kFreeLegHeight(parameter_record.free_leg_height),
    kStableMargin(parameter_record.stable_margin),
    free_leg_pos_leg_coordinate_({ {
        {160 * cos(XrR1Const::kCoxaDefaultAngle[0]), 160 * sin(XrR1Const::kCoxaDefaultAngle[0]), kFreeLegHeight},
        {160 * cos(XrR1Const::kCoxaDefaultAngle[1]), 160 * sin(XrR1Const::kCoxaDefaultAngle[1]), kFreeLegHeight},
        {160 * cos(XrR1Const::kCoxaDefaultAngle[2]), 160 * sin(XrR1Const::kCoxaDefaultAngle[2]), kFreeLegHeight},
        {160 * cos(XrR1Const::kCoxaDefaultAngle[3]), 160 * sin(XrR1Const::kCoxaDefaultAngle[3]), kFreeLegHeight},
        {160 * cos(XrR1Const::kCoxaDefaultAngle[4]), 160 * sin(XrR1Const::kCoxaDefaultAngle[4]), kFreeLegHeight},
        {160 * cos(XrR1Const::kCoxaDefaultAngle[5]), 160 * sin(XrR1Const::kCoxaDefaultAngle[5]), kFreeLegHeight},
    } }),
    leg_base_pos_robot_coordinate_({ {
        { XrR1Const::kCoxaBaseOffsetX, -XrR1Const::kCoxaBaseOffsetY, XrR1Const::kCoxaBaseOffsetZ },     // 脚0 右上.
        { 0.0f, -XrR1Const::kCenterCoxaBaseOffsetY, XrR1Const::kCoxaBaseOffsetZ },                      // 脚1 右横.
        { -XrR1Const::kCoxaBaseOffsetX, -XrR1Const::kCoxaBaseOffsetY, XrR1Const::kCoxaBaseOffsetZ },    // 脚2 右下.
        { -XrR1Const::kCoxaBaseOffsetX, XrR1Const::kCoxaBaseOffsetY, XrR1Const::kCoxaBaseOffsetZ },     // 脚3 左下.
        { 0.0f, XrR1Const::kCenterCoxaBaseOffsetY, XrR1Const::kCoxaBaseOffsetZ },                       // 脚4 左横.
        { XrR1Const::kCoxaBaseOffsetX, XrR1Const::kCoxaBaseOffsetY, XrR1Const::kCoxaBaseOffsetZ },      // 脚5 左上.
    } }),
    kMaxLegRArray(InitMaxLegR()),
    kMinLegPosXY(InitMinLegPosXY()),
    kMaxLegPosXY(InitMaxLegPosXY()),
    kMinLegDistance(50.0) {}
 
HexapodJointState XrR1::CalculateJointState(
    const int leg_index, const Vector3& leg_pos) const noexcept {
    // leg_indexは 0～5 である.
    assert(0 <= leg_index);
    assert(leg_index < HexapodConst::kLegNum);
 
    // 各パラメータを初期化する.
    HexapodJointState res;
 
    const int kJointNum = 4;
    const int kJointAngleNum = kJointNum - 1;
 
    res.joint_pos_leg_coordinate.clear();
    res.joint_angle.clear();
 
    res.joint_pos_leg_coordinate.resize(kJointNum);
    res.joint_angle.resize(kJointAngleNum);
 
 
    // coxa jointの計算.脚座標系では coxa joint は原点にある.
    res.joint_pos_leg_coordinate[0] = Vector3{ 0, 0, 0 };
 
    // 脚先の追加.
    res.joint_pos_leg_coordinate[3] = leg_pos;
 
    // coxa angleの計算.
    {
        float coxa_joint_angle = std::atan2f(leg_pos.y, leg_pos.x);
 
        if (leg_pos.y == 0 && leg_pos.x == 0) {
            coxa_joint_angle = XrR1Const::kCoxaDefaultAngle[leg_index];
        }
 
        if (!XrR1Const::IsValidCoxaAngle(leg_index, coxa_joint_angle)) {
            // 範囲外ならば,180度回転させた時に範囲内にあるかを調べる.
            if (XrR1Const::IsValidCoxaAngle(
                leg_index, coxa_joint_angle + std::numbers::pi_v<float>)) {
                coxa_joint_angle += std::numbers::pi_v<float>;
            }
            else if (XrR1Const::IsValidCoxaAngle(
                leg_index, coxa_joint_angle - std::numbers::pi_v<float>)) {
                coxa_joint_angle -= std::numbers::pi_v<float>;
            }
        }
 
        res.joint_angle[0] = coxa_joint_angle;
    }
 
    // femur jointの計算.
    {
        const Vector3 femur_joint_pos = Vector3{
            XrR1Const::kCoxaLength * std::cos(res.joint_angle[0]),
            XrR1Const::kCoxaLength * std::sin(res.joint_angle[0]),
            0
        };
 
        res.joint_pos_leg_coordinate[1] = femur_joint_pos;
 
        if (!math_util::CanMakeTriangle((leg_pos - femur_joint_pos).GetLength(),
            XrR1Const::kFemurLength,
            XrR1Const::kTibiaLength)) {
            // そもそも脚先が脚の付け根から届かない場合,一番近い位置まで脚を伸ばす.
 
            const float angle_ft = std::atan2(
                leg_pos.z - femur_joint_pos.z,
                (leg_pos.ProjectedXY() - femur_joint_pos.ProjectedXY()).GetLength());
 
            // angle_ftの位相を180度回転する.-180度～180度の範囲にする.
            float angle_ft_phase = angle_ft + std::numbers::pi_v<float>;
            angle_ft_phase = (angle_ft_phase > std::numbers::pi_v<float>) ?
                angle_ft_phase - std::numbers::pi_v<float> : angle_ft_phase;
 
            const Vector3 candidate_leg_pos = femur_joint_pos + Vector3{
                (XrR1Const::kFemurLength + XrR1Const::kTibiaLength) *
                    std::cos(res.joint_angle[0]) * std::cos(angle_ft),
 
                    (XrR1Const::kFemurLength + XrR1Const::kTibiaLength) *
                    std::sin(res.joint_angle[0]) * std::cos(angle_ft),
 
                    (XrR1Const::kFemurLength + XrR1Const::kTibiaLength) *
                    std::sin(angle_ft)
            };
 
            const Vector3 candidate_leg_pos_phase = femur_joint_pos + Vector3{
                (XrR1Const::kFemurLength * std::cos(angle_ft_phase) +
                XrR1Const::kTibiaLength * std::cos(angle_ft)) *
                    std::cos(res.joint_angle[0]),
 
                    (XrR1Const::kFemurLength * std::cos(angle_ft_phase) +
                    XrR1Const::kTibiaLength * std::cos(angle_ft)) *
                    std::sin(res.joint_angle[0]),
 
                    XrR1Const::kFemurLength * std::sin(angle_ft_phase) +
                    XrR1Const::kTibiaLength * std::sin(angle_ft)
            };
 
            const float distance = (leg_pos - candidate_leg_pos).GetLength();
            const float distance_phase = (leg_pos - candidate_leg_pos_phase).GetLength();
 
            float angle_f = 0, angle_t = 0;
 
            if (distance < distance_phase) {
                angle_f = angle_ft;
                angle_t = 0;
            }
            else {
                angle_f = angle_ft_phase;
                angle_t = -std::numbers::pi_v<float>;
            }
 
            res.joint_angle[1] = angle_f;
            res.joint_angle[2] = angle_t;
 
            res.joint_pos_leg_coordinate[2] = femur_joint_pos + Vector3{
                XrR1Const::kFemurLength * std::cos(angle_f) *
                    std::cos(res.joint_angle[0]),
 
                    XrR1Const::kFemurLength * std::cos(angle_f) *
                    std::sin(res.joint_angle[0]),
 
                    XrR1Const::kFemurLength * std::sin(angle_f)
            };
 
            res.joint_pos_leg_coordinate[3] = res.joint_pos_leg_coordinate[2] + Vector3{
                XrR1Const::kTibiaLength * std::cos(angle_f + angle_t) *
                    std::cos(res.joint_angle[0]),
 
                    XrR1Const::kTibiaLength * std::cos(angle_f + angle_t) *
                    std::sin(res.joint_angle[0]),
 
                    XrR1Const::kTibiaLength * std::sin(angle_f + angle_t)
            };
 
            res.is_in_range = false;  // 範囲外であることを示す.
 
            return res;
        }
    }
 
 
    // femur angle の計算.
 
    // 脚先から第一関節までの長さ.
    const Vector3 femur_to_leg = leg_pos - res.joint_pos_leg_coordinate[1];
 
    // 脚先へ向かう方向をxの正方向にする座標系に置き換える.
    // 脚先が第一関節よりも近い場合は正の方向にする.
    const float femur_to_leg_x =
        femur_to_leg.ProjectedXY().GetLength() *
        ((leg_pos.ProjectedXY().GetSquaredLength() >
         res.joint_pos_leg_coordinate[1].ProjectedXY().GetSquaredLength()) ? 1.f : -1.f);
 
    const float femur_to_leg_z = femur_to_leg.z;
 
    {
        const float arc_cos_upper =
            femur_to_leg.GetSquaredLength() +
            math_util::Squared(XrR1Const::kFemurLength) -
            math_util::Squared(XrR1Const::kTibiaLength);
 
        const float arc_cos_lower = 2 * XrR1Const::kFemurLength *
            femur_to_leg.GetLength();
 
        const float arc_cos_arg = arc_cos_upper / arc_cos_lower;
 
 
        const float femur_joint_angle =
            std::acos(arc_cos_arg) +
            std::atan2(femur_to_leg_z, femur_to_leg_x);
 
        res.joint_angle[1] = femur_joint_angle;
    }
 
    // tibia jointの計算.
    {
        const Vector3 femur_to_tibia = Vector3{
            XrR1Const::kFemurLength * std::cos(res.joint_angle[0]) *
            std::cos(res.joint_angle[1]),
 
            XrR1Const::kFemurLength * std::sin(res.joint_angle[0]) *
            std::cos(res.joint_angle[1]),
 
            XrR1Const::kFemurLength * std::sin(res.joint_angle[1])
        };
 
        const Vector3 tibia_joint_pos = res.joint_pos_leg_coordinate[1] + femur_to_tibia;
 
        res.joint_pos_leg_coordinate[2] = tibia_joint_pos;
    }
 
 
    // tibia angleの計算.
    {
        const float tibia_angle = std::atan2(
            (femur_to_leg_z - XrR1Const::kFemurLength * std::sin(res.joint_angle[1])),
            (femur_to_leg_x - XrR1Const::kFemurLength * std::cos(res.joint_angle[1]))) -
            res.joint_angle[1];
 
        res.joint_angle[2] = tibia_angle;
    }
 
    res.is_in_range = true;
 
    return res;
}
 
bool XrR1::IsValidJointState(const int leg_index,
                             const Vector3& leg_pos,
                             const HexapodJointState& joint_state) const noexcept {
    assert(joint_state.joint_pos_leg_coordinate.size() == 4);
    assert(joint_state.joint_angle.size() == 3);
 
    // coxa関節の範囲内に存在しているかを確認する.
    if (!XrR1Const::IsValidCoxaAngle(leg_index, joint_state.joint_angle[0])) {
        return false;
    }
 
    // femur関節の範囲内に存在しているかを確認する.
    if (!XrR1Const::IsValidFemurAngle(joint_state.joint_angle[1])) {
        return false;
    }
 
    // tibia関節の範囲内に存在しているかを確認する.
    if (!XrR1Const::IsValidTibiaAngle(joint_state.joint_angle[2])) {
        return false;
    }
 
    // リンクの長さを確認する.
    if (!math_util::IsEqual((joint_state.joint_pos_leg_coordinate[0] -
        joint_state.joint_pos_leg_coordinate[1]).GetLength(), XrR1Const::kCoxaLength)) {
        return false;
    }
 
    if (!math_util::IsEqual((joint_state.joint_pos_leg_coordinate[1] -
        joint_state.joint_pos_leg_coordinate[2]).GetLength(), XrR1Const::kFemurLength)) {
        return false;
    }
 
    if (!math_util::IsEqual((joint_state.joint_pos_leg_coordinate[2] -
        joint_state.joint_pos_leg_coordinate[3]).GetLength(), XrR1Const::kTibiaLength)) {
        return false;
    }
 
    // 脚位置と脚先座標が一致しているかを確認する.
    if (joint_state.joint_pos_leg_coordinate[3] != leg_pos) { return false; }
 
    return true;
}
 
 
Vector3 XrR1::ConvertGlobalToLegCoordinate(
    const Vector3& converted_position,
    const int leg_index,
    const Vector3& center_of_mass_global,
    const Quaternion& robot_quat,
    const bool consider_rot) const {
    // leg_indexは 0～5 である.
    assert(0 <= leg_index);
    assert(leg_index < HexapodConst::kLegNum);
 
    if (consider_rot) {
        return RotateVector3(converted_position - center_of_mass_global,
                             robot_quat.GetConjugate()) -
            GetLegBasePosRobotCoordinate(leg_index);
    }
    else {
        return converted_position -
            center_of_mass_global -
            GetLegBasePosRobotCoordinate(leg_index);
    }
}
 
Vector3 XrR1::ConvertLegToGlobalCoordinate(
    const Vector3& converted_position,
    int leg_index,
    const Vector3& center_of_mass_global,
    const Quaternion& robot_quat,
    const bool consider_rot) const {
    // leg_indexは 0～5 である.
    assert(0 <= leg_index);
    assert(leg_index < HexapodConst::kLegNum);
 
    if (consider_rot) {
        return RotateVector3(converted_position + GetLegBasePosRobotCoordinate(leg_index),
                             robot_quat) + center_of_mass_global;
    }
    else {
        return converted_position +
            GetLegBasePosRobotCoordinate(leg_index) + center_of_mass_global;
    }
}
 
Vector3 XrR1::ConvertRobotToGlobalCoordinate(
    const Vector3& converted_position,
    const Vector3& center_of_mass_global,
    const Quaternion& robot_quat,
    const bool consider_rot) const {
    if (consider_rot) {
        return RotateVector3(converted_position, robot_quat) + center_of_mass_global;
    }
    else {
        return converted_position + center_of_mass_global;
    }
}
 
Vector3 XrR1::ConvertRobotToLegCoordinate(const Vector3& converted_position,
                                          const int leg_index) const {
    return converted_position - GetLegBasePosRobotCoordinate(leg_index);
}
 
Vector3 XrR1::ConvertLegToRobotCoordinate(const Vector3& converted_position,
                                          const int leg_index) const {
    return converted_position + GetLegBasePosRobotCoordinate(leg_index);
}
 
 
Vector3 XrR1::GetFreeLegPosLegCoordinate(const int leg_index) const noexcept {
    // leg_indexは 0～5 である.
    assert(0 <= leg_index);
    assert(leg_index < HexapodConst::kLegNum);
 
    return free_leg_pos_leg_coordinate_[leg_index];
}
 
Vector3 XrR1::GetLegBasePosRobotCoordinate(const int leg_index) const noexcept {
    // leg_indexは 0～5 である.
    assert(0 <= leg_index);
    assert(leg_index < HexapodConst::kLegNum);
 
    return leg_base_pos_robot_coordinate_[leg_index];
}
 
 
bool XrR1::IsLegInRange(const int leg_index, const Vector3& leg_pos) const {
    // leg_indexは 0～5 である.
    assert(0 <= leg_index);
    assert(leg_index < HexapodConst::kLegNum);
 
    if (GetFreeLegPosLegCoordinate(leg_index).z < leg_pos.z) {
        return false;
    }
 
    const Vector2 leg_pos_xy = leg_pos.ProjectedXY();  // 投射した脚先座標をえる.
 
    // 脚の角度が範囲内にあるか調べる.外積計算で間にあるか調べる
    if (kMinLegPosXY[leg_index].Cross(leg_pos_xy) < 0.0f) {
        return false;
    }
 
    if (kMaxLegPosXY[leg_index].Cross(leg_pos_xy) > 0.0f) {
        return false;
    }
 
 
    // 脚を伸ばすことのできない範囲に伸ばしていないか調べる.
    if (static_cast<int>(leg_pos.z) < -kMaxLegRSize || 0 < static_cast<int>(leg_pos.z)) {
        return false;
    }
 
    if (leg_pos_xy.GetSquaredLength() < math_util::Squared(kMinLegR)) {
        return false;
    }
 
    if (math_util::Squared(kMaxLegR) < leg_pos_xy.GetSquaredLength()) {
        return false;
    }
 
    if (math_util::Squared(kMaxLegRArray[-static_cast<int>(leg_pos.z)]) <
        leg_pos_xy.GetSquaredLength()) {
        return false;
    }
 
    return true;
}
 
bool XrR1::IsLegInterfering(
    const std::array<Vector3, HexapodConst::kLegNum>& leg_pos) const {
    // 重心を原点とした,座標系において,脚の干渉を調べる.
 
    // 脚の干渉を調べる.
    Vector2 leg_pos_xy[HexapodConst::kLegNum];  // 脚先の座標(ロボット座標系).
    Vector2 joint_pos_xy[HexapodConst::kLegNum];  // 脚の根元の座標(ロボット座標系).
 
    // 脚の根元の座標(ロボット座標系)を取得する.
    for (int i = 0; i < HexapodConst::kLegNum; i++) {
        joint_pos_xy[i] = GetLegBasePosRobotCoordinate(i).ProjectedXY();
        leg_pos_xy[i] = ConvertLegToRobotCoordinate(leg_pos[i], i).ProjectedXY();
    }
 
    // 隣の脚との干渉を調べる.
    for (int i = 0; i < HexapodConst::kLegNum; i++) {
        // 脚が交差しているか調べる.
        LineSegment2 line1(joint_pos_xy[i], leg_pos_xy[i]);
        LineSegment2 line2(joint_pos_xy[(i + 1) % HexapodConst::kLegNum],
                           leg_pos_xy[(i + 1) % HexapodConst::kLegNum]);
 
        if (line1.HasIntersection(line2)) { return true; }
 
        // 脚先の距離を確認する.
        if (leg_pos_xy[i].GetDistanceFrom(leg_pos_xy[(i + 1) % HexapodConst::kLegNum]) < kMinLegDistance) {
            return true;
        }
    }
 
    return false;
}
 
float XrR1::GetGroundHeightMarginMin() const noexcept {
    return kBodyLiftingHeightMin;
}
 
float XrR1::GetGroundHeightMarginMax() const noexcept {
    return kBodyLiftingHeightMax;
}
 
float XrR1::CalculateStabilityMargin(
    const leg_func::LegStateBit& leg_state,
    const std::array<Vector3, HexapodConst::kLegNum>& leg_pos) const {
    // std::min をカッコで囲んでいるのは,マクロの min と被るため.
    // (std::min) と書くと名前が衝突しない
 
    // xy平面に投射した,重心を原点としたローカル(ロボット)座標系で,脚の位置を計算する.
    std::array<Vector2, HexapodConst::kLegNum> ground_leg_pos;
 
    // 速度の関係上 vectorでなく array を使う.
    int ground_leg_pos_num = 0;
 
 
    // 接地脚のみ追加する.
    for (int i = 0; i < HexapodConst::kLegNum; i++) {
        if (leg_func::IsGrounded(leg_state, i)) {
            ground_leg_pos[ground_leg_pos_num] = leg_pos[i].ProjectedXY() +
                GetLegBasePosRobotCoordinate(i).ProjectedXY();
 
            ground_leg_pos_num++;
        }
    }
 
 
    float min_margin = 0;  // 多角形の辺と重心の距離の最小値
    bool is_first = true;  // 初回かどうか,最初は必ず値を更新する
 
    for (int i = 0; i < ground_leg_pos_num; i++) {
        Vector2 i_to_i_plus_1 = ground_leg_pos[(i + 1) % ground_leg_pos_num] -
            ground_leg_pos[i];
        Vector2 i_to_com = Vector2{ 0, 0 } - ground_leg_pos[i];
 
        // 多角形の辺と重心の距離(静的安定余裕)
        float margin = i_to_com.Cross(i_to_i_plus_1.GetNormalized());
 
        if (is_first) {
            min_margin = margin;
            is_first = false;
        }
        else {
            min_margin = (std::min)(min_margin, margin);
        }
    }
 
    return min_margin;
}
 
bool XrR1::IsStable(const leg_func::LegStateBit& leg_state,
                    const std::array<Vector3, HexapodConst::kLegNum>& leg_pos) const {
    // kStableMargin 以上の余裕があるか調べる.
    return CalculateStabilityMargin(leg_state, leg_pos) > kStableMargin;
}
 
bool XrR1::IsBodyInterferingWithGround(const RobotStateNode& node,
                                       const DividedMapState& divided_map) const {
    // 重心の干渉を調べる.
    const float top_z = (std::max)(
        divided_map.GetMapMinZ(),
        divided_map.GetTopZ(
        divided_map.GetDividedMapIndexX(node.center_of_mass_global_coord.x),
        divided_map.GetDividedMapIndexY(node.center_of_mass_global_coord.y)));
 
    if (top_z != divided_map.GetMapMinZ() &&
        top_z + GetGroundHeightMarginMin() > node.center_of_mass_global_coord.z) {
        return true;
    }
 
 
    for (int i = 0; i < HexapodConst::kLegNum; ++i) {
        // 脚の根元の座標(グローバル)を取得する.
        const Vector3 coxa_pos_global_coord = ConvertRobotToGlobalCoordinate(
            GetLegBasePosRobotCoordinate(i),
            node.center_of_mass_global_coord,
            node.posture,
            true);
 
        if (divided_map.IsInMap(coxa_pos_global_coord)) {
            const float coxa_top_z = (std::max)(
                divided_map.GetTopZ(divided_map.GetDividedMapIndexX(coxa_pos_global_coord.x), divided_map.GetDividedMapIndexY(coxa_pos_global_coord.y)),
                divided_map.GetMapMinZ());
 
            if (coxa_top_z != divided_map.GetMapMinZ() &&
                coxa_top_z + GetGroundHeightMarginMin() - MathConst<float>::kAllowableError > coxa_pos_global_coord.z) {
                return true;
            }
        }
 
        // 脚先の座標(グローバル)を取得する.
        const Vector3 leg_pos_global_coord = leg_func::IsGrounded(node.leg_state, i) ?
            ConvertLegToGlobalCoordinate(GetFreeLegPosLegCoordinate(i), i, node.center_of_mass_global_coord, node.posture, true) :
            ConvertLegToGlobalCoordinate(node.leg_pos[i], i, node.center_of_mass_global_coord, node.posture, true);
 
        if (divided_map.IsInMap(leg_pos_global_coord)) {
            const float leg_top_z = (std::max)(
                divided_map.GetTopZ(divided_map.GetDividedMapIndexX(leg_pos_global_coord.x), divided_map.GetDividedMapIndexY(leg_pos_global_coord.y)),
                divided_map.GetMapMinZ());
 
            if (leg_top_z != divided_map.GetMapMinZ() &&
                leg_top_z + GetGroundHeightMarginMin() + GetFreeLegPosLegCoordinate(i).z - MathConst<float>::kAllowableError > leg_pos_global_coord.z) {
                return true;
            }
        }
    }
 
    return false;
}
 
 
std::array<float, XrR1::kMaxLegRSize> XrR1::InitMaxLegR() const {
    // 2019年度の先行研究のコードそのまま.
 
    // 逆運動学 coxaなしの計算結果を用いて準運動学を計算する.
    std::array <float, kMaxLegRSize> res;
 
    for (auto i : res) {
        i = 0;
    }
 
    // 逆運動学と運動学を行った結果が半径 Permission ^ 0.5 の円の中なら等しいと考える.
    const float PERMISSION = 0.5f;
 
 
    // 脚可動範囲 おそらく rad 変換したやつ(-81.8° -101.98° -68.41°)  190527
    const float mins[3] = { -1.428f, -1.780f, -1.194f };
 
    // 左から coxa, femur, tibia (80.32° 99.92° 101.36°)
    const float maxes[3] = { 1.402f,  1.744f,  1.769f };
 
    // ans of kinematics use solution of i_kinematics.
 
    // zは最大196.ixは最大248.
    for (int iz = 0; iz < 200; iz++) {
        for (int ix = 53; ix < 248; ix++) {
            // 脚先座標（ローカル）
            const Vector3 line_end(static_cast<float>(ix), 0.0f, static_cast<float>(iz));
 
            // 逆運動学 coxa なし.
 
            // const float _coxa_angle = atan2(line_end.x, line_end.y);  // coxa角度.
 
            // femurから足先までを結ぶベクトルをxy平面に投影したときのベクトルの大きさ.
            const float _IK_trueX = sqrt(math_util::Squared(line_end.x) +
                                         math_util::Squared(line_end.y)) -
                XrR1Const::kCoxaLength;
 
            // 絶対に正.
            float _im = sqrt(math_util::Squared(_IK_trueX) + math_util::Squared(line_end.z));
 
            if (_im == 0.0f) {
                // 0割り対策.
                _im += 0.01f;
            }
 
            // マイナスでおｋ座標系的にq1自体は常に負.xがゼロだと定義域エラー.
            const float _q1 = -atan2(line_end.z, _IK_trueX);
 
            // im = 0 だと定義域エラー.
            const float _q2 = acos(
                (math_util::Squared(XrR1Const::kFemurLength) + math_util::Squared(_im) -
                math_util::Squared(XrR1Const::kTibiaLength)) /
                (2.0f * XrR1Const::kFemurLength * _im));
 
            const float _femur_angle = _q1 + _q2;
            const float _tibia_angle = acos(
                (math_util::Squared(XrR1Const::kFemurLength) +
                math_util::Squared(XrR1Const::kTibiaLength) -
                math_util::Squared(_im)) /
                (2.0f * XrR1Const::kFemurLength * XrR1Const::kTibiaLength)) -
                std::numbers::pi_v<float> / 2.0f;
 
            // 運動学
            const float _K_trueX = XrR1Const::kFemurLength * cos(_femur_angle) +
                XrR1Const::kTibiaLength *
                cos(_femur_angle + _tibia_angle - std::numbers::pi_v<float> / 2.0f);
 
            Vector3 _kinematics;
            _kinematics.x = _K_trueX + XrR1Const::kCoxaLength;
            _kinematics.y = 0.0f;
            _kinematics.z =
                -(XrR1Const::kFemurLength * sin(_femur_angle) +
                  XrR1Const::kTibiaLength *
                  sin(_femur_angle + _tibia_angle - std::numbers::pi_v<float> / 2.0f));
 
            const float _Permission = (_kinematics - line_end).GetSquaredLength();
 
            if (PERMISSION > _Permission) {
                constexpr float kLegRom_RMargin = 2.f;
 
                // y = 0のとき,脚高さzのときのx方向の最大の範囲.
                res[iz] = static_cast<float>(ix) - kLegRom_RMargin;
 
#ifdef  MAX_LEG_RADIUS
                // 脚を置く位置が遠すぎるとトルクが足りなくて
                // 沈み込みが激しいから200までにした2020/11/09 Hato
                if (iz <= 115) {
                    res[iz] = MAX_LEG_RADIUS;
                }
#endif
            }
        }
    }
 
    return res;
}
 
std::array<Vector2, HexapodConst::kLegNum> XrR1::InitMinLegPosXY() const {
    std::array<Vector2, HexapodConst::kLegNum> res;
 
    for (int i = 0; i < HexapodConst::kLegNum; i++) {
        const float angle = kMovableCoxaAngleMin + XrR1Const::kCoxaDefaultAngle[i];
        res[i] = Vector2(cos(angle), sin(angle)).GetNormalized();
    }
 
    return res;
}
 
std::array<Vector2, HexapodConst::kLegNum> XrR1::InitMaxLegPosXY() const {
    std::array<Vector2, HexapodConst::kLegNum> res;
 
    for (int i = 0; i < HexapodConst::kLegNum; i++) {
        const float angle = kMovableCoxaAngleMax + XrR1Const::kCoxaDefaultAngle[i];
        res[i] = Vector2(cos(angle), sin(angle)).GetNormalized();
    }
 
    return res;
}
 
}  // namespace designlab