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