# Solution to Inverse Kinematics of a 6 DOF Articulated robot with a wrist joint

I'm working on getting a solution to the inverse kinematics of a 6DOF Articulated robot with a wrist joint.

I derived the robot's forward kinematics firstly, using these angles $$θ_1 = 60.102$$ $$θ_2 = 65.220$$ $$θ_3 = 40.390$$ $$θ_4 = 70.215$$ $$θ_5 = 40.002$$ $$θ_6 = 30.211$$

Using these joint parameters, I found a total transformation matrix

$$a_1 = 0.18;~~a_2 = 0.6;~~a_3 = 0.12;~~a_4 = 0;~~a_5 = 0;~~a_6 = 0;$$

$$d_1 = 0.4;~~d_2 = 0;~~d_3 = 0;~~d_4 = 0.62;~~d_5 = 0;~~d_6 = 0.115;$$

$$α_1 = 90;~~α_2 = 0;~~α_3 = -90;~~α_4 = 90;~~α_5 = -90;~~α_6 = 0;$$

This is the resulting total transformation matrix:

$$-0.920890~~~0.342692~~~~0.185808~~~~-0.077297$$ $$-0.010244~~~0.455209~~~~-0.890326~~~-0.273985$$ $$-0.389689~~~-0.821795~~~-0.415687~~~0.845690$$ $$0.000000~~~~0.000000~~~~0.000000~~~~1.000000$$

Now, I'm trying to find an analytical Inverse Kinematic solution that will output the original joint angles I used for the robot's forward kinematics using the total transformation matrix I found.

I used this formula to find the first three angles by decoupling the robot. The output of Theta 1 is correct but the output of Theta 2 & Theta 3 are wrong.

$$θ_1 = atan2(y_c,x_c) + π$$ $$θ_3 = atan2(s3,c3)$$ $$θ_2 = atan2((z_c-d_1),\sqrt(x_c^2+y_c^2)) - atan2(a_3s_3,a_2+a_3c_3)$$

KEY

$$x_c = P_x - (d_6*r_{13})$$ $$y_c = P_y - (d_6*r_{23})$$ $$z_c = P_z - (d_6*r_{33})$$ $$s_3 = sine theta 3$$ $$s_3 = sqrt(1-c_3^2)$$ $$c_3 = cosine theta 3$$ $$c_3 = \frac {(x_c^2 + y_c^2 + (z_c-d_1)^2 - a_2^2 - a_3^2)}{(2*a_2*a_3)}$$ $$a_2,a_3,d_1 == robot link parameters, defined earlier$$

Does someone know of another IK analytical method I can use that will get me my original joint angles?

• Welcome to Robotics izu201ph. We are fortunate enough to have MathJax support enabled, allowing you to easily create subscripts, superscripts, fractions, square roots, greek letters and more. This allows you to add both inline and block element mathematical expressions in robotics questions and answers. For a quick tutorial, take a look at How can I format mathematical expressions here, using MathJax?. Please edit your question to make it clearer.
– Ben
Sep 22 at 13:38
• Thanks @Ben I'll make the changes Sep 22 at 13:54