Escape_T6Controller.h

/*
 * Copyright © 2012, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All rights reserved.
 * 
 * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed
 * under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0.
 * 
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
 * either express or implied. See the License for the specific language
 * governing permissions and limitations under the License.
 */

#ifndef ESCAPE_T6CONTROLLER
#define ESCAPE_T6CONTROLLER

#include <vector>

#include "core/tgObserver.h"
#include "learning/Adapters/AnnealAdapter.h"

// Forward declarations
class Escape_T6Model;
class tgBasicActuator;

//namespace std for vectors
using namespace std;

class Escape_T6Controller : public tgObserver<Escape_T6Model>
{
    public:
        // Note that currently this is calibrated for decimeters.
        Escape_T6Controller(const double prefLength=5.0);

        virtual ~Escape_T6Controller() { }

        virtual void onSetup(Escape_T6Model& subject);

        virtual void onStep(Escape_T6Model& subject, double dt);

        virtual void onTeardown(Escape_T6Model& subject);

    protected:
        virtual vector< vector <double> > transformActions(vector< vector <double> > act);

        virtual void applyActions(Escape_T6Model& subject, vector< vector <double> > act);

    private:
        vector<double> initPosition; // Initial position of model
        const double m_initialLengths;
        double m_totalTime;
        double const maxStringLengthFactor; // Proportion of string's initial length by which a given actuator can increase/decrease

        // Evolution and Adapter
        AnnealAdapter evolutionAdapter;
        vector< vector<double> > actions; // For modifications between episodes

        // Muscle Clusters
        int nClusters;
        int musclesPerCluster;
        vector<vector<tgBasicActuator*> > clusters; 

        // Sine Wave Data
        double* amplitude;
        double* angularFrequency;
        double* phaseChange;
        double* dcOffset;

        void setupAdapter();

        double totalEnergySpent(Escape_T6Model& subject);

        void setPreferredMuscleLengths(Escape_T6Model& subject, double dt);

        void populateClusters(Escape_T6Model& subject);

        void initializeSineWaves();

        double displacement(Escape_T6Model& subject);

        std::vector<double> readManualParams(int lineNumber, string filename);

        void printSineParams();
};

#endif // ESCAPE_T6CONTROLLER