robot_operator_for_path.cpp

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#include "robot_operator_for_path.h"
 
#include <numbers>
 
#include "math_rot_converter.h"
 
 
namespace
{
 
// -π～πの範囲に収める．
float NormalizeAngle(float angle)
{
    while (angle > std::numbers::pi_v<float>)
    {
        angle -= 2 * std::numbers::pi_v<float>;
    }
 
    while (angle < -std::numbers::pi_v<float>)
    {
        angle += 2 * std::numbers::pi_v<float>;
    }
 
    return angle;
}
 
}  // namespace
 
 
namespace designlab
{
 
RobotOperatorForPath::RobotOperatorForPath(const std::vector<Vector3>& path) :
    global_route_(path),
    most_near_index_(0)
{
}
 
RobotOperation RobotOperatorForPath::Init() const
{
    RobotOperation operation;
 
    operation.operation_type = RobotOperationType::kStraightMoveVector;
    operation.straight_move_vector = Vector3::GetFrontVec();
 
    return operation;
}
 
RobotOperation RobotOperatorForPath::Update(const RobotStateNode& node)
{
    using enum RobotOperationType;
 
    if ((global_route_[most_near_index_].ProjectedXY() - node.center_of_mass_global_coord.ProjectedXY()).GetLength() < 150.f)
    {
        most_near_index_++;
    }
 
    if (most_near_index_ > 0 &&
        (global_route_[most_near_index_ - 1].ProjectedXY() - node.center_of_mass_global_coord.ProjectedXY()).GetLength() < 150.f)
    {
        // 旋回によって，most_near_index_の方を向く．
        const auto diff = global_route_[most_near_index_].ProjectedXY() -
            node.center_of_mass_global_coord.ProjectedXY();
 
        const float angle = atan2(diff.y, diff.x);
        const float euler_z_angle = ToEulerXYZ(node.posture).z_angle;
        const float rot_dif = NormalizeAngle(angle - euler_z_angle);
 
        if (abs(rot_dif) > kAllowableAngleError)
        {
            RobotOperation operation;
            operation.operation_type = kSpotTurnLastPosture;
            operation.spot_turn_last_posture = Quaternion::MakeByAngleAxis(angle, Vector3::GetUpVec());
 
            return operation;
        }
    }
 
    //if (most_near_index_ >= global_route_.size())
    //{
    //    most_near_index_ = 0;
    //}
 
    //const auto diff = global_route_[most_near_index_].ProjectedXY() -
    //    node.center_of_mass_global_coord.ProjectedXY();
 
    //const float angle = atan2(diff.y, diff.x);
    //const float euler_z_angle = NormalizeAngle(ToEulerXYZ(node.posture).z_angle);
    //const float rot_dif = angle - euler_z_angle;
 
    //if (abs(rot_dif) > kAllowableAngleError)
    //{
    //    RobotOperation operation;
    //    operation.operation_type = kSpotTurnLastPosture;
    //    operation.spot_turn_last_posture_ = Quaternion::MakeByAngleAxis(angle, Vector3::GetUpVec());
 
    //    return operation;
    //}
 
    RobotOperation operation_straight;
 
    operation_straight.operation_type = kStraightMoveVector;
 
    if (most_near_index_ == 0)
    {
        operation_straight.straight_move_vector = (global_route_[most_near_index_] - node.center_of_mass_global_coord).GetNormalized();
    }
    else
    {
        operation_straight.straight_move_vector = (global_route_[most_near_index_] - global_route_[most_near_index_ - 1]).GetNormalized();
    }
 
    return operation_straight;
}
 
}  // namespace designlab