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             : namespace Planners
      13             : {
      14           2 : InterpolatingAnglePlanner::InterpolatingAnglePlanner(double max_step_size)
      15           2 :     : max_step_size(max_step_size)
      16           2 : {}
      17             : 
      18             : const std::optional<std::vector<Matrix<double, 5, 1>>>
      19           3 : InterpolatingAnglePlanner::get_path(
      20             :     const Matrix<double, 5, 1>& current_state,
      21             :     const Matrix<double, 3, 1>& target_pos) const
      22             : {
      23           3 :     if (!Kinematics::forward(current_state).has_value())
      24             :     {
      25           1 :         return std::nullopt;
      26             :     }
      27             : 
      28           2 :     const auto target_solution = Kinematics::analytical_inverse(target_pos);
      29           2 :     const auto& target_state_opt = target_solution.first.has_value()
      30             :                                        ? target_solution.first
      31           2 :                                        : target_solution.second;
      32           2 :     if (!target_state_opt.has_value())
      33             :     {
      34           1 :         return std::nullopt;
      35             :     }
      36             : 
      37           1 :     const auto& target_state = target_state_opt.value();
      38             : 
      39             :     // Get the difference between target and current state
      40             :     // and scale the step size according to the greatest delta.
      41           1 :     const auto delta = target_state - current_state;
      42           1 :     double greatest_delta = delta[0][0];
      43           5 :     for (std::size_t i = 1; i < delta.get_m(); ++i)
      44             :     {
      45           4 :         greatest_delta =
      46           4 :             greatest_delta > delta[i][0] ? greatest_delta : delta[i][0];
      47             :     }
      48             : 
      49           1 :     Matrix<double, 5, 1> step(delta);
      50           6 :     for (std::size_t i = 0; i < step.get_m(); ++i)
      51             :     {
      52           5 :         step[i][0] = delta[i][0] / greatest_delta * max_step_size;
      53             :     }
      54             : 
      55           2 :     std::vector<Matrix<double, 5, 1>> path;
      56             : 
      57           1 :     Matrix<double, 5, 1> intermediate(current_state);
      58             : 
      59             :     const auto steps_to_take =
      60           1 :         static_cast<std::size_t>(std::abs(greatest_delta / max_step_size));
      61         942 :     for (std::size_t i = 0; i < steps_to_take; ++i)
      62             :     {
      63         941 :         intermediate += step;
      64         941 :         path.emplace_back(intermediate);
      65             :     }
      66             : 
      67           1 :     path.emplace_back(target_state);
      68             : 
      69           1 :     return path;
      70             : }
      71             : }
      72             : }
      73           3 : }