mahd/implementation/SCARA/01_kinematics/implementation.md

Kinematics model

This is a really simple model. It has some inverse kinematics and forwards kinematics.

Inverse model

The setpoint on the board is given as a x,y position. We have to convert this point in to angles of the SCARA joints. We have the following setup:

We can resolve this via the law of cosines and pythagoras theorem. We can find the length of C with pythagoras. Then with the use of a tangent we can find the angle phi. Then filling in the law of cosines with C calculated and A and B as arm lengths, we can calculate the angles a, b and c. As a check we sum these angles and they should add up to the value of Pi.

Then we add phi and b to get the corner of the first joint relative to the world frame. And c - Pi + phi + b gives the angle of the second joint relative to the world frame.

Forward model

The forward model is as follows.

joint1 = A_length*[cos(angle1);sin(angle1)];
joint2 = B_length*[cos(angle2);sin(angle2)];
end_effector = joint1 + joint2;

This is then drawn as two arms in a 3D viewer. It follows the outside of the square.