Line data    Source code

       1             : #include "Kinematics.hpp"
       2             : #include <Kinematics/Matrix.hpp>
       3             : 
       4             : #include "InterpolatingAnglePlanner.hpp"
       5             : 
       6             : #include <cmath>
       7             : 
       8             : namespace HLR
       9             : {
      10             : namespace Kinematics
      11             : {
      12           2 : InterpolatingAnglePlanner::InterpolatingAnglePlanner(double step_size)
      13           2 :     : step_size(step_size)
      14           2 : {}
      15             : 
      16             : const std::optional<std::vector<Matrix<double, 5, 1>>>
      17           3 : InterpolatingAnglePlanner::get_path(
      18             :     const Matrix<double, 5, 1>& current_state,
      19             :     const Matrix<double, 3, 1>& target_pos) const
      20             : {
      21           3 :     if (!Kinematics::forward(current_state).has_value())
      22             :     {
      23           1 :         return std::nullopt;
      24             :     }
      25             : 
      26           2 :     const auto target_solution = Kinematics::analytical_inverse(target_pos);
      27           2 :     const auto& target_state_opt = target_solution.first.has_value()
      28             :                                        ? target_solution.first
      29           2 :                                        : target_solution.second;
      30           2 :     if (!target_state_opt.has_value())
      31             :     {
      32           1 :         return std::nullopt;
      33             :     }
      34             : 
      35           1 :     const auto& target_state = target_state_opt.value();
      36             : 
      37           2 :     std::vector<Matrix<double, 5, 1>> path;
      38             : 
      39           1 :     Matrix<double, 5, 1> intermediate(current_state);
      40             : 
      41        1997 :     while (intermediate != target_state)
      42             :     {
      43        5988 :         for (std::size_t i = 0; i < current_state.get_m(); ++i)
      44             :         {
      45             :             const double sign =
      46        4990 :                 target_state[i][0] - intermediate[i][0] < 0 ? -1 : 1;
      47        9980 :             if (std::abs(target_state[i][0] - intermediate[i][0])
      48        4990 :                 > this->step_size)
      49             :             {
      50        2044 :                 intermediate[i][0] += sign * this->step_size;
      51             :             }
      52        2946 :             else if (intermediate[i][0] != target_state[i][0])
      53             :             {
      54           4 :                 intermediate[i][0] = target_state[i][0];
      55             :             }
      56             :         }
      57         998 :         path.emplace_back(intermediate);
      58             :     }
      59             : 
      60           1 :     return path;
      61             : }
      62             : }
      63           3 : }