clear all; 

Exercise 2.3 - Inverse Kinematic Anthropomorphic arm with Spherical Wrist

In this Exercise you will compute the inverse kinematic of an anthropomorphic arm with a spherical wrist

Please store your solutions in the predefined variables!

Task description:

below you will see a model of an Anthropomorphic arm with a spherical wrist.

Consider the following set of DH parameters:

Link	a [m]	alpha	d [m]	theta
1	0	pi/2	0	\(\displaystyle \theta_1\)
2	0.3	0	0	\(\displaystyle \theta_2\)
3	0	pi/2	0	\(\displaystyle \theta_3\)
4	0	-pi/2	0.4	\(\displaystyle \theta_4\)
5	0	pi/2	0	\(\displaystyle \theta_5\)
6	0	0	0.15	\(\displaystyle \theta_6\)

In the case of this manipulator with a spherical wrist, the solution is decoupled between position and orientation, i.e. the three joints of the arm are used to position the end-effector, and the three joints are used to fix its orientation.

Given the end-effector position \(p_{\textrm{ee}}\) and orientation \(R_{\textrm{ee}}\), the following steps should be followed:

1. Compute the wrist position \(p_w =p_{\textrm{ee}} -d_6 \cdot z_6\)
2. Solve inverse kinematics for the Anthropomorphic Arm: \(\theta_3 ,\theta_2 ,\theta_1\)
3. Compute \(R_3^0 \left(\theta_1 ,\theta_2 ,\theta_3 \right)\)
4. Compute \(R_6^3 \left(\theta_4 ,\theta_5 ,\theta_6 \right)={R_3^0 }^T \cdot R_{\textrm{ee}}\)
5. Solve inverse kinematics for Spherical Wrist: \(\theta_4 ,\theta_5 ,\theta_6\)

The four solutions of the IK of the arm combined with the two solution of the wrist result in a total of eight solutions.

Reach the following pose:

$$ T_{\textrm{desired}} =\left\lbrack \begin{array}{cccc} 0\ldotp 5 & 0 & 0\ldotp 866 & 0\ldotp 25\newline 0\ldotp 866 & 0 & -0\ldotp 5 & 0\ldotp 1\newline 0 & 1 & 0 & 0\ldotp 35\newline 0 & 0 & 0 & 1 \end{array}\right\rbrack $$

Answer all the questions and store your solution in the correct variable

syms q1 q2 q3 q4 q5 q6 real 
% DH Parameters Table
        % a      alpha      d       theta
DH = [    0,     pi/2,     0,       q1;    % Link 1
          0.3,   0,        0,       q2;    % Link 2
          0,     pi/2,     0,       q3+pi/2;    % Link 3
          0,     -pi/2,    0.4,     q4;    % Link 4
          0,     pi/2,     0,       q5;    % Link 5
          0,     0,        0.15,    q6];   % Link 6

Tdesired = [0.5,      0,     0.866, 0.25;
            0.866,    0,    -0.5,  0.1;
            0,        1,     0,    0.35;
            0,        0,     0,    1];

Task 1

1. Compute the wrist position \(p_w =p_{\textrm{ee}} -d_6 \cdot z_6\)
2. Solve inverse kinematics for the Anthropomorphic Arm: \(\theta_3 ,\theta_2 ,\theta_1\)

Use the following variables to store your solution:

• pee (end-effector position)
• pw (the wrist position)
• anthro_solutions (inverse kinematic solution where each row is a solution)

  pee = [];
  pw = [];
  anthro_solutions = []; 
  

You can check your work by clicking the Run:

check_exercise('2-3-1')

Task 2

1. Compute \(R_3^0 \left(\theta_1 ,\theta_2 ,\theta_3 \right)\)
2. Compute \(R_6^3 \left(\theta_4 ,\theta_5 ,\theta_6 \right)={R_3^0 }^T \cdot R_{\textrm{ee}}\)

hint: the rotation R03 and R36 changes for each anthropomorpic arm solution.

Use the following variables to store your solution:

• Ree (Rotation of the end-effector)
• R03 (Rotation from frame 0 to frame 3 as a 3D matrix)
• R36 (Rotation from frame 3 to frame 6 as a 3D matrix)

hint: you can use cat(3,Mat1,Mat2,Mat3,Mat4) to obtain a 3D array.

Ree = []; 
R03 = []; 
R36 = [];

You can check your work by clicking the Run:

check_exercise('2-3-2')

Task 3

1. Solve the inverse kinematics for Spherical Wrist: \(\theta_4 ,\theta_5 ,\theta_6\)

Use the following variables to store your solution:

• solutions (complete inverse kinematic solution for the anthropomorphic arm with spherical wrist, where each row represents a unique solution)

hint: You must compute two spherical wrist soluitions for each corresponding anthropomorpic arm solution.

solutions = [];

You can check your work by clicking the Run:

check_exercise('2-3-3')