ikfast question regarding UR packages

[TheURMaxer]

I have a UR5 CB1 robot. I'm looking for FK and IK code. I tried the forward() and inverse() from the github ROS package. If I put in a sample set of 6 q's apply forward() then backward() I get the same answer. But, if I put in an actual set of values (q's) from my robot, the answers are all wrong. The Tx and Ty seem inverted and I can make no sense of the rotation part. Also, Hawkins published a paper with a different solution to the forward kinematics then what is used in the package. This solution provides the correct answer to the translation part, although the rotation still baffles me. Funny observation, the DH parameters are different, too. What's going on, and does anyone have a working set of forward() backward() that actually corresponds to the numbers given on the UR touchscreen?

[gavanderhoorn]

Just making sure: geometry_msgs::Pose uses quaternions for orientation (so no Euler angles).

re: DH: each UR has slightly different DH parameters due to kinematic calibration. How 'different' are yours? It could be that CB1 and later URs have differences the plugin doesn't account for.

[TheURMaxer]

Thanks for the quick reply!

DH from the github sources (UR5):

//Denavit-Hartenberg parameters for the UR5 in [m]
const double d1 = 0.089159;
const double a2 = -0.42500;
const double a3 = -0.39225;
const double d4 = 0.10915;
const double d5 = 0.09465;
const double d6 = 0.0823;

DH from the original paper (Analytic Inverse Kinematics for the Universal Robots UR-5/UR-10 Arms by Kelsey P. Hawkins, December 7, 2013):

double const d1 = 0.08920f; //Denavit-Hartenberg parameters for the UR5 in [m]
double const d4 = 0.10900;
double const d5 = 0.09300;
double const d6 = 0.08200;
double const a2 = -0.42500;
double const a3 = -0.39243;

The forward() developed in the cited paper works, in the sense it produces the correct translation part in the 4x4 matrix. The rotation part, the upper left 3x3 portion of the 4x4, is a rotation matrix, but I cannot decompose it into euler angles in any way or form. Also, the forwad() in the github gives the wrong translation part and the rotation part also makes no sense.

[gavanderhoorn]

re: DH data: if I reorder the constants I get almost the same values. Almost, but as I wrote earlier: each UR robot is different. You might ask: "but why provide a reusable IK/FK implementation then?" and that is a good question. But the choice was made -- by @kphawkins and others -- to select one set of DH params from a UR that was used for development of the plugin and stick with that.

You keep referring to "github source", but could you tell us which file you are referring to in particular? I'm guessing that would be ur_kin.cpp?

[TheURMaxer]

Yes, I mean ur_kin.cpp. I'm not bothered by the DH differences, it was just a funny difference of no further importance. I just included the remark, since it may trigger someone into remembering a certain version issue. The main problem I have, is that forward() and inverse in ur_kin.cpp are indeed complementary, but the values produced by forward() do not correspond to the actual values as obtained from the UR controller. The implementation of forward is also different from the paper. The one in the paper does give the proper values (meaning the translation part of the 4x4 matrix is correct). So 2 questions, 1) why are there 2 different versions of forward() and 2) does anyone have the inverse() that belongs to the forward() as developped in the paper?

[TheURMaxer]

Ok, I've contacted Kelsey Hawkins via email. He has very kindly explained that the UR tablet uses some extra transformations to obtain the values show in the display. The forward() and inverse() apply only to the kinematic chain without those extra transformations. Implementing the additional matrices give me the proper translation components. I just need to figure out the rotation parts, and I'm off :-)

[gavanderhoorn]

Ok.

So in the interest of all future readers that might want to do the same thing as you are trying to do: what "additional matrices" are you and @kphawkins referring to?

[TheURMaxer]

You can find them in ur_kin.h, I'll replicate here for convieniance:

//These kinematics find the tranfrom from the base link to the end effector.
// Though the raw D-H parameters specify a transform from the 0th link to the 6th link,
// offset transforms are specified in this formulation.
// To work with the raw D-H kinematics, use the inverses of the transforms below.

// Transform from base link to 0th link
// -1, 0, 0, 0
// 0, -1, 0, 0
// 0, 0, 1, 0
// 0, 0, 0, 1

// Transform from 6th link to end effector
// 0, -1, 0, 0
// 0, 0, -1, 0
// 1, 0, 0, 0
// 0, 0, 0, 1
The above give the proper translation part, cfm to the ur controller when used with the forward() and inverse(). However, the angles are still unaccounted for. Help is much appreciated.

[atenpas]

This issue is confusing me. Is the UR5 CB1 robot different from other UR5 robots? Could you give an example where the inverse function does not work for you?

I've been using the ur_kin module without any problems on our UR5 robot (I use this directly without moveit though).

[gavanderhoorn]

This issue confuses me as well and I believe I would have the same questions / comments as @atenpas and @a-price in #300.

I've marked it as wrid18 so that participants could take a look at this and figure out a strategy wrt the various IK related PRs and issues that are currently open.

[gavanderhoorn]

You can find them in ur_kin.h, I'll replicate here for convieniance:

//These kinematics find the tranfrom from the base link to the end effector.
// Though the raw D-H parameters specify a transform from the 0th link to the 6th link,
// offset transforms are specified in this formulation.
// To work with the raw D-H kinematics, use the inverses of the transforms below.

// Transform from base link to 0th link
// -1, 0, 0, 0
// 0, -1, 0, 0
// 0, 0, 1, 0
// 0, 0, 0, 1

// Transform from 6th link to end effector
// 0, -1, 0, 0
// 0, 0, -1, 0
// 1, 0, 0, 0
// 0, 0, 0, 1
The above give the proper translation part, cfm to the ur controller when used with the forward() and inverse(). However, the angles are still unaccounted for. Help is much appreciated.

I believe I now understand this comment: the xacros we ship as part of ur_description have base_link rotated 180 degrees over the Z axis wrt the real robot (see comment for the UR5). ee_link also doesn't coincide with the pose the flange or unconfigured toolframe on the real robot has.

The matrices shown in the comment I quoted seem to deal with this in the IK plugin.