The past several assignments have culminated in generation of a joint trajectory that moves a foot frame of your biped (relative to the torso frame) from one arbitrary pose in planar space, to another. You utilized resolved-rate inverse kinematics to generate the joint trajectory that accomplished this.

This week, we'll use the Optragen package and underlying nonlinear optimizer, SNOPT, to generate joint trajectories that accomplish our desired foot frame motion (again, with the TORSO frame assumed to be our stance frame). The prescribed foot frame motion will be specified as a set of constraints (eg. starting constraint, ending constraint and other constraints governing motion between start and end of the trajectory) as well as a cost function (eg. energy expended by the joints). Optragen will then solve for the optimal control/joint trajectory that satisfies these constraints while minimizing the cost.

1. To setup Optragen on your machine, you will need to clone the following Git repository. Please make sure you clone the 'refactor' branch and NOT the 'main' branch:

https://github.com/ivalab/Optragen

Next, download the following contents and place them in a 'snopt/' directory:

https://www.dropbox.com/sh/xirrteofcx0wkt2/AADRhHSPYDTcjcyqT2AwbRL-a?dl=0

The latter link contains SNOPT .mex files; variants are compiled for Windows (32- and 64-bit), Linux (64-bit) and Mac (64-bit).

Your objective is to take the prescribed foot frame trajectory you defined last week and, instead, use Optragen to generate the corresponding joint trajectory. Alternatively, if you'd like, you may define a new trajectory for your foot frame.

In the Optragen repository, the following demo script serves as a good contextual example to work off of and/or modify to meet your needs: Optragen/examples/pathPlanning/planar2R.m

In the above demo script, please make sure to change SNOPTPATH to be the location of the 'snopt/' directory you downloaded from Dropbox. Successfully running this script will additionally serve as validation that Optragen and SNOPT are correctly configured on your machine.

At a minimum, you'll need to define a constraint for your starting condition and one for your ending condition. You'll also need to specify a cost function; you can define your cost function to be energy-like, similar to how the demo script already defines it.

Generate:

a. Plots of each relevant joint angle vs. time (ie. 3 plots)

b. An animation of your biped model over time (again, assuming TORSO frame is your stance frame) following the joint trajectory solution coming out of Optragen. You will have to perform forward kinematics (eg. using fwd_kinematics( … )) to obtain the the actual accomplished trajectory of each frame.

2. Optragen will become very convenient in the coming weeks. It will allow you to accomplish inverse kinematics for your walking gaits while satisfying several other constraints - eg. CoM-based passive stability, velocity limits, etc.

In preparation for following weeks, please formulate the constraint that restricts the projection of the CoM (of the entire robot) to fall within a connected region on the ground (ie. your 1-D support polygon). You should express the constraint assuming:

a. LEFT_FOOT is your stance frame

and

b. RIGHT_FOOT is your stance frame.

No coding needed - just provide the mathematical expression for each case. You may formulate it with respect to $g$'s (ie. $SE(2)$ elements); if you do, please take time to clearly define your $g$'s as functions of your joint angles.