Implementing an analytic version of an inverse kinematic

Question

People have recommended me implement an analytic version of inverse Jacobian solver, such that I won't be forced only the least square solution, but would have an local area of solution near to the one I desire.

I can't seem to implement it correctly, I mean how much does it differ from the least square inverse kinematics which I have implemented here?

Eigen::MatrixXd jq(device_.get()->baseJend(state).e().cols(),device_.get()->baseJend(state).e().rows());
      jq = device_.get()->baseJend(state).e(); //Extract J(q) directly from robot


      //Least square solver - [AtA]⁻1AtB

      Eigen::MatrixXd A (6,6);
      A = jq.transpose()*(jq*jq.transpose()).inverse();



      Eigen::VectorXd du(6);
      du(0) = 0.1 - t_tool_base.P().e()[0];
      du(1) = 0 - t_tool_base.P().e()[1];
      du(2) = 0 - t_tool_base.P().e()[2];
      du(3) = 0;  // Should these be set to something if i don't want the tool position to rotate?
      du(4) = 0;
      du(5) = 0;

      ROS_ERROR("What you want!");
      Eigen::VectorXd q(6);
      q = A*du;


      cout << q << endl; // Least square solution - want a vector of solutions. 

I want a vector of solution - how do I get that?

the Q is related to this https://robotics.stackexchange.com/questions/9672/how-do-i-construct-i-a-transformation-matrix-given-only-x-y-z-of-tool-position

The robot being used is a UR5 - https://smartech.gatech.edu/bitstream/handle/1853/50782/ur_kin_tech_report_1.pdf

Answer 1

Given a desired pose:

$$T^{desired} = \begin{bmatrix} s_x&n_x&a_x&P_x\\ s_y&n_y&a_y&P_y\\ s_z&n_z&a_z&P_z\\ 0& 0& 0& 1 \end{bmatrix}$$

we know that this is given as follows:

$$T^{desired} = {}^{0}T_{1}{}^{1}T_{2}{}^{2}T_{3}{}^{3}T_{4}{}^{4}T_{5}{}^{5}T_{6}$$

where each ${}^{j-1}T_{j}$ is the transformation between frame attached to joint j with respect to joint j-1.

Now the idea is to start pre-multiplying the equation with the inverse transformation and try to deduce the joint angles as we go along, as an example let's start by the first joint (i.e. $q_1$):

$${}^{1}T_{0}T^{desired} = {}^{1}T_{2}{}^{2}T_{3}{}^{3}T_{4}{}^{4}T_{5}{}^{5}T_{6}$$

When you do this you find the following:

$$-\cos(q_5) = -a_x \sin(q_1) + a_y \cos(q_1)$$

and

$$-d_4 - d_6\cos(q_5) = -p_x\sin(q_1)+p_y\cos(q1)$$

so by manipulating those two equations you end up with a quadratic equation in $\sin(q_1)$ or $\cos(q_5)$ which gives two solutions, only one of them satisfy the second equation above which gives the other parameter (i.e. the $\sin(q_1) or \cos(q_1)$) then you get $q_1$ as

$$q_1 = atan2(\sin(q_1),\cos(q_1))$$

then you can get $q_5$ and then proceed in the same manner to get the rest. If you want I can share with you some document that explain the procedure in more details, and also some matlab m files that help you to symbolically advance through the procedure.

Small remark: I am not using exactly the Denavit-Hartenberg parameters but a slightly different ones (i.e. Khalil-Kleinfinger) which is also explained in the document.

Answer 2

Your Jacobian-based approach is great for velocity control, or when the manipulator is close to the original point. But remember, the Jacobian only gives a first-order approximation to the manipulator's motion. With the highly nonlinear kinematics of manipulators, accuracy will vary throughout the workspace, and will decrease as the step size between the current pose and desired pose gets larger.

Closed-form, or analytic, inverse kinematics do not require the Jacobian for computing the joint angles which will achieve a desired pose. They are based on the trigonometry of the manipulator. To compute analytic inverse kinematics, you need to take the forward kinematics and solve for each $q$. This can be challenging for some manipulator types. It is easier if the wrist is spherical (all three wrist axes intersect at a point). The good news is that many robotics books contain inverse kinematic solutions for each of the manipulator types.