<<robotica.m;

DataFile["C:\\Users\\User\\Desktop\ICs\\robotTools\\DHparams.txt"]
FKin[]

Find equation for foot position. This is done by converting the point (0,0,0), i.e. the tip of the foot, to the world coordinate frame.

Clear[q1,q2,q3]
coords=Transpose[{{0,0,0,1}}];
footposition = MatrixForm[Simplify[A[1].A[2].A[3].coords, Element[{q1,q2,q3}, Reals]]]

For testing, input joint angles here and it will spit out foot position:

transformation[q1_,q2_,q3_]=footposition;
transformation[Pi/2,Pi/2,0]

The Jacobian matrix can also be found analytically, but it's so long that I've decided to not use this and just find it numerically in the program

MatrixForm[J]

JTinv = PseudoInverse[J]

For backup, here' s the text file DHparams.txt :


Some notes:
z0 is the axis of actuation for joint 1, z1 is the axis for joint 2, etc.
For my robot specifically, the +x0 axis is toward the front of the robot, the +y is toward the left.
Note: the transformation matrices T[0,3] appear to not work, so make it yourself with A[1].A[2].A[3]
DOF=3
(Robotica requires a line of comment text between DOF and joint1)  -90 Degree
joint1 = revolute
a1 = seg1length
alpha1 = 90 Degree
d1 = 0
theta1 = q1
joint2 = revolute
a2 = seg2length
alpha2 = 0
d2 = 0
theta2 = q2
joint3 = revolute
a3 = seg3length
alpha3 = 0
d3 = 0
theta3 = q3