cleanup LMA kinematics solver

[rhaschke]

• perform similar cleanup as in KDL IK solver cleanup #1294
• actually use KDL's LMA solver (was instantiated but not used)
• remove notion of mimic joints, as the LMA solver doesn't support this

[mlautman]

Great improvement! Looks good to me :)

[rhaschke]

@mlautman Any reason you didn't yet merged this?

[davetcoleman]

There is so much removed code - how is it possible?

You say that you "remove notion of mimic joints, as the LMA solver doesn't support this" but doesn't the custom version in the deleted files provide support for this?

actually use KDL's LMA solver (was instantiated but not used)

Are you sure about this?

@fsuarez6 can you review since the original PR was yours: moveit/moveit_ros#723

[rhaschke]

LMA solver was instantiated but not used

Are you sure about this?

Not anymore. Looks like I confused variable names. Will need to fix this later. Closing this PR for now.
Thanks for being attentive!

[rhaschke]

There is so much removed code - how is it possible?

Most of this code was just copied 1:1 from the KDL solver, but not actually used. That's why the cleanup.

You say that you "remove notion of mimic joints, as the LMA solver doesn't support this" but doesn't the custom version in the deleted files provide support for this?

No. The KDL::ChainIkSolverVel_pinv_mimic, copied from the KDL IK plugin, and instantiated here, indeed would support mimic joints for the computation of the Jacobian. However, this class instance isn't actually used for any computation. Instead, the LMA plugin instantiates the standard KDL::ChainIkSolverPos_LMA solver (which instantiates its own VelocityIKSolver) here. This PositionIK solver is then wrapped by ChainIkSolverPos_LMA_JL_Mimic, which adds a thin layer of additional joint harmonization and limit checking. All the other code is essentially unused.

So, my original comment was slightly wrong, because the LMA solver was actually used before already.
Looks like I mixed this up, with the KDL::ChainIkSolverVel_pinv_mimic solver that was not used.
Sorry for that.

Although, I personally think that joint-limit rejection should be better handled in the solution_callback, I added joint harmonization and limit checking back to the cleaned code.
@fsuarez6 Can you motivate why your harmonization code uses the range -2 Pi ... 2 Pi instead of -Pi .. Pi ? The new code uses the default interval.

[rhaschke]

Rebased to resolve conflicts.

[fsuarez6]

Most of this code was just copied 1:1 from the KDL solver, but not actually used. That's why the cleanup.

That's right. It was copied 1:1 from the KDL solver.

Can you motivate why your harmonization code uses the range -2 Pi ... 2 Pi instead of -Pi .. Pi ?

I cannot recall this. I think the default interval (-pi, pi) should work better.

Related question: Do we have any tests for the kinematics plugins?

[rhaschke]

Do we have any tests for the kinematics plugins?

@fsuarez6 We have, but only since a few weeks (#1272). You are invited to add a test for the LMA solver.

[fsuarez6]

Great. If you guys are OK with holding this PR for a short while, I'll add some tests along side to be sure that we aren't breaking anything here.

[rhaschke]

Thanks for contributing more test. We will hold back this cleanup.
If you have questions, how to setup the unit tests, please contact me. All you should need to to is setup a rostest similar to https://github.com/ros-planning/moveit/pull/1320/files#diff-63b259ba139f6ad13f7f3a8e5248e81b. I hope I will get merged this PR in today...

[fsuarez6]

You guys already have tests for this: https://github.com/ros-planning/moveit/blob/melodic-devel/moveit_kinematics/test/fanuc-kdl.test

@davetcoleman, based on the tests and the removed code, this PR looks good to me.