InterpolatingAnglePlanner.hpp

#ifndef KINEMATICS_INTERPOLATINGANGLEPLANNER_HPP
#define KINEMATICS_INTERPOLATINGANGLEPLANNER_HPP

#include "Planner.hpp"
#include <Kinematics/Matrix.hpp>

namespace HLR
{
namespace Kinematics
{
class InterpolatingAnglePlanner : public Planner
{
public:
    InterpolatingAnglePlanner(double step_size);

    const std::optional<std::vector<Matrix<double, 5, 1>>> get_path(
        const Matrix<double, 5, 1>& current_state,
        const Matrix<double, 3, 1>& target_pos) const override;

private:
    double step_size;
};
}
}

#endif