Line data    Source code

       1             : #include "Kinematics.hpp"
       2             : #include <Kinematics/Matrix.hpp>
       3             : 
       4             : #include "InterpolatingEuclideanPlanner.hpp"
       5             : 
       6             : #include <cmath>
       7             : 
       8             : namespace HLR
       9             : {
      10             : namespace Kinematics
      11             : {
      12           1 : InterpolatingEuclideanPlanner::InterpolatingEuclideanPlanner(double step_size)
      13           1 :     : step_size(step_size)
      14           1 : {}
      15             : 
      16             : const std::optional<std::vector<Matrix<double, 5, 1>>>
      17           2 : InterpolatingEuclideanPlanner::get_path(
      18             :     const Matrix<double, 5, 1>& current_state,
      19             :     const Matrix<double, 3, 1>& target_pos) const
      20             : {
      21           2 :     const auto current_pos_opt = Kinematics::forward(current_state);
      22           2 :     if (!current_pos_opt.has_value())
      23             :     {
      24           1 :         return std::nullopt;
      25             :     }
      26             : 
      27             :     const auto current_pos =
      28           1 :         current_pos_opt.value().template slice<0, 3, 3, 1>();
      29             : 
      30           2 :     std::vector<Matrix<double, 5, 1>> path;
      31             : 
      32           1 :     const auto delta = target_pos - current_pos;
      33           1 :     const auto magnitude = delta.get_magnitude();
      34           1 :     const auto steps_to_take =
      35           1 :         static_cast<std::size_t>(magnitude / this->step_size);
      36             : 
      37           1 :     Matrix<double, 5, 1> previous_state;
      38           1 :     for (std::size_t i = 0; i <= steps_to_take; ++i)
      39             :     {
      40           1 :         Matrix<double, 3, 1> current_node(target_pos);
      41           1 :         if (i < steps_to_take)
      42             :         {
      43           1 :             auto j = static_cast<double>(i + 1);
      44           1 :             current_node[0][0] =
      45           1 :                 current_pos[0][0]
      46           1 :                 + ((j / magnitude) * (target_pos[0][0] - current_pos[0][0]));
      47           1 :             current_node[1][0] =
      48           1 :                 current_pos[1][0]
      49           1 :                 + ((j / magnitude) * (target_pos[1][0] - current_pos[1][0]));
      50           1 :             current_node[2][0] =
      51           1 :                 current_pos[2][0]
      52           1 :                 + ((j / magnitude) * (target_pos[2][0] - current_pos[2][0]));
      53             :         }
      54             : 
      55             :         const auto target_solutions =
      56           1 :             Kinematics::analytical_inverse(current_node);
      57           1 :         Matrix<double, 5, 1> solution;
      58           2 :         if (!target_solutions.first.has_value()
      59           1 :             && !target_solutions.second.has_value())
      60             :         {
      61           1 :             return std::nullopt;
      62             :         }
      63             : 
      64           0 :         if (target_solutions.first.has_value()
      65           0 :             && target_solutions.second.has_value())
      66             :         {
      67           0 :             const auto& s1 = target_solutions.first.value();
      68           0 :             const auto& s2 = target_solutions.second.value();
      69           0 :             const auto m1 = (s1 - previous_state).get_magnitude();
      70           0 :             const auto m2 = (s2 - previous_state).get_magnitude();
      71           0 :             solution = m1 > m2 ? s2 : s1;
      72             :         }
      73             :         else
      74             :         {
      75           0 :             solution = target_solutions.first.has_value()
      76           0 :                            ? target_solutions.first.value()
      77             :                            : target_solutions.second.value();
      78             :         }
      79             : 
      80           0 :         path.emplace_back(solution);
      81           0 :         previous_state = solution;
      82             :     }
      83             : 
      84           0 :     return path;
      85             : }
      86             : }
      87           3 : }