Walking Trajectory Path (WTP)

[dhruvthanki]

Walking Trajectory Path (WTP)

This pull request introduces the computation and optimization of the Walking Trajectory Path (WTP) based on the next two footsteps in the queue. The key points are:

• Computation: Each waypoint of the WTP is determined as the midpoint of each pair of sequential footsteps.
• Frame Position Update: The starting position of the WTP frame is updated at each state change, matching the position of the WTP frame at the end of the previous state.
• Time Duration Calculation: The time to reach the next waypoint is calculated as the sum of the transfer state duration of the current footstep, the swing state duration of the current footstep, and half of the transfer state duration of the subsequent footstep, with specific exceptions for the beginning and end of the trajectory.
• Optimization: The WTP is optimized by minimizing the total acceleration along the overall trajectory, ensuring a smooth and efficient path.

These changes enhance the accuracy and smoothness of the robot's walking trajectory, providing better performance and stability.