Fix admittance controller interface read/write logic

[Nibanovic]

Using ROS rolling and gazebo Harmonic, I've found two bugs while trying to control a robot using both velocity and position control.

1. In write_state_to_hardware(), state_commanded.positions are mistakenly being written to both velocity and acceleration command interfaces.
2. Both in write_state_to_hardware() and read_state_from_hardware(), checking whether the controller has pos/vel/acc interfaces is done using if-else clauses. This causes the block to exit upon first true condition, meaning, after confirming position interface exists, velocity and acceleration interfaces never get written to or read from

These fixes are verified in my use case, but seem obvious enough that no tests need to be written to verify these claims.

Nonetheless, let me know if you'd like some tests before merging.

Cheers!

[bmagyar]

This makes a lot of sense sir :D