replaced tf transformations with kdl
reverted gains