====== Bipedal Robot Locomotion: Single Leg Quasi-static Gait  ======
----------------------------------------------------------------

Last week you used Optragen to generate an arbitrary trajectory for a chosen foot frame (relative to the **TORSO** frame). This week we'll begin building on that experience and take baby steps toward generating our full (quasi-static) walking gait.

We'll break our walking gait up into multiple gait phases: **SWING** and **DOUBLE**. The gait phases will progress over time as **SWING (right)**, **DOUBLE (right-left)**, **SWING (left)**, **DOUBLE (left-right)** and then repeat. We'll assume that feet in contact with the ground do not slip/slide; hence, their frames serve as a good choice for the stance frame, w.r.t. motions of other parts of the body may be planned. During the **SWING (right)** and **SWING (left)** phases, the foot not in contact with the ground (ie. **RIGHT_FOOT** and **LEFT_FOOT**, respectively) will be 'swinging' to the next ground contact position from which the next phase of the gait (ie. **DOUBLE**) can continue. Why is the **DOUBLE** phase of the gait important to accomplish quasi-static walking?

This week we'll focus on the **SWING (right)** phase of the gait. However, keep in mind the **SWING (right)** and **SWING (left)** phases should be symmetric; **LEFT_FOOT** and **RIGHT_FOOT** frames should move identically. Similarly, **DOUBLE (right-left)** and **DOUBLE (left-right)** will be symmetric (we will develop these in future weeks).

**1.** Generate a feasible motion plan for the **SWING (right)** phase of the walking gait using Optragen. I suggest the following sequence of steps:
  ***a.)** Select your stance frame to the be the **LEFT_FOOT** frame
  ***b.)** Define an initial joint configuration for this phase of the gait. Both feet must be in contact with the ground. Further, the initial positions of both feet should not coincide; the **RIGHT_FOOT** frame should begin 'behind' the **LEFT_FOOT** frame - remember that this initial configuration must be identical to the final configuration of the **DOUBLE** (left-right) phase of the gait (since gaits are cyclic in nature). Because of this, you will have to consider and define the initial configuration of your biped such that it is 'mid-stride'.
  ***c.)** Design a trajectory (eg. set of waypoints) such that your **RIGHT_FOOT** frame begins on the ground (you can assume y = 0, w.r.t. your stance frame, is ground), lifts into the air, moves forward and steps back onto the ground. Remember that at the end of the trajectory, the **RIGHT_FOOT** frame orientation must be aligned such that the foot is level with the ground.
  ***d.)** You must consider additional constraints:
    ***(1)** the CoM projection onto the ground (expression you formulated last week) should remain within the bounds of the stance foot 
    ***(2)** the knee should not invert (to remain humanly-realistic) 
    ***(3)** your **RIGHT_FOOT** frame should not penetrate the ground
  ***e.)** Use Optragen/SNOPT to generate your trajectory

The Optragen examples demonstrate path planning between an initial and final configuration. If you have more than 2 waypoints, you can run the relevant portions of your Optragen script multiple times (eg. in a loop), where each iteration generates the trajectory between consecutive pairs of waypoints. The final trajectory is the concatenation of the trajectory pieces generated during each iteration.

**Please generate:**

**a.** An animation of your biped model executing the **SWING (right)** phase of the gait. The biped and all relevant frames (eg. **TORSO**, **RIGHT_FOOT**, **LEFT_FOOT**) should be displayed along with the CoM. The animation should display the robot with respect to the stance frame (ie. **LEFT_FOOT**).

**b.** A plot of all joint angles over time. Please remember to label your axes and title your plots!