interpolated_node_creator.cpp

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#include "interpolated_node_creator.h"
 
#include "cassert_define.h"
#include "hexapod_const.h"
#include "math_util.h"
 
namespace designlab {
 
InterpolatedNodeCreator::InterpolatedNodeCreator(
    const std::shared_ptr<const IHexapodCoordinateConverter>& converter_ptr)
    : converter_ptr_(converter_ptr) {}
 
std::vector<RobotStateNode> InterpolatedNodeCreator::CreateInterpolatedNode(
    const RobotStateNode& current_node, const RobotStateNode& next_node) const {
  if (IsNoChange(current_node, next_node)) {
    return {};  // 空のvectorを返す.
  }
 
  if (IsBodyRot(current_node, next_node)) {
    return CreateBodyRotInterpolatedNode(current_node, next_node);
  }
 
  if (IsBodyMove(current_node, next_node)) {
    return CreateBodyMoveInterpolatedNode(current_node, next_node);
  }
 
  return CreateLegMoveInterpolatedNode(current_node, next_node);
}
 
bool InterpolatedNodeCreator::IsNoChange(
    const RobotStateNode& current_node, const RobotStateNode& next_node) const {
  bool res = true;
 
  for (int i = 0; i < HexapodConst::kLegNum; i++) {
    if (current_node.leg_pos[i] != next_node.leg_pos[i]) {
      res = false;
      break;
    }
  }
 
  if (current_node.center_of_mass_global_coord !=
      next_node.center_of_mass_global_coord) {
    res = false;
  }
 
  if (current_node.posture != next_node.posture) {
    res = false;
  }
 
  return res;
}
 
bool InterpolatedNodeCreator::IsBodyMove(
    const RobotStateNode& current_node, const RobotStateNode& next_node) const {
  return current_node.center_of_mass_global_coord !=
         next_node.center_of_mass_global_coord;
}
 
bool InterpolatedNodeCreator::IsBodyRot(const RobotStateNode& current_node,
                                        const RobotStateNode& next_node) const {
  return current_node.posture.GetNormalized() !=
         next_node.posture.GetNormalized();
}
 
std::vector<RobotStateNode>
InterpolatedNodeCreator::CreateBodyMoveInterpolatedNode(
    const RobotStateNode& current_node, const RobotStateNode& next_node) const {
  std::vector<RobotStateNode> res;
 
  const Vector3 dif = next_node.center_of_mass_global_coord -
                      current_node.center_of_mass_global_coord;
 
  if (dif.GetLength() < kInterpolatedDistance) {
    return {};
  }
 
  if (dif.GetLength() > kBodyMoveMaxInterpolatedDistance) {
    return {};
  }
 
  int cnt = 1;
 
  while (kInterpolatedDistance * static_cast<float>(cnt) < dif.GetLength()) {
    RobotStateNode temp_node = current_node;
 
    temp_node.ChangeGlobalCenterOfMass(
        temp_node.center_of_mass_global_coord + dif.GetNormalized() *
                                                    kInterpolatedDistance *
                                                    static_cast<float>(cnt),
        true);
 
    res.push_back(temp_node);
 
    cnt++;
  }
 
  return res;
}
 
std::vector<RobotStateNode>
InterpolatedNodeCreator::CreateBodyRotInterpolatedNode(
    const RobotStateNode& current_node, const RobotStateNode& next_node) const {
  std::vector<RobotStateNode> res;
 
  constexpr int kBodyMoveInterpolatedNodeNum =
      100;  
 
  for (int i = 0; i < kBodyMoveInterpolatedNodeNum; i++) {
    RobotStateNode temp_node = current_node;
    const float ex = (static_cast<float>(i) + 1.0f) /
                     (static_cast<float>(kBodyMoveInterpolatedNodeNum));
 
    const Quaternion quat =
        SlerpQuaternion(current_node.posture, next_node.posture, ex)
            .GetNormalized();
 
    temp_node.ChangePosture(converter_ptr_, quat);
 
    res.push_back(temp_node);
  }
 
  return res;
}
 
std::vector<RobotStateNode>
InterpolatedNodeCreator::CreateLegMoveInterpolatedNode(
    const RobotStateNode& current_node, const RobotStateNode& next_node) const {
  std::vector<RobotStateNode> res;
 
  // 接地脚の平行移動,下降動作,遊脚の上昇動作,平行移動を行う.
 
  // 接地.
  {
    std::vector<int> ground_move_index =
        GetGroundMoveIndex(current_node, next_node);
 
    // 接地脚の平行移動.
    while (true) {
      RobotStateNode temp_node = res.empty() ? current_node : res.back();
 
      bool is_end = true;
 
      for (const auto& i : ground_move_index) {
        // 角度方向に移動.
        const float angle_current =
            atan2(temp_node.leg_pos[i].y, temp_node.leg_pos[i].x);
        const float angle_next =
            atan2(next_node.leg_pos[i].y, next_node.leg_pos[i].x);
 
        if (angle_current > angle_next + kInterpolatedAngle) {
          const float length = temp_node.leg_pos[i].ProjectedXY().GetLength();
          temp_node.leg_pos[i].x =
              length * cos(angle_current - kInterpolatedAngle);
          temp_node.leg_pos[i].y =
              length * sin(angle_current - kInterpolatedAngle);
          is_end = false;
        } else if (angle_current < angle_next - kInterpolatedAngle) {
          const float length = temp_node.leg_pos[i].ProjectedXY().GetLength();
          temp_node.leg_pos[i].x =
              length * cos(angle_current + kInterpolatedAngle);
          temp_node.leg_pos[i].y =
              length * sin(angle_current + kInterpolatedAngle);
          is_end = false;
        } else {
          const float length = temp_node.leg_pos[i].ProjectedXY().GetLength();
          temp_node.leg_pos[i].x = length * cos(angle_next);
          temp_node.leg_pos[i].y = length * sin(angle_next);
        }
 
        // 半径方向に移動.
        const float length_current =
            temp_node.leg_pos[i].ProjectedXY().GetLength();
        const float length_next =
            next_node.leg_pos[i].ProjectedXY().GetLength();
 
        if (length_current > length_next + kInterpolatedDistance) {
          const float angle =
              atan2(temp_node.leg_pos[i].y, temp_node.leg_pos[i].x);
          temp_node.leg_pos[i].x =
              (length_current - kInterpolatedDistance) * cos(angle);
          temp_node.leg_pos[i].y =
              (length_current - kInterpolatedDistance) * sin(angle);
          is_end = false;
        } else if (length_current < length_next - kInterpolatedDistance) {
          const float angle =
              atan2(temp_node.leg_pos[i].y, temp_node.leg_pos[i].x);
          temp_node.leg_pos[i].x =
              (length_current + kInterpolatedDistance) * cos(angle);
          temp_node.leg_pos[i].y =
              (length_current + kInterpolatedDistance) * sin(angle);
          is_end = false;
        } else {
          const float angle =
              atan2(temp_node.leg_pos[i].y, temp_node.leg_pos[i].x);
          temp_node.leg_pos[i].x = length_next * cos(angle);
          temp_node.leg_pos[i].y = length_next * sin(angle);
        }
      }
 
      res.push_back(temp_node);
 
      if (is_end) {
        break;
      }
    }
 
    // 接地脚の下降.
    // 全ての脚先を kInterpolatedDistance づつ下げていき,距離が
    // kInterpolatedDistance 以下になったら next_node と同じz座標にする.
    while (true) {
      RobotStateNode temp_node = res.back();
 
      bool is_end = true;
 
      for (const auto& i : ground_move_index) {
        if (temp_node.leg_pos[i].z >
            next_node.leg_pos[i].z + kInterpolatedDistance) {
          temp_node.leg_pos[i].z -= kInterpolatedDistance;
          is_end = false;
        } else if (temp_node.leg_pos[i].z <
                   next_node.leg_pos[i].z - kInterpolatedDistance) {
          temp_node.leg_pos[i].z += kInterpolatedDistance;
          is_end = false;
        } else {
          temp_node.leg_pos[i].z = next_node.leg_pos[i].z;
          leg_func::ChangeGround(i, true, &temp_node.leg_state);
        }
      }
 
      res.push_back(temp_node);
 
      if (is_end) {
        break;
      }
    }
  }
 
  // 遊脚.
  {
    std::vector<int> free_move_index =
        GetFreeMoveIndex(current_node, next_node);
 
    // 接地脚の下降.
    // 全ての脚先を kInterpolatedDistance づつ下げていき,距離が
    // kInterpolatedDistance 以下になったら next_node と同じz座標にする.
    while (true) {
      RobotStateNode temp_node = res.back();
 
      bool is_end = true;
 
      for (const auto& i : free_move_index) {
        if (temp_node.leg_pos[i].z >
            next_node.leg_pos[i].z + kInterpolatedDistance) {
          temp_node.leg_pos[i].z -= kInterpolatedDistance;
          is_end = false;
        } else if (temp_node.leg_pos[i].z <
                   next_node.leg_pos[i].z - kInterpolatedDistance) {
          temp_node.leg_pos[i].z += kInterpolatedDistance;
          is_end = false;
        } else {
          temp_node.leg_pos[i].z = next_node.leg_pos[i].z;
          leg_func::ChangeGround(i, false, &temp_node.leg_state);
        }
      }
 
      res.push_back(temp_node);
 
      if (is_end) {
        break;
      }
    }
 
    // 接地脚の平行移動.
    while (true) {
      RobotStateNode temp_node = res.empty() ? current_node : res.back();
 
      bool is_end = true;
 
      for (const auto& i : free_move_index) {
        // 角度方向に移動.
        const float angle_current =
            atan2(temp_node.leg_pos[i].y, temp_node.leg_pos[i].x);
        const float angle_next =
            atan2(next_node.leg_pos[i].y, next_node.leg_pos[i].x);
 
        if (angle_current > angle_next + kInterpolatedAngle) {
          const float length = temp_node.leg_pos[i].ProjectedXY().GetLength();
          temp_node.leg_pos[i].x =
              length * cos(angle_current - kInterpolatedAngle);
          temp_node.leg_pos[i].y =
              length * sin(angle_current - kInterpolatedAngle);
          is_end = false;
        } else if (angle_current < angle_next - kInterpolatedAngle) {
          const float length = temp_node.leg_pos[i].ProjectedXY().GetLength();
          temp_node.leg_pos[i].x =
              length * cos(angle_current + kInterpolatedAngle);
          temp_node.leg_pos[i].y =
              length * sin(angle_current + kInterpolatedAngle);
          is_end = false;
        } else {
          const float length = temp_node.leg_pos[i].ProjectedXY().GetLength();
          temp_node.leg_pos[i].x = length * cos(angle_next);
          temp_node.leg_pos[i].y = length * sin(angle_next);
        }
 
        // 半径方向に移動.
        const float length_current =
            temp_node.leg_pos[i].ProjectedXY().GetLength();
        const float length_next =
            next_node.leg_pos[i].ProjectedXY().GetLength();
 
        if (length_current > length_next + kInterpolatedDistance) {
          const float angle =
              atan2(temp_node.leg_pos[i].y, temp_node.leg_pos[i].x);
          temp_node.leg_pos[i].x =
              (length_current - kInterpolatedDistance) * cos(angle);
          temp_node.leg_pos[i].y =
              (length_current - kInterpolatedDistance) * sin(angle);
          is_end = false;
        } else if (length_current < length_next - kInterpolatedDistance) {
          const float angle =
              atan2(temp_node.leg_pos[i].y, temp_node.leg_pos[i].x);
          temp_node.leg_pos[i].x =
              (length_current + kInterpolatedDistance) * cos(angle);
          temp_node.leg_pos[i].y =
              (length_current + kInterpolatedDistance) * sin(angle);
          is_end = false;
        } else {
          const float angle =
              atan2(temp_node.leg_pos[i].y, temp_node.leg_pos[i].x);
          temp_node.leg_pos[i].x = length_next * cos(angle);
          temp_node.leg_pos[i].y = length_next * sin(angle);
        }
      }
 
      res.push_back(temp_node);
 
      if (is_end) {
        break;
      }
    }
  }
 
  return res;
}
 
std::vector<int> InterpolatedNodeCreator::GetGroundMoveIndex(
    const RobotStateNode& current_node, const RobotStateNode& next_node) const {
  // 脚先座標の差分を計算.
  std::array<Vector3, HexapodConst::kLegNum> dif;
 
  for (int i = 0; i < HexapodConst::kLegNum; i++) {
    dif[i] = next_node.leg_pos[i] - current_node.leg_pos[i];
  }
 
  // indexを返す.
  std::vector<int> res_index;
 
  for (int i = 0; i < HexapodConst::kLegNum; i++) {
    if (dif[i].z < 0) {
      res_index.push_back(i);
    }
  }
 
  return res_index;
}
 
std::vector<int> InterpolatedNodeCreator::GetFreeMoveIndex(
    const RobotStateNode& current_node, const RobotStateNode& next_node) const {
  // 脚先座標の差分を計算.
  std::array<Vector3, HexapodConst::kLegNum> dif;
 
  for (int i = 0; i < HexapodConst::kLegNum; i++) {
    dif[i] = next_node.leg_pos[i] - current_node.leg_pos[i];
  }
 
  // indexを返す.
  std::vector<int> res_index;
 
  for (int i = 0; i < HexapodConst::kLegNum; i++) {
    if (dif[i].z > 0) {
      res_index.push_back(i);
    }
  }
 
  return res_index;
}
 
}  // namespace designlab