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

  • $\begingroup$ can you send me or share your robot articulation, because for the analytical inverse kinematic model (also known as the inverse geometric model) one needs to know the articulation of the robot and the starting configuration to develop the forward geometric model then by going backwards try to infer analytical expression for each joint's angle in terms of the desired tool pose and previously obtained angles, as SteveO said. Furthermore the code you have shared actually outputs joints velocities to produce a tool twist (du in your code), and what you are trying to do is to use infinitesima $\endgroup$ Apr 20, 2016 at 10:00
  • $\begingroup$ The robot i am using us an UR 5 $\endgroup$
    – test
    Apr 20, 2016 at 15:22
  • $\begingroup$ smartech.gatech.edu/bitstream/handle/1853/50782/… $\endgroup$
    – test
    Apr 20, 2016 at 15:22
  • $\begingroup$ The last part of ghanimmukhtar's comment (which was truncated when converted to a comment) was "what you are trying to do is to use infinitesimal desplacements to mimic the underlying kinematic model (i.e. du = J*dq), where J is the jacobian matrix and dq is the infinitesimal displacement in joint space." $\endgroup$
    – Mark Booth
    Apr 21, 2016 at 18:40
  • $\begingroup$ yes... @Mark Booth not sure whether that was an answer or question.. $\endgroup$
    – test
    Apr 21, 2016 at 19:28

2 Answers 2


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)$$


$$-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.

  • $\begingroup$ Thanks for the new answer ghanimmukhtar. Editing a deleted answer doesn't automatically undelete it, sadly. You could have flagged it for moderator attention (asking for it to be undeleted saying information had been added to make it into an answer), but nothing is really lost by creating a new answer. $\endgroup$
    – Mark Booth
    Apr 25, 2016 at 13:30
  • $\begingroup$ I have converted many of you inline formulae to block formulae to improve the readabilty of you answer, but the content of you answer is excellent. I have never seen this explained so well in so few words. $\endgroup$
    – Mark Booth
    Apr 25, 2016 at 13:40
  • $\begingroup$ thanks Mark Booth, I just didn't know how to notify the moderator, i guess i was in a hurry and didn't investigate the "flag" option :) anyway thanks a lot for your editing and guidance, Cheers! $\endgroup$ Apr 25, 2016 at 15:17

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.

  • $\begingroup$ The one implemented there doesn't not necessarily output the correct the Q state.. it seem swing toward the base... $\endgroup$
    – test
    Apr 20, 2016 at 5:10
  • $\begingroup$ I am not sure I agree with your statement.. dq + current_q = would provide me with the new position right? $\endgroup$
    – test
    Apr 20, 2016 at 16:33

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.