JSONMGFeedbackControl.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 JSON_MG_FEEDBACK_CONTROL_H
#define JSON_MG_FEEDBACK_CONTROL_H

#include "JSONMGCPGGeneralControl.h"

#include <json/value.h>

// Forward Declarations
class neuralNetwork;
class tgSpringCableActuator;

typedef boost::multi_array<double, 3> array_3D; //Will treat entire spine (and eventually body) as one big segment, so reducing the multi_array by one dimension.

class JSONMGFeedbackControl : public JSONMGCPGGeneralControl
{
public:

struct Config : public JSONMGCPGGeneralControl::Config
    {
    public:
        Config( int ss,
        int tm,
        int om,
        int param,
        int segnum = 6,
        double ct = 0.1,
        double la = 0,
        double ha = 30,
        double lp = -1 * M_PI,
        double hp = M_PI,
        double kt = 0.0,
        double kp = 1000.0,
        double kv = 100.0,
        bool def = true,
        double cl = 10.0,
        double lf = 0.0,
        double hf = 30.0,
        double ffMin = 0.0,
        double ffMax = 0.0,
        double afMin = 0.0,
        double afMax = 0.0,
        double pfMin = 0.0,
        double pfMax = 0.0,
                double maxH = 60.0, //May need to tune this value more
                double minH = 1.0   //Perhaps same
        );
        
        const double freqFeedbackMin;
        const double freqFeedbackMax;
        const double ampFeedbackMin;
        const double ampFeedbackMax;
        const double phaseFeedbackMin;
        const double phaseFeedbackMax;

                const double maxHeight;
                const double minHeight;
        
        // Values to be filled in by JSON file during onSetup
        int numStates;
        int numActions;
        
    };

    JSONMGFeedbackControl(JSONMGFeedbackControl::Config config, 
                                                                                                                std::string args,
                                                                                                                std::string resourcePath = "");
    
    virtual ~JSONMGFeedbackControl();
    
    virtual void onSetup(BaseQuadModelLearning& subject);
    
    virtual void onStep(BaseQuadModelLearning& subject, double dt);
    
    virtual void onTeardown(BaseQuadModelLearning& subject);
                
protected:
//ToDo: Need to restructure the for loops in here, so that have separate cases for the first and last segments (long muscles)....
    virtual void setupCPGs(BaseQuadModelLearning& subject, array_2D nodeActions, array_3D edgeActions);
    
    virtual array_3D scaleEdgeActions (Json::Value edgeParam);
    virtual array_2D scaleNodeActions (Json::Value actions);

//ToDo: May have to write a new function in this subclass, to handle the fact that we'll be skipping segments now. Not sure if scale edge actions will work in all cases.... and maybe there's something better?
    
    std::vector<double> getFeedback(BaseQuadModelLearning& subject);
    
    std::vector<double> getCableState(const tgSpringCableActuator& cable);
    
    std::vector<double> transformFeedbackActions(std::vector< std::vector<double> >& actions);
    
    JSONMGFeedbackControl::Config m_config;

    std::vector<tgCPGMGActuatorControl*> m_spineControllers;
    
    neuralNetwork* nn;
    
};

#endif // JSON_MG_FEEDBACK_CONTROL_H