Task 2: Approach cube from the side rather than from the top

[nepfaff]

Sometimes, it is not possible to move the gripper above the cube before going down and gripping. This is due to joint limits or workspace limits.

Try to use Pythagoras to approach the cubes from the side. We could use the existing offset function to do this: One waypoint at an offset and then one waypoint above. The z-coordinate for both should probably be the place z-coordinate with a tiny offset (e.g. 0.5cm).

There are cases when approaching from the top is still desirable. It could make sense to have a function that quickly checks if a waypoint has valid IK (already have this: doValidIKExist). We can then try different approaches in decreasing order of priority if this function returns false. Something like we want to do a but if a does not work, try b, but if b does not work try c.

[nepfaff]

Now done for modifyWaypointFromTopToSideApproach. Also need to do this for the three other task 2 waypoint functions.

[nepfaff]

All done