UndercarriageCtrl with position steer controller

[benmaidel]

ref https://github.com/mojin-robotics/mojin_robots/issues/54
prevents angle wrap by adding the shortes angle distance between current position and desired position to the current state and use this as new command value.

See here for a discussion of the problem ipa320/cob_robots#751

[floweisshardt]

should be done before merging!

[benmaidel]

tested with velocity controller on cob4-19. I think it is better than before. @ipa-fmw can you please take a look at cob4-19

[fmessmer]

I tested this again today with cob4-19 (see https://github.com/mojin-robotics/mojin_robots/pull/71)
I tested this branch both with the WheelController (velocity-drivern rotation_joints - see https://github.com/ipa320/cob_control/pull/179#discussion_r171303457) and WheelMultiController (position-driven rotation_joints)
Here is my summary:

SignSwitches

controller wheel_commands sometimes show sign switches 1.5->-1.5->1.5 within subsequent control steps - might be some shortest angular distance issue

I definitely think that this is an improvement regarding the sign-switches - reported in ipa320/cob_robots#751 (comment)!

Works with WheelController (velocity-drivern rotation_joints)

I'd also say that this PR works well with the WheelController (velocity-drivern rotation_joints) - thus the ToDo can be removed

Works with WheelMultiController (position-driven rotation_joints)

It also seems to work well with the WheelMultiController (position-driven rotation_joints.
However, only with the current acceleration limits:

     "6083": "1000000" # profile acceleration
     "6084": "1000000" # profile deceleration
     "60C5": "1000000" # maximum acceleration
     "60C6": "1000000" # maximum deceleration

When increasing the limits to the value found to be max during single wheel testing with @timhappy, there is significant oscillation (and noise) from the rotation joints and the velocity joint_states do not really follow the commands anymore:

     "6083": "10000000" # profile acceleration
     "6084": "10000000" # profile deceleration
     "60C5": "10000000" # maximum acceleration
     "60C6": "10000000" # maximum deceleration

Conclusion

Either we can improve the controller behavior of the position-driven rotation joints to work with the increased max accelerations or otherwise we won't benefit much from using WheelMultiController over WheelController with the motivation to overcome #143 and #144

Also, given #187 and https://github.com/mojin-robotics/cob4/issues/684, cob4-19 might not be the ideal platform to test this.

Thus, my suggestion is to do more tests with another platform as well as with a max acceleration increase of only factor 2x/5x instead of 10x the previous max acceleration and then decide...
@benmaidel @timhappy @ipa-mdl @floweisshardt FYI

[benmaidel]

I definitely think that this is an improvement regarding the sign-switches - reported in ipa320/cob_robots#751 (comment)!

Do you still recognize sign-switches?

[fmessmer]

not planned