First minimal running version of constraint controller in gazebo.