Add dynamic reconfigure parameters for PID error reset
Add feedforward controller based on robot and motor dynamics