Small bugs + performance tweaks

Corrected a bug where the indices of Fit where transposed (when topPop
is filled with just one pareto front)

Added a Segments property to speed up the AdvSeq calculation.
Running the GA

Made a bunch of changes...
* Small change in controller's ClearTorques to correct a bug when using
only 1 pulse.
* Genome now has a crossover type property.
* When instantiating Genome, KeyLength and KeyExtras are defined before
calling SetKeys and SetRange.
* Added new method SwitchDir to turn around a controller that moves
backwards.
* Slowly deleted transferred code from old algorithm.
* GetTopPop now takes only the number of top sequences desired and uses
the fitness data from within the MOOGA object.
* Added a few properties to MOOGA to make it work like an actual
optimization algorithm
* Added some fitness functions to MOOGA.
** STILL MISSING: Energy fitness and Stability fitness **
* Sim can now be passed a figure handle so it won't open a new figure
* Ground clearance is checked again after going through all the events.
THIS NEEDS MORE TESTING. THIS. IS. SPARTA!!!
* Sim.Out now has also the intended 'tend'
* Initially set Once to 1, as should be
* Re-wrote TestGA completely. Now it tests the genetic algorithm like
it's meant to be. Unfortunately didn't have time to think of easy ways
to keep a simple test of MOOGA now that the Run method exists (unless
it's not used).
* Added InitGen method to MOOGA. It initializes the optimization
variables: Seqs, Fit, randomly or using a provided input file.
* Added Run method to MOOGA. It runs the optimization algorithm: runs
tests and uses fittest results to create new generation.
Fine tuning the MOOGA + first results

* Added a mutation to the children inside the Crossover method.
* Added a Try variable to Mutate to prevent high recursion levels (up to
10 currently).
* Mutate now returns the result of the mutation as a cell (num +
string).
* RandSeq saves the sequence also when it's checked. This way, if the
sequence is modified to shorten a pulse, the corrected sequence is
saved.
* Changed (fixed?) the normalization of HeightFit.
* Added the parallel computing lines to MOOGA's Run method
* When a genome is reversed (because of negative VelFit) it is checked
before being saved.
* MOOGA.Run stops after GA.Generations (calculates the fitness but
doesn't create a new generation).
* Tested the algorithm by finding a controller that has an externally
triggered pulse at the ankle, a swing pulse + limited ankle pulse.
* Added a RunSeq method to test the sequences obtained by the algorithm.
Should come quite handy for the GUI.
Cost of transport fitness

Added a method to calculate the COT fitness.
Corrected a bug where sim.nOuts wouldn't be set when Graphics == 0,
causing the torques not to be saved.
Bug-zapping

* Corrected the distance fitness calculation (affecting nrg_fit too).
The base position of the stance leg needed to be reset.
* Added the fitness calculation code to RunSeq, so the genome's fitness
is shown after it's run.
* The algorithm produces good results for the specified controller
structure.
** TO-DO ** : Check what happens to each fitness if the model converges
before reaching Tend. Add fitness for Poincare Map eigenvalues.
Added a closed loop impulsive controller
Long term commit

Here comes a long commit...

* AddPulse
Added zero gains when the FB type isn't 2 (otherwise the FB calculation
doesn't work)

* Controller
Set default values to 0
Erased deprecated LoadParameters function (replaced by Gen.Decode and
Controller.Set)

* Decode
When CheckGenome fails, instead of throwing an error, a random sequence
is used to allow the algorithm to continue running

* Genome
Fixed a wrong implementation of SwitchDir

* Mutate
Added a CheckGenome call even if no mutation happens, in case the
mutation is used on an invalid child genome
There are still some bugs that I didn't manage to catch which keep
throwing the 'Sequence outside allowed genome range' error... must
investigate further.

* RandSeq
Added some bug checks. So far no bugs caught

* GetTopPop
Avoided call to Pareto when there's just a single fitness function.
Instead the vector is sorted and returned as TopIDs.

* InitGen
Fixed a bug when loading an old file (changed to Progress+1 after
changing Progress on MOOGA.Run)

* MOOGA
Changed Progress' initial value, erased deprecated InitSim.
Added some fitness functions:
- EigenFit: runs Poincare and returns as the fitness the distance from
the unit circle of the largest eigenvalue.
- Uphill and Downhill fitness: Added some functions to calculate the
up/downhill fitness (max slope reached) either straight from the
simulation (if it already run on a slope) or by running a simulation on
a slope.

* Pareto
If input is single fitness then sort data and return as fronts (much
faster)

* MOOGA/Run
Added base genome decoding (if provided)
Started displaying some info: time to run, sequence in progress, etc

* RunSeq
Now displays the sequence

* Poincare
Now checks if some of the simulations didn't finish a step and returns
eigenvalues of 2.

* Simulation/Render
Now displays the IC error from step to step to show convergence

* Simulation/Run
There seems to be a problem calculating the step period sometimes so
we'll try another "back-up" way

* MOOGA/Plot
Added a plot method for displaying different algorithm results. Right
now it only plots the fitness over the generations.

* TwoMassJumper
Added a two-mass jumper model but didn't finish the implementation. It
can render a nice model though :)
More debugging

* Genome
Added seq2str function for displaying a sequence nicely

* MOOGA
Changed the way of dealing with large eigenvalues
*** THIS IS STILL A BUG. Not sure how a controller can converge to a
period 1 LC and still have an eigenvalue larger than 1
The IC for checking the performance on up/downhill are now se to
IClimCyc if the controller converged.

* MOOGA/Run
Added some functions for showing the estimated time for the current
generation
Now the best genomes fitness is copied to the next generation so they
are not run again and again (big oopsie there)

* RunSeq
Added option of running the sequence for a time 'tend'

* GAfine
Implemented a function to increase the allowed genome range after each
generation

* RescueFit
Implemented a method for recovering the best genomes from all the
generations, in case some got lost along the way. I think I fixed the
bug that cause it so it might be unnecessary

* GAflat
Run this function for optimizing parameters only on flat terrain

* CompassBiped
Changed the "angle from vertical" condition that checks if the robot is
falling. Now it takes into account the incline as well.

* CompassBiped, Controller, Terrain
These classes now derive from mixin.Copyable so it's possible to
properly make copies of them

* Simulation
Implemented a deep copy method to copy also objects of Mod, Con and Env
Almost fully operational, still one bug to catch (at least)

* Controller
Removed deprecated pSoff (implementation for turning off pulses)
Added a reset function to set the pulses to 0 (needed when making a copy
of the Controller object)

* Mutate
Implemented a different way of dealing with bad mutation. There's still
a weird bug that appears only during the first generations and seems to
"break" all the genome sequences...

* InitGen
Corrected an old bug that shows up when using a small population as a
base for a new, larger population.

* MOOGA
Worked some more on the up/downhill fitness calculations and finally got
the simulations running properly! :+1:  (not the cleanest implementation
though)

* Simulation/Init
Corrected an initialization error thrown when Sim.IC is not initialized
and Graphics = 1.

* Terrain/Render
Vastly improved rendering speed by removing the for loop, now that
Terrain.Surf works with vectors :+1:            * Terrain: removed the
function handles which were causing trouble when copying the object and
moved back to the old implementation.
Small bugs + ZMP

* InitGen
Still correcting bugs related to Progress

* MOOGA
Changed implementation of the fitness functions to also output Sim.Out
(when needed)
Implemented a function that calculates the ZMP and returns the fitness
based on the foot size that would be required. STILL NEEDS TO BE CHECKED
PROPERLY

* MOOGA/Run
Changed to parpool (MATLAB 2014)
Started implementing the pre-processing for the ZMP fitness function

* Simulation/Render
Corrected a bug that occurs when the display window is closed

Added NumAnalysis and Plots from old simulation, for reference
Most bugs caught, 99% functional

There are still some bugs out there. Mostly the one with the failed
sequences after the first (or second) generation. Also, it looks like
there was some problem with the ZMP calculation which still needs to be
checked.

* CompassBiped
Corrected the way in which it's checked whether the robot falls down
(large hip angle)

* Controller
Added an adaptation "low pass filter" to prevent strong jumps in torques
when the robot falls and gets a bad slope "measurement"

* GetTopPop
Added an option to use weighted population when selecting the best
genomes. Genomes from the first pareto front will have more chances of
reproducing than the ones on the second, third and so on.

* MOOGA/MOOGA
Added some more initialization parameters for up/downhill simulations.
Also, when removing the push-off torque from the ankle torques, the
pulse amplitude is not updated according to the slope (as happens in
simulation).

* MOOGA/Run
Added the required preparation for running the ZMPFit function and
implemented the weighted pairing

* RunSeq
Added the ZMPFit processing functions.

* Simulation/Render
Corrected the torque display bug cause when running two different
simulations on the same render figure

* GAfine
The mutation strength is now reduced after each generation (to converge
more easily)
Some more improvements to the algorithm

* Controller
Added a saturation parameter to bound the controller's output.
Corrected a small bug regarding events.

* MOOGA
Changed the implementation of the fitness functions to be static (this
way they can be called without requiring a MOOGA instance)
ZMPFit now returns 0 if the robot didn't take at least 3 good steps.

* UpdateStats
Corrected a small bug when running the simulation with EndCond = [1,N]
(for N steps on end_slope)

* GAfine
Ran the algorithm with an increased number of torques per joint (higher
complexity)

* TestGen
Added some code for testing the genomes optimized by GAfine
Merge from Simulation
Merge remote-tracking branch 'origin/Simulation'
Early beta of genome analysis after evolution

* CheckGenome / Decode
Removed mergetool garbage. Don't know how it got there...

* Analyze
Added a function that runs the simulation with the selected genome over
a range of slopes, until the robot fails to walk. It then calculates
different performance parameters such as the limit cycle, ZMP, power
required, etc.

* MOOGA
Added Find function to search for genomes that fit certain fitness
requirements
Transferred the "impulse clean-up" function to Simulation

* Poincare
Added two lines to reset the controller and apply the effects of ground
contact. Maybe this will fix the BUG where the robot converges and yet
the eigenvalues come back larger than 1.

* Simulation/Render
Small changes to how the window size is changed.

* GAfine
Set PMFull to 1
More analysis and bug catching

* Analyze
Solved a bug caused when only a 0 slope is loaded from a saved file.
Moved most of the code to 2 functions: "WalkOnSlope" and
"SavePerformance".
Now 2 attempts are made for each slope, starting from the previous slope
and starting from a 0 slope. Also, 3 more slopes are tried after the
first failed one (so far this hasn't helped to find more stable slopes).
Also, slopes are now increased by base_d instead of 1, in preparation
for finer analysis.

* MOOGA/Find
Added the possibility of searching for fit genomes on previous
generations.

* MOOGA/Run -> Simulation/Render
Moved the cla command for clearing previous renders

* Simulation/Render
Changed the way in which the figure size/position is changed so it
doesn't reset when the user modifies it, but still works the first time.

* Simulation/Run
Caught a bug caused when the "Check clearance" statement is triggered (a
ground contact trying to slip through the cracks) and the impulsive
controller was not activated.

* Simulation/Simulation
Caught a bug when separating the impulsive torques, where lastPhi needed
to be updated before the first call to Controller.Adaptation.
Also improved the way in which impulsive torques are detected and
separated (MAY NEED MORE TESTING).
Poincare+ZMP+more bugs and new MOOGA options

* Controller
The Reset method can now reset the switches to a certain CPG phase. This
caused some one of the bugs with calculating the Poincare map.
Also, the adaptation function now receives the slope directly instead of
calculating it from the leg angles. This should also fix another bug
with calculating ZMP since the slope would change when the IC where
disturbed.

* Analyze
Added some formatting to the results to show "OK" or "FAILED" in green
and red after each run. Also applied the controller adaptation changes.

* MOOGA
Added an option to add new random genes to each generation.
Added a case to the Find method that shows the max fitness when called
without arguments and improved the way that output is displayed.

* MOOGA/Plot
Added some colors and moved the strsplit method to a separate file.

* MOOGA/Run
Corrected a bug with the detection of which fitness is being calculated
(uses strfind now to look for specific names).
Added post-processing for ZMPFit to "normalize" the fitness by
multiplying it by the largest slope attained (up or down).

* Simulation/Run
In order to process the Torques after running the different simulations,
a new variable "Slopes" was added to store the current Slope at each
moment.

* Simulation
Worked on improving the GetCleanTorques method.

* GAfine
Modified the code so that it can run with zero ankle torques
(under-actuated).
New slope fitness functions and some bug zapping

Commented out some of the "debugging" messages that I've been ignoring
for the past week anyway.

* Analyze
Corrected some bugs and added a code to skip the second attempt when the
first ran out of time (didn't fall or converge).
Moved most of the WalkOnSlope function to a method in Simulation.

* MOOGA - Fitness functions
Distance travelled is now calculated based on Sim.Out.SuppPos for all
fitness functions (except NrgEffFit).
Added a SlopeFit (Up/Down) function that replaces the old Up/Downhill
function. In this case the slope is increased gradually as well but
after each increase the robot is required to "show convergence" before
increasing the slope further.

* CheckConvergence
Added some code to check sooner if the simulation is converging (rather
than waiting until diff<minDiff). With "doGoNoGo" set to 1, the
simulation can be extended while converging to allow it to fully
converge (up to 3 times the original simulation time). With "doGoNoGo"
set to 2, the simulation can be stopped ahead of time if it shows that
it's converging/diverging.

* Simulation/Run
Commented out the "double-check" for ground clearance which seemed to be
causing more troubles than it solved.
T is now stored as sim.Out.T during the run (so it can be used by
CheckConvergence).

* Simulation
Added a JoinOuts method that allows to join the outputs from different
simulations (useful for ZMPFit and SlopeFit).
Updated GetCleanPulses to work properly with the new JoinOuts method.

* Terrain
Corrected the display bug in Terrain.