simulation_end_checker_by_posture.cpp

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#include "simulation_end_checker_by_posture.h"
 
#include <numbers>
 
#include "math_rot_converter.h"
 
namespace designlab {
 
SimulationEndCheckerByPosture::SimulationEndCheckerByPosture(
    const Quaternion& goal_orientation, const float allowable_error)
    : goal_orientation_(goal_orientation),
      goal_euler_(ToEulerXYZ(goal_orientation)),
      allowable_error_(allowable_error) {}
 
bool SimulationEndCheckerByPosture::IsEnd(const RobotStateNode& node) const {
  // 角度を取得し,誤差を計算.
  auto now = ToEulerXYZ(node.posture);
 
  float error_x = std::abs(now.x_angle - goal_euler_.x_angle);
  error_x = (std::min)(error_x, 2 * std::numbers::pi_v<float> - error_x);
 
  if (error_x > allowable_error_) {
    return false;
  }
 
  float error_y = std::abs(now.y_angle - goal_euler_.y_angle);
  error_y = (std::min)(error_y, 2 * std::numbers::pi_v<float> - error_y);
 
  if (error_y > allowable_error_) {
    return false;
  }
 
  float error_z = std::abs(now.z_angle - goal_euler_.z_angle);
  error_z = (std::min)(error_z, 2 * std::numbers::pi_v<float> - error_z);
 
  if (error_z > allowable_error_) {
    return false;
  }
 
  return true;
}
 
}  // namespace designlab