Search code examples
matlabmatrixtransformationkinematics

Forward Kinematics on Matlab

hope you are very well. I am studying Forward Kinematics at the moment and was trying to implement the ideas on MATLAB for a simple Robotic Arm:

enter image description here

I want to calculate the Transformation matrice for q3 if the joint is rotated 45 degrees counter-clockwise.

The last part on the paper shows the Transformation Matrice fpr the end-effector frame.

Here is what I tried on MATLAB:

1 clc
2 clear
3 omega3 = [0 0 1]';
4 v3 = [0 -2 0]';
5 omega3Bracket = [0, -omega3(3), omega3(2); omega3(3), 0, -omega3(1); -omega3(2), omega3(1), 0];
6 S3Bracket = [omega3Bracket, v3; 0 0 0 0];
7 expm(S3Bracket.*pi/4)

On the third and fourth lines, I created the angular and linear velocity vectors.

On the fifth line, I converted the angular velocity vector to a 3x3 skew-symmetric matrix.

On the sixth line, I created the skew-symmetric Screw matrix.

On the seventh line, I used matrix exponentilal to calculate the Transformation Matrix. However, the last column of the matrix I got is different than the one I should be able to get. Dou you know the reason foe that? What am I doing wrong?

Solution

  • If the final answer in the paper is correct, then the length of the end-effector arm should be 1 unit and you did not mention this. Also in the code, you should multiply the final answer by the M matrix. So the code should be:

    clc
clear
omega3 = [0 0 1]';
v3 = [0 -2 0]';
omega3Bracket = [0, -omega3(3), omega3(2); omega3(3), 0, -omega3(1); -omega3(2), omega3(1), 0];
S3Bracket = [omega3Bracket, v3; 0 0 0 0];
M = [1 0 0 3; 0 1 0 0;0 0 1 0; 0 0 0 1];
expm(S3Bracket*pi/4)*M

    Note that M(1,4) = 3, which is the distance between the origin and the end-effector at the beginning, 2 units for the prismatic joint and 1 unit for the end-effector arm.