Add unit tests

[cmower]

Todo

• test_builder.py
• test_models.py
• test_optas_utils.py
• test_optimization.py
• test_solver.py
• test_spatialmath.py
• test_sx_container.py
• tester_robot_model.py
• test_examples.py

[cmower]

@joaomoura24, I have completed the unit tests. All checks are passing. When you have time, it would be great if you could try out on your PC. There is an explanation on how to run the tests in the README, just make sure to pip install . from the root of the repo before running. Also, before merging, would be a good idea for you to double check the list of commits - these may affect your code (however, I don't think these should affect you so much).

[joaomoura24]

Hey, for me it seems all good. I also tested my code base and everything runs well.
You can merge it.