Task 2: Object avoidance while moving between cube locations

[nepfaff]

At the moment, we often crash into other cubes while moving a cube from one place to another.

We somehow want to move between cubes at a higher z-coordinate. However, simply increasing the z-coordinate does not work due to joint limits.

From videos, this should be achievable by decreasing joint angle 2 and keeping all other joint angles constant. Something like a negative offset for joint angle 2 while moving between cubes. In practice, this could be achieved by computing the FK of decreasing joint angle 2 by some amount and keeping all others constant. We could then place a waypoint using the FK coordinates in the middle of the two cube locations. The middle could be determined using the L2 norm. Such a motion can become smooth using spline interpolation.

[nepfaff]

It does not make sense to do this directly in the low-level waypoint generation functions as we might want to place an additional waypoint in the middle of two actions. For such a waypoint, we would need to know the start and end waypoints that come from different functions. A postprocessing algorithm that iterates over all waypoints and inserts additional waypoints if two consecutive waypoints are too far from each other (L2 norm), could work.

[nepfaff]

This worked well for task 2 b in simulation:

[nepfaff]

This approach looks decently promising in simulation. However, in practice most collisions occur when moving away from a start cube or approaching a finish cube. This approach might not help in these cases, as the altered trajectory might still lead to cube collisions before sufficient height is reached.

[nepfaff]

Closing this for now. Re-open if testing reveals that we need something more sophisticated.