Cartesian goal conversion

[Nibanovic]

Instead of assigning the cartesian goal directly in AdmittanceRule, tried converting the goal to joint-space when message arrives.

This way, little has to be changed in AdmittanceRule.

Result of this change is not satisfactory, as the conversion is not very precise. Details in my report.

TLDR, i presume this is due to using jacobians for conversion, which has significant error for delta_x > 0.1

Anyways, I'd look for another way to resolve the issue of empty reference_ when giving goals over Pose topic.

Note:

Before using, please review the logic in last commit, as I was losing my mind from testing by the end of it. I've left some debug prints in.

[mergify]

@Nibanovic, all pull requests must be targeted towards the master development branch.
Once merged into master, it is possible to backport to admittance-robot-description-param, but it must be in master
to have these changes reflected into new distributions.

[github-actions]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.