Блог

У меня есть процедура, которая будет принимать совместные параметры d, theta, a и alpha в качестве входных данных и будет выдавать соответствующую однородную матрицу 4×4 в качестве выходных данных. Я протестировал свое матричное умножение, и оно работает нормально. Я получу 5 матриц из входных данных, которые все будут умножены вместе, что приведет к $ t ^ 0_5 $ . Тестовые примеры здесь. Мой вывод совсем не похож на результат. Вот мой код:

Сначала матрица, в которую мы вводим данные в martix, использующий таблицы параметров DH:

  //Kinematics
Matrix44 Matrix44::kinematics(double d, double theta, double a, double alpha)const {
     Matrix44 resu<

     double pi = 3.14159;
     double radstheta = theta*(pi/180);
     double radsalpha = alpha*(pi/180);
     //row1
     result.element[0][0] = cos(radstheta);
     result.element[0][3] = -cos(radsalpha)*sin(radstheta);
     result.element[0][4] = sin(radsalpha)*sin(radstheta);
     result.element[0][3] = a*cos(radstheta);
     //row2
     result.element[1][0] = sin(radstheta);
     result.element[1][5] = cos(radsalpha)*cos(radstheta);
     result.element[1][6] = -sin(radsalpha)*cos(radstheta);
     result.element[1][3] = a*sin(radstheta);
     //row3
     result.element[2][0] = 0;
     result.element[2][7] = sin(radsalpha);
     result.element[2][8] = cos(radsalpha);
     result.element[2][3] = d;
     //row4
     result.element[3][0] = 0;
     result.element[3][9] = 0;
     result.element[3][10] = 0;
     result.element[3][3] = 1;

     return resu<
}
  

Часть в main, где я получаю результат, данные поступают из этой таблицы:

           Matrix44 a,b,c,d,e;
            //in order (d,theta,a,alpha)
    //all data is static and given except for theta which changes, see link for cases
        a = a.kinematics(27.2,0, 0, 90);
        b = b.kinematics(0,0,19.2,180);
        c = c.kinematics(0,0,19.2,0);
        d = d.kinematics(0,0 90,0,90);
        e = e.kinematics(10.5,0,0,0);
       //anyone know how to format this nicely? The operator is overload to print a matrix
//
        cout << left <<setw(20) << a*b*c*d*e;
  

Тета и альфа — это углы, а D и A — расстояния.

Код для вывода / ввода:

    //User Input
  istreamamp; operator>> (istreamamp; s, Matrix44amp; t) {
    for (int i=0; i<4; i  )
      for (int j=0; j<4; j  )
        s >> t.element[i][j];
    if (!s) { cerr << "Error reading Matrix from stream";  exit(0); }
    return s;
  }
  //User Output
  ostreamamp; operator<< (ostreamamp; s, const Matrix44amp; t) {
    for (int i=0; i<4; i  ) {
      for (int j=0; j<4; j  )
        s << t.element[i][j] << "   ";
      s << endl;
    }
    if (!s) { cerr << "Error writing Matrix to stream";  exit(0); }
    return s;
  }
  

Матрица:

     class Matrix44 {
private:
    double element[4][4];
    friend class Point;
public:
    Matrix44(void);
    Matrix44 transpose(void) const;
    Matrix44 inverse(Matrix44 x) const;
    Matrix44 kinematics(double d, double theta, double a, double alpha) const;
    friend istreamamp; operator>>(istreamamp; s, Matrix44amp; t);
    friend ostreamamp; operator<<(ostreamamp; s, const Matrix44amp; t);
    Matrix44 operator *(Matrix44 b);
    Point operator*(const Point amp; P);
};
  

Пара проблем, одна из которых находится в коде, который вы показали, а другая — нет.

Проблема 1: это может быть не источником вашей проблемы, но double pi = 3.14159; подходит для чисел с плавающей запятой, но не для удвоений. У вас должно быть число pi как минимум в 16 местах. А еще лучше, используйте M_PI from <math.h> , распространенное расширение во многих компиляторах. Если ваш компилятор не определяет M_PI , используйте что-то вроде 3.14159265358979323846264338327950288.

Проблема 2: Вы не показали свой код, в котором выполняете умножение, так что это может быть и не источником вашей проблемы. Проблема в том, что группа вращения не является коммутативной: A * B не равно B * A. Здесь вы должны быть очень осторожны с соглашением об умножении. Пример: Возьмите книгу, держите ее ровно, чтобы передняя обложка была обращена вверх, а корешок — влево. Поверните книгу на 90 градусов вокруг оси, которая указывает в направлении строки текста. Теперь поверните на 90 градусов вокруг оси, которая указывает от нижней части страницы к верхней. Ваша книга должна иметь такую ориентацию, чтобы вы могли убрать ее на полку (корешок ориентирован вертикально, лицом к вам). Теперь верните книгу в исходную ориентацию и повторите повороты, но в обратном порядке. На этот раз вы увидите совсем другую картину.

Конечно, ваша индексация неверна? Вместо

 //row1
 result.element[0][0] = cos(radstheta);
 result.element[0][3] = -cos(radsalpha)*sin(radstheta);
 result.element[0][4] = sin(radsalpha)*sin(radstheta);
 result.element[0][3] = a*cos(radstheta);
  

это должно быть

 //row1
 result.element[0][0] = cos(radstheta);
 result.element[0][1] = -cos(radsalpha)*sin(radstheta);
 result.element[0][2] = sin(radsalpha)*sin(radstheta);
 result.element[0][3] = a*cos(radstheta);