robot_state_node.cpp

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#include "robot_state_node.h"
 
#include <iostream>
#include <sstream>
#include <vector>
 
#include "leg_state.h"
#include "math_util.h"
#include "string_util.h"
 
namespace designlab {
 
void RobotStateNode::ChangeGlobalCenterOfMass(const Vector3& new_com,
                                              const bool base) {
  const Vector3 dif = new_com - center_of_mass_global_coord;
  const Vector3 com_dif = RotateVector3(dif, posture.GetConjugate());
 
  center_of_mass_global_coord = new_com;
 
  for (int i = 0; i < HexapodConst::kLegNum; i++) {
    if (leg_func::IsGrounded(leg_state, i)) {
      leg_pos[i] -= com_dif;
      leg_reference_pos[i] = leg_pos[i];
    } else {
      if (base) leg_reference_pos[i] -= com_dif;
    }
  }
}
 
void RobotStateNode::ChangePosture(
    const std::shared_ptr<const IHexapodCoordinateConverter>& converter_ptr,
    const Quaternion& new_posture) {
  const Quaternion dif = new_posture * posture.GetConjugate();
 
  posture = new_posture;
 
  for (int i = 0; i < HexapodConst::kLegNum; i++) {
    if (leg_func::IsGrounded(leg_state, i)) {
      Vector3 leg_pos_robot_coord =
          converter_ptr->ConvertLegToRobotCoordinate(leg_pos[i], i);
      leg_pos_robot_coord =
          RotateVector3(leg_pos_robot_coord, dif.GetConjugate());
      leg_pos[i] =
          converter_ptr->ConvertRobotToLegCoordinate(leg_pos_robot_coord, i);
 
      Vector3 leg_reference_pos_robot_coord =
          converter_ptr->ConvertLegToRobotCoordinate(leg_reference_pos[i], i);
      leg_reference_pos_robot_coord =
          RotateVector3(leg_reference_pos_robot_coord, dif.GetConjugate());
      leg_reference_pos[i] = converter_ptr->ConvertRobotToLegCoordinate(
          leg_reference_pos_robot_coord, i);
    }
  }
}
 
std::string RobotStateNode::ToString() const {
  // \t はタブを表す文字.また,スペースのみを追加したい場合は std::string(" ")
  // とする.
 
  std::string res;  // 結果として返す文字列.
 
  // 脚状態のビット列.
  res += "Leg State Bit : " + leg_state.to_string() + "\n";
  res += "\n";
 
  // 重心位置の出力.
  enums::DiscreteComPos com = leg_func::GetDiscreteComPos(leg_state);
  res += "Com Pattern : " + string_util::EnumToStringRemoveTopK(com) + "(" +
         std::to_string(static_cast<int>(com)) + ")\n";
 
  // 脚の接地状態.
  res += "Ground : ";
 
  for (int i = 0; i < HexapodConst::kLegNum; ++i) {
    res += (leg_func::IsGrounded(leg_state, i) ? "ground" : "lifted") +
           std::string(" ");
  }
 
  res += "\n";
 
  // 脚の階層.
  res += "Hierarchy : ";
 
  for (int i = 0; i < HexapodConst::kLegNum; ++i) {
    DiscreteLegPos dis_leg_pos = leg_func::GetDiscreteLegPos(leg_state, i);
    res += string_util::EnumToStringRemoveTopK(dis_leg_pos) + "(" +
           std::to_string(static_cast<int>(dis_leg_pos)) + ") ";
  }
 
  res += "\n\n";
 
  // 脚位置.
  res += "Leg Position : \n";
 
  for (int i = 0; i < HexapodConst::kLegNum; i++) {
    res += "  " + std::to_string(i) + ":" + leg_pos[i].ToString() + "\n";
  }
 
  res += "Leg Base Position : \n";
 
  for (int i = 0; i < HexapodConst::kLegNum; i++) {
    res +=
        "  " + std::to_string(i) + ":" + leg_reference_pos[i].ToString() + "\n";
  }
 
  // 重心位置.
  res += "\nGlobal Center of Mass : " + center_of_mass_global_coord.ToString() +
         "\n";
 
  // 回転姿勢.
  res += "Quaternion  : " + posture.ToString() + "\n";
 
  // 次動作.
  res += "\n";
  res += "(Next Move : " + string_util::EnumToStringRemoveTopK(next_move) +
         std::string(")\n");
  res += "(Depth : " + std::to_string(depth) + std::string(")\n");
  res +=
      "(parent number : " + std::to_string(parent_index) + std::string(")\n");
 
  return res;
}
 
std::string RobotStateNode::ToCsvString() const {
  std::stringstream ss;
 
  ss << *this;
 
  return ss.str();
}
 
RobotStateNode RobotStateNode::FromString(const std::string& str) {
  std::vector<std::string> data = string_util::Split(str, ",");
 
  RobotStateNode res;
  int cnt = 0;
 
  try {
    // 脚状態のビット列.
    res.leg_state = std::bitset<leg_func::kLegStateBitNum>(data[cnt++]);
 
    // 脚の位置.
    for (int i = 0; i < HexapodConst::kLegNum; i++) {
      res.leg_pos[i] = Vector3{std::stof(data[cnt++]), std::stof(data[cnt++]),
                               std::stof(data[cnt++])};
    }
 
    // 脚の基準位置.
    for (int i = 0; i < HexapodConst::kLegNum; i++) {
      res.leg_reference_pos[i] =
          Vector3{std::stof(data[cnt++]), std::stof(data[cnt++]),
                  std::stof(data[cnt++])};
    }
 
    // 重心位置.
    res.center_of_mass_global_coord = Vector3{
        std::stof(data[cnt++]), std::stof(data[cnt++]), std::stof(data[cnt++])};
 
    // 回転姿勢.
    res.posture = Quaternion{std::stof(data[cnt++]), std::stof(data[cnt++]),
                             std::stof(data[cnt++]), std::stof(data[cnt++])};
 
    // 次動作.
    res.next_move = magic_enum::enum_cast<HexapodMove>(data[cnt++]).value();
 
    // 親ノードの番号.
    res.parent_index = std::stoi(data[cnt++]);
 
    // 深さ.
    res.depth = std::stoi(data[cnt++]);
  } catch (...) {
  }
 
  return res;
}
 
}  // namespace designlab