I have been trying to find the HTM of a 2-DOF arm in 2D by finding the rotation matrices and displacement vectors by hand rather than use DH parameters to better understand them. I have followed the steps in this video to find the rotation matrices by finding the rotation when the joint angle is 0, and multiplying with the 2x2 rotation matrix around the z-axis.
However, when I solve for the HTM with theta1 = 90 degrees in python, I get a displacement vector of 0,0. I am wondering if the method I'm using to find the R0-1 is correct, especially since frame 1 is offset from frame 0. My steps are listed below:
a1 and a2 are the link lengths, and theta1 and theta2 are the joint positions, shown starting at 0.
Any help is very appreciated.