add set commanded position to the current robot position in impedance mode

[jacknlliu]

Update the commanded position to the current robot position if we don't have received any position commands from the user side in control loop.

Reference

• ROS Monitor gravity compensation mode