ConvexRegionSelector and SwingTrajectoryPlanner

[kslee12]

Thank you for the great work.

This code has been a valuable learning resource for me, and I hope it will be helpful to others as well.

1. I found that lastStandMiddleTime is not updated in ConvexRegionSelector.cpp.
   Although I could not fully understand the code, it might be fixed as follows:

if (!numerics::almost_eq(standMiddleTime, lastStandMiddleTime)) {
     lastStandMiddleTime = standMiddleTime
...

2. Although the code works great, I think there are some improvements to be made in the swing trajectory generation :

• During steps, the swing trajectory passes through the terrain, which might be related to the collision penalty.
• On slopes, the swing trajectory does not seem to work properly. The trajectory is generated too high, especially showing a tendency to rise on downhill slopes. I tried to solve this problem (by modifying foot height or adding some coding tricks), but it still does not work well.