HLR  0.0.1
Kinematics.hpp
1 #ifndef KINEMATICS_KINEMATICS_HPP
2 #define KINEMATICS_KINEMATICS_HPP
3 
4 #include <Kinematics/Matrix.hpp>
5 
6 #include <boost/math/constants/constants.hpp>
7 
8 #include <cmath>
9 #include <optional>
10 #include <utility>
11 
12 namespace HLR
13 {
14 namespace Kinematics
15 {
21 {
22 public:
26  using IKSolution = std::pair<std::optional<Matrix<double, 5, 1>>,
27  std::optional<Matrix<double, 5, 1>>>;
28 
32  Kinematics() = delete;
33 
37  ~Kinematics() = default;
38 
44  Kinematics(const Kinematics& k) = delete;
45 
51  Kinematics(Kinematics&& k) = delete;
52 
59  Kinematics& operator=(const Kinematics& k) = delete;
60 
67  Kinematics& operator=(Kinematics&& k) = delete;
68 
78  static std::optional<Matrix<double, 3, 4>> forward(
79  Matrix<double, 5, 1> angles);
80 
91  static IKSolution jacobian_inverse(Matrix<double, 5, 1> current_angles,
92  const Matrix<double, 3, 1>& goal);
93 
104 
105 private:
112  static Matrix<double, 3, 3> compute_jacobian(
113  const Matrix<double, 5, 1>& current_angles);
114 
123  static bool angles_within_limits(const Matrix<double, 5, 1>& angles);
124 
129  static constexpr double precision = 0.0005;
130 
135  static constexpr double deviation = 0.001;
136 
141  static constexpr std::size_t max_iterations = 50000;
142 
146  static constexpr double pi = boost::math::constants::pi<double>();
147 
151  static constexpr Matrix<double, 3, 1> joint_lengths = {{0.250},
152  {0.160},
153  {0.190}};
154 
160  static constexpr Matrix<double, 5, 2> servo_limits = {{-150.0, 150.0},
161  {-60.0, 120.0},
162  {-110.0, 120.0},
163  {-90.0, 90.0},
164  {-200.0, 200.0}};
165 };
166 
167 } // namespace Kinematics
168 } // namespace HLR
169 
170 #endif
static std::optional< Matrix< double, 3, 4 > > forward(Matrix< double, 5, 1 > angles)
Calculate forward kinematics given the angles of the robot arm.
~Kinematics()=default
Default destructor.
Contains the functions to compute the forward and inverse kinematics for the mitsubishi rv-2aj robot ...
Definition: Kinematics.hpp:20
Kinematics()=delete
Object is not constructable so default constructor is deleted.
std::pair< std::optional< Matrix< double, 5, 1 >>, std::optional< Matrix< double, 5, 1 >>> IKSolution
Alias std::pair to IKSolution.
Definition: Kinematics.hpp:27
static IKSolution jacobian_inverse(Matrix< double, 5, 1 > current_angles, const Matrix< double, 3, 1 > &goal)
Calculate joint angles based on the position the end effector of the robot arm should have...
Kinematics & operator=(const Kinematics &k)=delete
Object is not constructable so copy assignment is deleted.
static IKSolution analytical_inverse(const Matrix< double, 3, 1 > &goal)
Calculate joint angles based on the position the end effector of the robot arm should have...
Create a matrix with numerical values.
Definition: Matrix.hpp:22