Energy considerations

Added some equations to the CB model file to analyze how much energy is
lost to impact depending on leg aperture and angular velocities.
Merge branch 'Analysis' of https://github.com/lothas/CB_sim2 into Analysis

Conflicts:
	@CompassBiped/Model.mn
Fix merge conflict in Analysis
Worked on analysis

* Analyze
Added some features for period doubling analysis. The display function
now needs updating to include the new data. Corrected a small bug about
ExtPulses (see Simulation below).

* DisplayGen
Added a method for MOOGA that displays the data from a genome analysis
(and performs it if it hasn't been performed). The GUI can show
different plots: ICs, Eigenvalues, ZMP, Torques or Power required VS
Slope, as well as LC time series for each individual slope.

* GetCleanPulses
Method was moved to a separate file. I added some lines to identify
which pulses are external (based on sim.Con.ExtPulses) instead of
assuming it's pulse #1. This was causing trouble when a genome didn't
possess an external pulse. THIS NEW CORRECTION NEEDS CHECKING!!!

* Simulation
Decreased epsilon to 5e-6 (instead of 1e-5).
Moved GetCleanPulses to a separate file.
Merge remote-tracking branch 'origin/Analysis_Stash' into Analysis
Period doubling eigenvalues bug

Forgot to specify the output length when adding the slope datum to
Data.EigV.
Period doubling IC bug

When more than 1 set of IC is saved to Data.IC, this caused problems for
the next slope. To correct it, only 1:sim.stDim coordinates are grabbed
from Data.IC for the next slope.
Small improvements

Phased out the "Out" variable for the "Data" variable. Added a "Done"
field to know when all slopes have been processed.
Catching analysis bugs

* Analyze
Added code to save the genome and sequence as part of the output file.
Solved the issues with weird limit cycles (first slopes around 0 and
other problematic slopes) by resetting the biped's position to (0,0)

* DisplayGen
Improved the GUI. Added code to skip areas where the controller failed.
Used "ConnectVectors" to straighten lines with "jumping" points.
Added "Foot design" menu option to find the best foot size for ZMP
compliance.

* GetCleanPulses
Improved the automatic "cleaning" of impulses from the torque signal.
More MOOGA improvements

Reduced the max front and max back size allowed for ZMP.
Limited the slope fitness to 15 degrees and updated the UDZMP fitness
to: (Slope+5*ZMP)/20

* WalkOnSlope
Corrected an issue where the robot would start walking "on air" when a
new slope was introduced (initial terrain height was wrong). This should
improve the number of genomes able to climb slopes larger than 2
degrees.

* GA_ROBIO
Added arguments for calling the function. This should help implement a
MOOGA that runs first with a large population and then with a small
population.

* ParetoPlot
Implemented a simple script for plotting 2D Pareto fronts.
ZMP implementation changes

Implemented the option of stopping the simulation if a ZMP value is
exceeded. The limits are defined by the foot size in CompassBiped (A2T
and A2H).

* CompassBiped
Added the A2T and A2H properties and included them in Set.
Implemented a GetZMP method which calculates the ZMP and returns the
min/max value. While coding it I discovered that GetGRF's implementation
was wrong since CB.Torques wasn't getting updated!

* GetCleanPulses
Modified the arguments required while keeping it backwards compatible

* Simulation/Run
Added a ZMP check after each simulation segment. If the limits are
exceeded the simulation stops.

* GA_ZMPcond
Added a new GA function which uses the new ZMP limited simulation.

STILL NEEDS TO BE IMPLEMENTED ON Analyze.m

--------------

Also
* MOOGA/Plot
Implemented an option to plot only the max fitness without averages

* Stages
Calls GA functions in stages.
Plots and small bugs

** Worked on compass biped model equations and calculated a ZMP bound

* Analyze
Added the zone calculations and eigenvalue sorting to the analyze method
so they are already included in the genome analysis .mat file. Also
moved the ConnectVectors and SortLines functions into Analyze.

* DisplayGen
Removed ConnectVector and SortLines. The code for loading the results is
now executed in Analyze (check if file exists and/or perform analysis).

* MOOGA/Plot & PlotGen
Added "Gen" for plotting genome results for the paper. Partially
implemented.

* RunSeq
Changed the call to the CPG fire event for the more general
Con.Reset(phase).

* GA_ZMPcond
Switched positions of gain genes so all the upslope genes are together
and all the downslope genes are together.

* ParetoPlot
Updated the script to plot the newest results with vel/energy and
up/downslope.

* Plots4Paper
Added a function for preparing plots for the CB paper. Just add a genome
results and hit Run.
Some more debugging and GA attempts

* Controller
Added a PhaseDiff method to return the distance between 2 phases when
the range is wrapped 1->0.

* InitGen
Added extra implementation for not copying the fitness results when ReDo
is set to 1.

* MOOGA
Implemented a method for combining the values of 2 previously calculated
fitness functions. In retrospect, this can be done with another fitness
function so I'll deprecate this and implement the other method.
Also, reduced the max slope fitness to 10 degrees since most genomes
don't make it to 10 anyway.

* MOOGA/Run
Implemented the fitness combination method described above.

* CheckConvergence
Used the new Controller PhaseDiff method to calculate delta_{k+1}.

* Poincare
Added a verification that resets the CPG phase if the disturbance sends
it past P_threshold.
Used the new Controller PhaseDiff method to calculate delta_{k+1}.
NOTE: This didn't solve the issue with jumping eigenvalues when a pulse
occurs just before/after ground impact. I believe that the issue is with
ode45 and events.

* GA_UpDown
New GA file to be implemented

* GA_ZMPcond
Added the combined fitness function described above. This helps produce
genomes that are good for walking up and downslopes.
NOTE: This
UpDownFit

Reimplemented the fitness combination function.
Now the fitness functions used are: Velocity, Energy, Stability and
Up*Down. This encourages the selection of genomes that are both skilled
at walking up and down slope (the max up/down slope achieved is still
shown).
Hopefully reducing the number of fitness functions will encourage the
selection of genomes that converge (stability fitness>0).
Smaller slope change

Reduced delta_slope to 1 degree.
Corrected a bug in the Go-no-go implementation.
Convergence

* MOOGA
Only genomes that converged will get a score on UpDownFit (this should
weed out results that didn't converge).
Reduced the threshold required for Go-no-go convergence.

* CheckConvergence
Now the calculated diff gets stored in a Sim property.
Also changed Period to a single result (find 1 first) to reduce
computations.

* Simulation/Render
Now that ICdiff is stored, it can show the smallest diff and a guess as
to which period the gait will be.

* Plots4Paper
Changed the x axis of the torque signal to the CPG phase (instead of
time).