Question

I have a routine which will accept the joint parameters d, theta, a, and alpha as input and will produce the corresponding 4×4 homogeneous matrix as output. I have tested my matrix multiplication and it does work fine. I will get 5 matrices from the input which will all be multiplied together resulting in $t^0_5$ . The test cases are here. My output is nothing like the result. Here is my code:

First the matrix that we input the data into a martix that uses the DH parameter tables:

 //Kinematics
Matrix44 Matrix44::kinematics(double d, double theta, double a, double alpha)const {
     Matrix44 result;

     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 result;
}

The part in main where I get the result, the data comes from this table:

          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;

Theta and alpha are angles while D and A are distances.

The code for the output / input:

   //User Input
  istream& operator>> (istream& s, Matrix44& 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
  ostream& operator<< (ostream& s, const Matrix44& 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;
  }

Matrix:

    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 istream& operator>>(istream& s, Matrix44& t);
    friend ostream& operator<<(ostream& s, const Matrix44& t);
    Matrix44 operator *(Matrix44 b);
    Point operator*(const Point & P);
};

Answer 1

A couple of problems, one is in the code you showed, the other is not.

Problem 1: This may not the source of your problem, but double pi = 3.14159; is fine for floats, but is not for doubles. You should have pi to at least 16 places there. Better yet, use M_PI from <math.h>, a common extension in many compilers. If your compiler doesn’t define M_PI, use something like 3.14159265358979323846264338327950288.

Problem 2: You didn’t show the your code where you do the multiplication, so this may not be a source of your problem, either. The issue is that the rotation group is not commutative: A*B is not equal to B*A. You have to be very careful of the multiplication convention here. Example: Pick up a book, hold it flat so the front cover is facing up and the spine is to the left. Rotate the book +90 degrees about the axis that points in the direction of a line of text. Now rotate by 90 degrees about the axis that points from the bottom of a page to the top. Your book should be in an orientation such that you can put it away on a shelf (spine oriented vertically, facing you). Now put the book back in its original orientation and repeat the rotations, but in the reverse order. You will see a very different picture this time around.