JSONHierarchyFeedbackControl.h

Go to the documentation of this file.

/*
 * 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_HIERARCHY_FEEDBACK_CONTROL_H
#define JSON_HIERARCHY_FEEDBACK_CONTROL_H

#include "dev/dhustigschultz/BP_SC_NoLegs_Stats/JSONQuadCPGControl.h"

#include <json/value.h>

// Forward Declarations
class neuralNetwork;
class tgSpringCableActuator;


class JSONHierarchyFeedbackControl : public JSONQuadCPGControl
{
public:

struct Config : public JSONQuadCPGControl::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
                int ohm = 10,
                int thm = 10,
                int olm = 10,
                int tlm = 10,
        int ohighm = 5,
        int thighm = 5,
                double hf2 = 20.0,
                double ffMax2 = 0.0
        );
        
        const double freqFeedbackMin;
        const double freqFeedbackMax;
        const double ampFeedbackMin;
        const double ampFeedbackMax;
        const double phaseFeedbackMin;
        const double phaseFeedbackMax;

                //So that these limits don't have to be hardcoded:
                const double maxHeight;
                const double minHeight;
        
        // Values to be filled in by JSON file during onSetup
        int numStates;
        int numActions;

                // New values of numMuscles, for non-spine segments:
                int ourHipMuscles;
                int theirHipMuscles;
                int ourLegMuscles;
                int theirLegMuscles;
        int ourHighMuscles;
        int theirHighMuscles;

                // These are for the higher level CPGs
                const double highFreq2;
        const double freqFeedbackMax2;
    };

    JSONHierarchyFeedbackControl(JSONHierarchyFeedbackControl::Config config,   
                                                                                                                std::string args,
                                                                                                                std::string resourcePath = "");
    
    virtual ~JSONHierarchyFeedbackControl();
    
    virtual void onSetup(BaseQuadModelLearning& subject);
    
    virtual void onStep(BaseQuadModelLearning& subject, double dt);
    
    virtual void onTeardown(BaseQuadModelLearning& subject);
                
protected:

    virtual void setupCPGs(BaseQuadModelLearning& subject, array_2D spineNodeActions, array_2D legNodeActions, array_4D spineEdgeActions, array_4D hipEdgeActions, array_4D legEdgeActions);

    virtual void setupHighCPGs(BaseQuadModelLearning& subject, array_2D highNodeActions, array_4D highEdgeActions);

    //If need to couple the other way, then may need to pass more arrays as parameters
    virtual void setupHighLowCouplings(BaseQuadModelLearning& subject, Json::Value highLowEdgeActions); 
    
    virtual array_2D scaleNodeActions (Json::Value actions, double highFreq, double freqFeedbackMax);

    virtual array_4D scaleEdgeActions (Json::Value actions, int segmentSpan, int theirMuscles, int ourMuscles); 
 
    std::vector<double> getFeedback(BaseQuadModelLearning& subject);
    
    std::vector<double> getCableState(const tgSpringCableActuator& cable);
    
    std::vector<double> transformFeedbackActions(std::vector< std::vector<double> >& actions);
    
    JSONHierarchyFeedbackControl::Config m_config;

    // @todo: doing separate vectors of controllers, until I can find a way to make them into a vector of vectors, without segfaulting.
    std::vector<tgCPGActuatorControl*> m_spineControllers;

    std::vector<tgCPGActuatorControl*> m_leftShoulderControllers;
    std::vector<tgCPGActuatorControl*> m_rightShoulderControllers;

    std::vector<tgCPGActuatorControl*> m_leftHipControllers;
    std::vector<tgCPGActuatorControl*> m_rightHipControllers;

    std::vector<tgCPGActuatorControl*> m_leftForelegControllers;
    std::vector<tgCPGActuatorControl*> m_rightForelegControllers;

    std::vector<tgCPGActuatorControl*> m_leftHindlegControllers;
    std::vector<tgCPGActuatorControl*> m_rightHindlegControllers;

    //std::vector<tgCPGActuatorControl*> m_hipControllers;
    //std::vector<tgCPGActuatorControl*> m_legControllers;

    std::vector<tgCPGActuatorControl*> m_highControllers;
    
    // @todo generalize this if we need more than one
    neuralNetwork* nn;

    std::vector< std::vector<double> > m_quadCOM;

};

#endif // JSON_HIERARCHY_FEEDBACK_CONTROL_H