Use of bounds variable is counterintuitive

[Markus Sigg]

When using trac_ik with a 5DOF arm we used the variable

bounds.rot.x(FLT_MAX);

in order to set the roll angle of the IK solution to free. However this did not lead to the desired results. After some investigation I noticed that the the difference between the solution and the desired frame, in order to check if the solution is within the bounds, is computed with respect to the joint group's base link.

My understanding of a bounds variable is much more that the tolerances should be applied w.r.t. the target frame. Which is also much more convenient when used with a mobile robot where the base link of the joint group may change its pose while the target pose is fixed in the world frame.

I fixed the issue for us in our github fork of trac_ik: https://github.com/tu-darmstadt-ros-pkg/hector_trac_ik/ The attached file also includes my changes.

What do you think about it?

[Patrick Beeson]

You are correct that the idea was that the tolerances/bounds be in the end effector frame. I'll look into this in the morning, but I do not understand how bounds.rot.x(FLT_MAX) would not work for you, as the check for error within that large tolerance would be true regardless of what frame is used to calculate error. Please let me know why you think frame of reference matters when tolerances are FLT_MAX (I can see how it would matter for smaller tolerances).

[Markus Sigg]

Our endeffector is a camera which has its optical axis aligned to the X-axis of the endeffector frame. As we can just rotate the image later, we don't care about the rotation about this axis.

However when the X-axes of the 'desired endeffector pose' and the 'chain's base-link/frame' are not aligned and we freely rotate the endeffector about the base-link X-axis, then the resulting pose might not be what we want, i.e. our camera does not point towards our object of interest.

e.g:

• base-link frame is aligned with the world frame

• desired endeffector pose is rotated about 90 degree by the Z-axis

• then the free rotation about the base-link X-axis results in a negative rotation about the endeffectors Y-axis which is not the desired.

I can attach a screenshot this evening if this helps...

[Patrick Beeson]

Yes this makes sense. We never caught this before because we hadn't fully tested the bounds other than setting all orientations to FLT_MAX. I will merge your changes manually because they are small. In the future, please submit a pull request from your fork to my master branch for proper git workflow.

[Patrick Beeson]

Patch incorporated into commit 6994051.

[Patrick Beeson]

Thanks!

[Patrick Beeson]

Actually, this fix introduced another bug. This has been fixed in tag 1.3.7. In the KDL solver, your relative twist needed to be put back into the base frame for the Inverse Jacobian to properly move toward the goal. Without doing this, KDL was never finding a solution, and your success with TRAC-IK was based purely on the NLOpt solver thread success.

[Patrick Beeson]

See commit bd778d1 for the resolution of your bug.