fixed typo in control gain
edited matlab node for debugging
fixed issues with PoseArray/TwistArray initialization
Merge remote-tracking branch 'origin/master'