Kinematics.hpp

#ifndef KINEMATICS_KINEMATICS_HPP
#define KINEMATICS_KINEMATICS_HPP

#include <Kinematics/Matrix.hpp>

#include <boost/math/constants/constants.hpp>

#include <cmath>
#include <optional>
#include <utility>

namespace HLR
{
namespace Kinematics
{
class Kinematics
{
public:
    using IKSolution = std::pair<std::optional<Matrix<double, 5, 1>>,
                                 std::optional<Matrix<double, 5, 1>>>;

    Kinematics() = delete;

    ~Kinematics() = default;

    Kinematics(const Kinematics& k) = delete;

    Kinematics(Kinematics&& k) = delete;

    Kinematics& operator=(const Kinematics& k) = delete;

    Kinematics& operator=(Kinematics&& k) = delete;

    static std::optional<Matrix<double, 3, 4>> forward(
        Matrix<double, 5, 1> angles);

    static IKSolution jacobian_inverse(Matrix<double, 5, 1> current_angles,
                                       const Matrix<double, 3, 1>& goal);

    static IKSolution analytical_inverse(const Matrix<double, 3, 1>& goal);

private:
    static Matrix<double, 3, 3> compute_jacobian(
        const Matrix<double, 5, 1>& current_angles);

    static bool angles_within_limits(const Matrix<double, 5, 1>& angles);

    static constexpr double precision = 0.0005;

    static constexpr double deviation = 0.001;

    static constexpr std::size_t max_iterations = 50000;

    static constexpr double pi = boost::math::constants::pi<double>();

    static constexpr Matrix<double, 3, 1> joint_lengths = {{0.250},
                                                           {0.160},
                                                           {0.072}};

    static constexpr Matrix<double, 5, 2> servo_limits = {{-150.0, 150.0},
                                                          {-60.0, 120.0},
                                                          {-110.0, 120.0},
                                                          {-90.0, 90.0},
                                                          {-200.0, 200.0}};
};

} // namespace Kinematics
} // namespace HLR

#endif