Reaching joint position limits affects TCP orientation

[PeterBowman]

Steps to reproduce:

1. Launch streamingDeviceController.
2. Move the robot TCP so that a joint limit is hit (motion stops, cannot proceed further).
3. Move away from that joint limit.

We observed that the last step may result in a different TCP orientation even if that was not intended. All subsequent commands do not seem to take this into account and the TCP behaves erratically, even moving in the opposite direction.

Follows up #173. Also keep in mind #186.

[PeterBowman]

Perhaps adding a look-ahead would help, as in tiago_spnav_teleop. That is, calculate the new step the robot would process and move to, but then calculate another one to decide whether the first should be ignored (e.g. we are approaching joint limits or a singularity) or not.

[jgvictores]

Definitely could be considered dated (but imho, first replicating and then advancing can be a good approach) but this of reminds me of "S. Kim, A. Shukla and A. Billard, Catching Objects in Flight, in IEEE Transactions on Robotics, vol. 30, no. 5, pp. 1049-1065, Oct. 2014", https://doi.org/10.1109/TRO.2014.2316022 -> I recall one of the constraints that was difficult to attain was complying with robot joint velocity limits (looking through the paper, they also mention singularities and joint limits).

• https://www.youtube.com/watch?v=H0ds54G53kI
• https://www.youtube.com/watch?v=M413lLWvrbI

[edit] PS: https://scholar.google.com/scholar?cites=5957588769371584553&as_sdt=2005&sciodt=0,5&hl=es

[jgvictores]

PS: From https://scholar.google.com/scholar?cites=5957588769371584553&as_sdt=2005&sciodt=0,5&hl=es "trajectory tracking control" seem like important keywords.