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
"Merging" ZMP and UP/Down slope fitness

Made some implementation changes to allow merging the ZMP fitness and
the up/down slope fitness, while still keeping each datum separately.

* GetTopPop
Now the best population is selected considering only the fitness values
in GA.FitIDs.

* InitGen
Added the modifications to GA.FitFcn (which now includes the name of the
function and the IDs on the Fit vector that it will occupy). MOST LIKELY
WILL NEED FURTHER IMPLEMENTATION, ESPECIALLY FOR BACKWARDS COMPATIBILITY
WHEN INPUTTING AN OLD FILE.

* MOOGA
Added the modifications to GA.FitFcn.
Modified SlopeFit to save all the output when walking on the first slope
(only the steps on the desired final slope are saved usually to reduce
computations).
Implemented the combined Up/Down+ZMP slope fitness function UDZMPFit.

* MOOGA/Plot
Added the modifications to GA.FitFcn.

* MOOGA/Run
Added the modifications to GA.FitFcn.

* RunSeq
Added the modifications to GA.FitFcn.
Bug squashing of new implementation

* MOOGA
INCREASED MaxBack to 0.2 (i.e. the max ankle length for scoring ZMP
points is 20cm)
Corrected the implementation of the new FitInd to properly display the
top results.

* Plot
Corrected the implementation of the new FitInd to properly display the
results.

* Quantile
Removed the option of duplicating the few top results when so many
scored 0 in almost all categories. This feature conflicted with the
Pareto implementation.

* MOOGA/Run
Added a call to the Neuron fire event when initializing the simulation.
Now the init cond for the CPG's phase is 0.

* RunSeq
Commented the call to handle a ground collision event when initializing
(only neuron fire event is called).

* Simulation/Run
That sneaky MOOGA was at it again... it managed to find a simulation
that sneaked under the ground and somehow ran underground super fast...
So, now we routinely check for "tunnels".

* WalkOnSlope
Instead of resetting the model stance foot back to (0,0), the terrain's
start at the last stance foot position.

* Terrain/Set
Added start_y and end_y as options to set. Don't know why they were
missing.
New ZMP fit limits and some improvements

* MOOGA
Fixed the implementation of Find so it can work with the new multiple
fitness output implementation.
Changed the ZMPFit output so it will reach 2 when the required foot
length is 0, 1 when the foot is as long as allowed and go quickly gown
to 0 as the foot gets longer.

* Plot
Added some code to normalize fitness values that go above 1 but still
display the maximum achieved.
Some more bugs and different MOOGA attempts

* Analyze
Reduced the step size.
Corrected the bug where the sorted slopes data wasn't saved.

* MOOGA
Added a SetFittest method to easily select amounts of top individuals to
keep / mutate and new random individuals as percentage of the population
(instead of fixed numbers).

* MOOGA/Plot
Corrected the fitness normalization code.

* MOOGA/Run
Commented the "post-processing" for ZMP which multiplied it by the best
up/down fitness achieved.

* Poincare
The simulation is now run without a set tstep (ode45 should choose
automatically) to prevent errors due to large tstep. To run faster, the
simulations are run without graphics when calculating the eigenvalues.

* Simulation/Render
Added an RSkip property to skip rendering some frames when the tstep is
very small. This can result in "jumpy" animations but it runs faster.

* Simulation/Run
Implemented the ability to run without a set tstep (it's set to 0.0111
to avoid conflicts with other parts of the code).

* Simulation
Increased the tolerance required for convergence (1e7 instead of 8e7)
and reduced the error size (from 1e4 to 1e5). Also implemented the
ability to run without a set tstep (it's set to 0.0111 to avoid
conflicts with other parts of the code).

Added some GA implementations to optimize controllers first on flat
terrain, then optimize FB gains on slopes. Also GenFBtuner runs a
simulation with a "flat genome" and allows to hand tune the FB gains.
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).