У меня есть модель манипулятора с 6 степенями свободы:
Я хочу рассчитать прямую кинематику, поэтому я использую матрицу D-H. параметры D-H:
static const std::vector<float> theta = {
0,0,90.0f,0,-90.0f,0};
// d
static const std::vector<float> d = {
380.948f,0,0,-560.18f,0,0};
// a
static const std::vector<float> a = {
-220.0f,522.331f,80.0f,0,0,94.77f};
// alpha
static const std::vector<float> alpha = {
90.0f,0,90.0f,-90.0f,-90.0f,0};
и расчет:
glm::mat4 Robothand::armForKinematics() noexcept
{
glm::mat4 pose(1.0f);
float cos_theta, sin_theta, cos_alpha, sin_alpha;
for (auto i = 0; i < 6;i++)
{
cos_theta = cosf(glm::radians(theta[i]));
sin_theta = sinf(glm::radians(theta[i]));
cos_alpha = cosf(glm::radians(alpha[i]));
sin_alpha = sinf(glm::radians(alpha[i]));
glm::mat4 Ai = {
cos_theta, -sin_theta * cos_alpha,sin_theta * sin_alpha, a[i] * cos_theta,
sin_theta, cos_theta * cos_alpha, -cos_theta * sin_alpha,a[i] * sin_theta,
0, sin_alpha, cos_alpha, d[i],
0, 0, 0, 1 };
pose = pose * Ai;
}
return pose;
}
у меня проблема в том, что я не могу получить правильный результат, например, я хочу вычислить матрицу преобразования от первого сустава до четвертого сустава, я изменю цикл for i ‹3, тогда я могу получить позу матрицу, и я могу исходную координату в 4-й системе координат с помощью позы * (0,0,0,1). но результат (380.948,382.331,0) кажется неправильным, потому что он должен перемещаться по оси x, а не y- ось. Я прочитал много книг и материалов о матрице D-H, но не могу понять, что с ней не так.