node_initializer.cpp

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#include "node_initializer.h"
 
#include "divided_map_state.h"
#include "leg_state.h"
#include "math_rot_converter.h"
#include "math_util.h"
#include "phantomx_mk2_const.h"
 
namespace designlab {
 
NodeInitializer::NodeInitializer(const Vector3& pos, const EulerXYZ& posture,
                                 HexapodMove move)
    : pos_(pos), posture_(posture), move_(move) {}
 
RobotStateNode NodeInitializer::InitNode() const {
  RobotStateNode res;
 
  // 脚状態.
  res.leg_state = leg_func::MakeLegStateBit(
      enums::DiscreteComPos::kCenterBack, {true, true, true, true, true, true},
      {DiscreteLegPos::kCenter, DiscreteLegPos::kCenter,
       DiscreteLegPos::kCenter, DiscreteLegPos::kCenter,
       DiscreteLegPos::kCenter, DiscreteLegPos::kCenter});
 
  const float base_z = 0.0f;  // 地面のZ座標.
  [[maybe_unused]] const float com_z =
      pos_.z + base_z;  // ロボットの重心のZ座標.
 
  for (int i = 0; i < HexapodConst::kLegNum; i++) {
    res.leg_pos[i] = res.leg_reference_pos[i] = {
        160.f * cos(PhantomXMkIIConst::kCoxaDefaultAngle[i]),
        160.f * sin(PhantomXMkIIConst::kCoxaDefaultAngle[i]), -com_z};
  }
 
  res.center_of_mass_global_coord = pos_;
 
  // 座標をランダマイズする.
  // res.center_of_mass_global_coord.x +=
  //    math_util::GenerateRandomNumber(-30.f, 30.f);
  // res.center_of_mass_global_coord.y +=
  //    math_util::GenerateRandomNumber(-30.f, 30.f);
 
  // 15 deg
  // res.leg_pos[0].z -= 20.0f;
  // res.leg_pos[5].z -= 20.0f;
  // res.leg_pos[1].z -= 80.0f;
  // res.leg_pos[4].z -= 80.0f;
  // res.leg_pos[2].z -= 135.0f;
  // res.leg_pos[3].z -= 135.0f;
 
  // 10 deg
  // res.leg_pos[0].z -= 32.0f;
  // res.leg_pos[5].z -= 32.0f;
  // res.leg_pos[1].z -= 74.0f;
  // res.leg_pos[4].z -= 74.0f;
  // res.leg_pos[2].z -= 116.0f;
  // res.leg_pos[3].z -= 116.0f;
 
  // 5 deg
  // res.leg_pos[0].z -= 10.0f;
  // res.leg_pos[5].z -= 10.0f;
  // res.leg_pos[1].z -= 34.0f;
  // res.leg_pos[4].z -= 34.0f;
  // res.leg_pos[2].z -= 57.0f;
  // res.leg_pos[3].z -= 57.0f;
 
  // ロールピッチヨーで回転を表現する.ロボットの重心を中心にして回転する.
  const auto posture = EulerXYZ{math_util::ConvertDegToRad(posture_.x_angle),
                                math_util::ConvertDegToRad(posture_.y_angle),
                                math_util::ConvertDegToRad(posture_.z_angle)};
 
  res.posture = ToQuaternion(posture);
 
  res.next_move = move_;
  res.parent_index = -1;
  res.depth = 0;
 
  return res;
}
 
RobotStateNode NodeInitializer::InitNodeForTerrain(const RobotStateNode& node,
                                                   const MapState map) const {
  DividedMapState divided_map_state;
  divided_map_state.Init(map, node.center_of_mass_global_coord);
 
  // ロボットの重心
  const float map_z = divided_map_state.GetTopZ(
      divided_map_state.GetDividedMapIndexX(node.center_of_mass_global_coord.x),
      divided_map_state.GetDividedMapIndexY(
          node.center_of_mass_global_coord.y));
 
  RobotStateNode res = node;
  res.ChangeGlobalCenterOfMass(
      Vector3(node.center_of_mass_global_coord.x,
              node.center_of_mass_global_coord.y, map_z + 30),
      true);
 
  return res;
}
 
}  // namespace designlab