Made lat_reproducer work with Gazebo AND real robot.
Startet implementing a possibility to find constraints.