====== Bipedal Robot Locomotion: Extend Gait to Two Legs  ======
----------------------------------------------------------------

This week we'll complete the **DOUBLE (right-left)** phase of the gait. Keep in mind the initial joint configuration of your robot for this phase of the gait should be identical to its final configuration from the **SWING (right)** phase you have developed over the past two weeks. The **SWING (left)** and **DOUBLE (left-right)** phases are then mirror images of **SWING (right)** and **DOUBLE (right-left)**, respectively. One cycle of the full gait is accomplished by sequencing these phases together: **SWING (right)** -> **DOUBLE (right-left)** -> **SWING (left)** -> **DOUBLE (left-right)**. In other words, the right foot/leg first swings, the CoM is adjusted while both feet are in contact with the ground, the left foot/leg swings and finally the CoM is adjusted again while both feet contact the ground. The final joint configuration for the **DOUBLE (right-left)** phase should be identical to the initial configuration needed for the **SWING (left)** phase.

**1.** Generate a feasible motion plan for the **DOUBLE (right-left)** phase of the walking gait using Optragen. During this phase, both feet must be in static contact with and oriented with the ground. The fundamental purpose of this phase is to transfer the CoM such that its projection onto the ground is now over the right foot; this should then allow the **SWING (left)** phase to proceed. From previous weeks, you should have defined your **SWING** phase such that the CoM projection begins within the bounds of the stance foot (ie. with an appropriate initial joint configuration). Another way, then, to interpret this is that the **DOUBLE** phase should transition the biped joint configuration to match the required initial joint configuration of the follow-on **SWING** phase.

Similar constraints as before still apply:
  ***(1)** the CoM projection onto the ground should remain within the bounds of the feet contacting the ground (ie. 1-D support 'polygon' formed by both feet)
  ***(2)** the knees should not invert (to remain humanly-realistic)

**Please generate:**

An animation of your biped model executing the **DOUBLE (right-left)** 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**).

**2.** Generate an animation of the full walking gait. The Biped Matlab class skeleton included a ''gR0'' member variable. Use this to track the spatial pose of your robot. Begin with the **SWING (right)** phase where the stance frame is **LEFT_FOOT** and ''gR0'' is initially set to the identity (ie. ''gR0'' represents the pose of the current stance frame relative to the global origin). Execute **SWING (right)** followed by **DOUBLE (right-left)** where the stance frame remains **LEFT_FOOT**. Prior to executing **SWING (left)**, set ''gR0'' to hold the spatial pose of the next stance frame, **RIGHT_FOOT** (you can compute this as ''gR0'' will always hold the $SE(2)$ pose of the previous stance frame and you know the pose of the next stance frame, relative to the previous, from forward kinematics). Then execute **SWING (left)** and **DOUBLE (left-right)** where **RIGHT_FOOT** is the stance frame. This is one gait cycle. Do this for 3 gait cycles.

**Please generate:**

An animation of your biped model executing 3 full gait cycles. With appropriate maintenance of ''gR0'', the animation should demonstrate your robot walking forward over 3 left-right strides. If the initial stance frame (**LEFT_FOOT**) began at the origin and each stride length was 5 cm, the final position of **LEFT_FOOT** should be $50$ cm $\times 3$ left foot strides $= +150$ cm relative to the global origin, along its x-axis.