Solve inverse kinematics for 6DOF arm

Problem 2

➡️ ✍️ 1. Assign frames to the following 6 DOF robot joints and a final "tool frame", in a way that the last three rotational joint axes (i.e. the wrist) are all incident.

➡️ ✍️ 2. Find the direct and inverse kinematics.

❗ Note: to solve symbolic matrix multiplications and simplifying we can use https://live.sympy.org/

• You can declare symbols with:

q1,q2 = symbols('q1 q2')

• You can declare matrices with:

 R1 = Matrix([
          [cos(q1), -sin(q1), 0, 0],
          [sin(q1), cos(q1), 0, 0],
          [0, 0, 1, 0],
          [0, 0, 0, 1]
      ]) 

• You can simplify symbolic expressions with:

 T01 = simplify(T0*R1)