Preliminary version of the library (not working yet). In this version I just implemented the constraints, the constant trajectories, the robot wrapper and the task SE3 equality.