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?
