Write tests for position and orientation controllers

[nycrat]

Description of the task

Tests remain to be written for the PositionController and OrientationController classes that are used for motion control. These tests should be written to verify the behaviour of these classes that applying the motion controller's output will follow a target trajectory.

Acceptance criteria

• Tests written in software/embedded/motion_control/position_controller_test.cpp
• Tests written in software/embedded/motion_control/orientation_controller_test.cpp

Blocked By

#3726 #3737