From 866499fe367bfb3b20a7f65d8961ed29e757d797 Mon Sep 17 00:00:00 2001 From: Dawn Hustig-Schultz Date: Fri, 29 Apr 2016 17:24:16 -0700 Subject: [PATCH] Delete JSONGeneralFeedbackControl.cpp No longer using this app. --- .../JSONGeneralFeedbackControl.cpp | 637 ------------------ 1 file changed, 637 deletions(-) delete mode 100644 src/dev/dhustigschultz/BP_SC_SymmetricSpiral_All/JSONGeneralFeedbackControl.cpp diff --git a/src/dev/dhustigschultz/BP_SC_SymmetricSpiral_All/JSONGeneralFeedbackControl.cpp b/src/dev/dhustigschultz/BP_SC_SymmetricSpiral_All/JSONGeneralFeedbackControl.cpp deleted file mode 100644 index e84dd4c73..000000000 --- a/src/dev/dhustigschultz/BP_SC_SymmetricSpiral_All/JSONGeneralFeedbackControl.cpp +++ /dev/null @@ -1,637 +0,0 @@ -/* - * 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. -*/ - -/** - * @file JSONGeneralFeedbackControl.cpp - * @brief A controller for the template class BaseSpineModelLearning. - * @Includes more metrics, such as center of mass of entire structure. - * @author Brian Mirletz, Dawn Hustig-Schultz - * @version 1.1.0 - * $Id$ - */ - -#include "JSONGeneralFeedbackControl.h" - - -// Should include tgString, but compiler complains since its been -// included from BaseSpineModelLearning. Perhaps we should move things -// to a cpp over there -#include "core/tgSpringCableActuator.h" -#include "core/tgBasicActuator.h" -#include "controllers/tgImpedanceController.h" -#include "tgCPGGeneralActuatorControl.h" -#include "tgCPGGeneralCableControl.h" - -#include "dev/dhustigschultz/BigPuppy_SpineOnly_Stats/BaseQuadModelLearning.h" -#include "helpers/FileHelpers.h" - -#include "learning/AnnealEvolution/AnnealEvolution.h" -#include "learning/Configuration/configuration.h" - -#include "dev/CPG_feedback/CPGEquationsFB.h" -#include "dev/CPG_feedback/CPGNodeFB.h" - -#include "neuralNet/Neural Network v2/neuralNetwork.h" - -#include - -#include -#include -#include - -//#define LOGGING -#define USE_KINEMATIC - -using namespace std; - -JSONGeneralFeedbackControl::Config::Config(int ss, - int tm, - int om, - int param, - int segnum, - double ct, - double la, - double ha, - double lp, - double hp, - double kt, - double kp, - double kv, - bool def, - double cl, - double lf, - double hf, - double ffMin, - double ffMax, - double afMin, - double afMax, - double pfMin, - double pfMax, - double maxH, - double minH) : -JSONQuadCPGGeneralControl::Config::Config(ss, tm, om, param, segnum, ct, la, ha, - lp, hp, kt, kp, kv, def, cl, lf, hf), -freqFeedbackMin(ffMin), -freqFeedbackMax(ffMax), -ampFeedbackMin(afMin), -ampFeedbackMax(afMax), -phaseFeedbackMin(pfMin), -phaseFeedbackMax(pfMax), -maxHeight(maxH), -minHeight(minH) -{ - -} -/** - * Defining the adapters here assumes the controller is around and - * attached for the lifecycle of the learning runs. I.E. that the setup - * and teardown functions are used for tgModel - */ -JSONGeneralFeedbackControl::JSONGeneralFeedbackControl(JSONGeneralFeedbackControl::Config config, - std::string args, - std::string resourcePath) : -JSONQuadCPGGeneralControl(config, args, resourcePath), -m_config(config) -{ - // Path and filename handled by base class - -} - -JSONGeneralFeedbackControl::~JSONGeneralFeedbackControl() -{ - delete nn; -} - -void JSONGeneralFeedbackControl::onSetup(BaseQuadModelLearning& subject) -{ - m_pCPGSys = new CPGEquationsFB(100000); - - Json::Value root; // will contains the root value after parsing. - Json::Reader reader; - - bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root ); - if ( !parsingSuccessful ) - { - // report to the user the failure and their locations in the document. - std::cout << "Failed to parse configuration\n" - << reader.getFormattedErrorMessages(); - throw std::invalid_argument("Bad filename for JSON"); - } - // Get the value of the member of root named 'encoding', return 'UTF-8' if there is no - // such member. - Json::Value nodeVals = root.get("nodeVals", "UTF-8"); - Json::Value edgeVals = root.get("edgeVals", "UTF-8"); - - std::cout << nodeVals << std::endl; - - nodeVals = nodeVals.get("params", "UTF-8"); - edgeVals = edgeVals.get("params", "UTF-8"); - - array_4D edgeParams = scaleEdgeActions(edgeVals); - array_2D nodeParams = scaleNodeActions(nodeVals); - - setupCPGs(subject, nodeParams, edgeParams); - - Json::Value feedbackParams = root.get("feedbackVals", "UTF-8"); - feedbackParams = feedbackParams.get("params", "UTF-8"); - - // Setup neural network - m_config.numStates = feedbackParams.get("numStates", "UTF-8").asInt(); - m_config.numActions = feedbackParams.get("numActions", "UTF-8").asInt(); - //m_config.numHidden = feedbackParams.get("numHidden", "UTF-8").asInt(); - - std::string nnFile = controlFilePath + feedbackParams.get("neuralFilename", "UTF-8").asString(); - - nn = new neuralNetwork(m_config.numStates, m_config.numStates*2, m_config.numActions); - - nn->loadWeights(nnFile.c_str()); - - initConditions = subject.getSegmentCOM(m_config.segmentNumber); - for (int i = 0; i < initConditions.size(); i++) - { - std::cout << initConditions[i] << " "; - } - std::cout << std::endl; -#ifdef LOGGING // Conditional compile for data logging - m_dataObserver.onSetup(subject); -#endif - -#if (0) // Conditional Compile for debug info - std::cout << *m_pCPGSys << std::endl; -#endif - m_updateTime = 0.0; - m_totalTime = 0.0; //For metrics. - bogus = false; - - metrics.clear(); - - //Getting the center of mass of the entire structure. - std::vector structureCOM = subject.getCOM(m_config.segmentNumber); - - for(std::size_t i=0; i<3; i++) - { - metrics.push_back(structureCOM[i]); - } - - //"metrics" is a new section of the controller's JSON file that is - //added in the getNewFile function in evolution_job_master.py - Json::Value prevMetrics = root.get("metrics", Json::nullValue); - - Json::Value subMetrics; - subMetrics["initial COM x"] = metrics[0]; - subMetrics["initial COM y"] = metrics[1]; - subMetrics["initial COM z"] = metrics[2]; - - prevMetrics.append(subMetrics); - root["metrics"] = prevMetrics; - - ofstream payloadLog; - payloadLog.open(controlFilename.c_str(),ofstream::out); - - payloadLog << root << std::endl; -} - -void JSONGeneralFeedbackControl::onStep(BaseQuadModelLearning& subject, double dt) -{ - m_updateTime += dt; - m_totalTime += dt; - if (m_updateTime >= m_config.controlTime) - { -#if (1) - std::vector desComs = getFeedback(subject); - -#else - std::size_t numControllers = subject.getNumberofMuslces() * 3; - - double descendingCommand = 0.0; - std::vector desComs (numControllers, descendingCommand); -#endif - try - { - m_pCPGSys->update(desComs, m_updateTime); - } - catch (std::runtime_error& e) - { - // Stops the trial immediately, lets teardown know it broke - bogus = true; - throw (e); - } - -#ifdef LOGGING // Conditional compile for data logging - m_dataObserver.onStep(subject, m_updateTime); -#endif - notifyStep(m_updateTime); - m_updateTime = 0; - } - - double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1]; - - /// Max and min heights added to config - if (currentHeight > m_config.maxHeight || currentHeight < m_config.minHeight) - { - /// @todo if bogus, stop trial (reset simulation) - bogus = true; - throw std::runtime_error("Height out of range"); - } - //every 100 steps, get the COM and tensions of active muscles and store them in the JSON file. - if(0){ - static int count = 0; - if(count > 100) { - std::cout << m_totalTime << std::endl; - - //Getting the center of mass of the entire structure. - std::vector structureCOM = subject.getCOM(m_config.segmentNumber); - std::cout << "COM: " << structureCOM[0] << " " << structureCOM[1] << " " << structureCOM[2] << " "; - std::cout << std::endl; - //Clear the metrics vector for ease of adding tensions. - //metrics.clear(); - - std::vector tmpStrings = subject.find ("all "); - - for(std::size_t i=0; igetTension() << std::endl; - } - std::cout << std::endl; - - for(std::size_t i=0; igetCurrentLength() << std::endl; - } - std::cout << std::endl; - - count = 0; - } - else { - count++; - } - } -} - -void JSONGeneralFeedbackControl::onTeardown(BaseQuadModelLearning& subject) -{ - scores.clear(); - metrics.clear(); - // @todo - check to make sure we ran for the right amount of time - - std::vector finalConditions = subject.getSegmentCOM(m_config.segmentNumber); - - const double newX = finalConditions[0]; - const double newZ = finalConditions[2]; - const double oldX = initConditions[0]; - const double oldZ = initConditions[2]; - - const double distanceMoved = sqrt((newX-oldX) * (newX-oldX) + - (newZ-oldZ) * (newZ-oldZ)); - - if (bogus) - { - scores.push_back(-1.0); - } - else - { - scores.push_back(distanceMoved); - } - - /// @todo - consolidate with other controller classes. - /// @todo - return length scale as a parameter - double totalEnergySpent=0; - - std::vector tmpStrings = subject.find ("all "); - - for(std::size_t i=0; igetHistory(); - - for(std::size_t j=1; j 0) // Vestigial code - motorSpeed = 0; - const double workDone = previousTension * motorSpeed; - totalEnergySpent += workDone; - } - } - - scores.push_back(totalEnergySpent); - - //Getting the center of mass of the entire structure. - std::vector structureCOM = subject.getCOM(m_config.segmentNumber); - - for(std::size_t i=0; i<3; i++) - { - metrics.push_back(structureCOM[i]); - } - - std::cout << "Dist travelled " << scores[0] << std::endl; - - Json::Value root; // will contain the root value after parsing. - Json::Reader reader; - - bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root ); - if ( !parsingSuccessful ) - { - // report to the user the failure and their locations in the document. - std::cout << "Failed to parse configuration\n" - << reader.getFormattedErrorMessages(); - throw std::invalid_argument("Bad filename for JSON"); - } - - Json::Value prevScores = root.get("scores", Json::nullValue); - Json::Value prevMetrics = root.get("metrics", Json::nullValue); - - Json::Value subScores; - subScores["distance"] = scores[0]; - subScores["energy"] = scores[1]; - - Json::Value subMetrics; - subMetrics["final COM x"] = metrics[0]; - subMetrics["final COM y"] = metrics[1]; - subMetrics["final COM z"] = metrics[2]; - - prevScores.append(subScores); - prevMetrics.append(subMetrics); - - root["scores"] = prevScores; - root["metrics"] = prevMetrics; - - ofstream payloadLog; - payloadLog.open(controlFilename.c_str(),ofstream::out); - - payloadLog << root << std::endl; - - delete m_pCPGSys; - m_pCPGSys = NULL; - - for(size_t i = 0; i < m_spineControllers.size(); i++) - { - delete m_spineControllers[i]; - } - m_spineControllers.clear(); -} - -void JSONGeneralFeedbackControl::setupCPGs(BaseQuadModelLearning& subject, array_2D nodeActions, array_4D edgeActions) -{ - - std::vector spineMuscles = subject.find ("all "); - - CPGEquationsFB& m_CPGFBSys = *(tgCast::cast(m_pCPGSys)); - - for (std::size_t i = 0; i < spineMuscles.size(); i++) - { - - tgPIDController::Config config(20000.0, 0.0, 5.0, true); // Non backdrivable - tgCPGGeneralCableControl* pStringControl = new tgCPGGeneralCableControl(config); - - spineMuscles[i]->attach(pStringControl); - - // First assign node numbers - pStringControl->assignNodeNumberFB(m_CPGFBSys, nodeActions); - - m_spineControllers.push_back(pStringControl); - } - - // Then determine connectivity and setup string - for (std::size_t i = 0; i < m_spineControllers.size(); i++) - { - tgCPGGeneralActuatorControl * const pStringInfo = m_spineControllers[i]; - assert(pStringInfo != NULL); - pStringInfo->setConnectivity(m_spineControllers, edgeActions); //May need a new function, written in a subclass, to deal with the long muscles, though this will be fine for segments 1 through 5. - - //String will own this pointer - tgImpedanceController* p_ipc = new tgImpedanceController( m_config.tension, - m_config.kPosition, - m_config.kVelocity); - if (m_config.useDefault) - { - pStringInfo->setupControl(*p_ipc); - } - else - { - pStringInfo->setupControl(*p_ipc, m_config.controlLength); - } - } - -} - -array_2D JSONGeneralFeedbackControl::scaleNodeActions (Json::Value actions) -{ - std::size_t numControllers = actions.size(); - std::size_t numActions = actions[0].size(); - - array_2D nodeActions(boost::extents[numControllers][numActions]); - - array_2D limits(boost::extents[2][numActions]); - - // Check if we need to update limits - assert(numActions == 5); - - limits[0][0] = m_config.lowFreq; - limits[1][0] = m_config.highFreq; - limits[0][1] = m_config.lowAmp; - limits[1][1] = m_config.highAmp; - limits[0][2] = m_config.freqFeedbackMin; - limits[1][2] = m_config.freqFeedbackMax; - limits[0][3] = m_config.ampFeedbackMin; - limits[1][3] = m_config.ampFeedbackMax; - limits[0][4] = m_config.phaseFeedbackMin; - limits[1][4] = m_config.phaseFeedbackMax; - - Json::Value::iterator nodeIt = actions.begin(); - - // This one is square - for( std::size_t i = 0; i < numControllers; i++) - { - Json::Value nodeParam = *nodeIt; - for( std::size_t j = 0; j < numActions; j++) - { - nodeActions[i][j] = ( (nodeParam.get(j, 0.0)).asDouble() * - (limits[1][j] - limits[0][j])) + limits[0][j]; - } - nodeIt++; - } - - return nodeActions; -} - - -array_4D JSONGeneralFeedbackControl::scaleEdgeActions - (Json::Value edgeParam) -{ - assert(edgeParam[0].size() == 2); - - double lowerLimit = m_config.lowPhase; - double upperLimit = m_config.highPhase; - double range = upperLimit - lowerLimit; - - array_4D actionList(boost::extents[m_config.segmentSpan] - [m_config.theirMuscles] - [m_config.ourMuscles] - [m_config.params]); - - /* Horrid while loop to populate upper diagonal of matrix, since - * its symmetric and we want to minimze parameters used in learing - * note that i==1, j==k will refer to the same muscle - * @todo use boost to set up array so storage is only allocated for - * elements that are used - */ - int i = 0; - int j = 0; - int k = 0; - - // Quirk of the old learning code. Future examples can move forward - Json::Value::iterator edgeIt = edgeParam.end(); - - int count = 0; - - while (i < m_config.segmentSpan) - { - while(j < m_config.theirMuscles) - { - while(k < m_config.ourMuscles) - { - if (edgeIt == edgeParam.begin()) - { - std::cout << "ran out before table populated!" - << std::endl; - /// @todo consider adding exception here - break; - } - else - { - if (i == 1 && j == k) - { - // std::cout << "Skipped identical muscle" << std::endl; - //Skip since its the same muscle - } - else - { - edgeIt--; - Json::Value edgeParam = *edgeIt; - assert(edgeParam.size() == 2); - // Weight from 0 to 1 - actionList[i][j][k][0] = edgeParam[0].asDouble(); - //std::cout << actionList[i][j][k][0] << " "; - // Phase offset from -pi to pi - actionList[i][j][k][1] = edgeParam[1].asDouble() * - (range) + lowerLimit; - //std::cout << actionList[i][j][k][1] << std::endl; - count++; - } - } - k++; - } - j++; - k = j; - - } - j = 0; - k = 0; - i++; - } - - std::cout<< "Params used: " << count << std::endl; - - assert(edgeParam.begin() == edgeIt); - - return actionList; -} -std::vector JSONGeneralFeedbackControl::getFeedback(BaseQuadModelLearning& subject) -{ - // Placeholder - std::vector feedback; - - const std::vector& spineCables = subject.find ("all "); - - double *inputs = new double[m_config.numStates]; - - std::size_t n = spineCables.size(); - for(std::size_t i = 0; i != n; i++) - { - std::vector< std::vector > actions; - - const tgSpringCableActuator& cable = *(spineCables[i]); - std::vector state = getCableState(cable); - - // Rescale to 0 to 1 (consider doing this inside getState - for (std::size_t i = 0; i < state.size(); i++) - { - inputs[i]=state[i] / 2.0 + 0.5; - } - - double *output = nn->feedForwardPattern(inputs); - vector tmpAct; - for(int j=0;j cableFeedback = transformFeedbackActions(actions); - - feedback.insert(feedback.end(), cableFeedback.begin(), cableFeedback.end()); - } - - - return feedback; -} - -std::vector JSONGeneralFeedbackControl::getCableState(const tgSpringCableActuator& cable) -{ - // For each string, scale value from -1 to 1 based on initial length or max tension of motor - - std::vector state; - - // Scale length by starting length - const double startLength = cable.getStartLength(); - state.push_back((cable.getCurrentLength() - startLength) / startLength); - - const double maxTension = cable.getConfig().maxTens; - state.push_back((cable.getTension() - maxTension / 2.0) / maxTension); - - return state; -} - -std::vector JSONGeneralFeedbackControl::transformFeedbackActions(std::vector< std::vector >& actions) -{ - // Placeholder - std::vector feedback; - - // Leave in place for generalization later - const std::size_t numControllers = 1; - const std::size_t numActions = m_config.numActions; - - assert( actions.size() == numControllers); - assert( actions[0].size() == numActions); - - // Scale values back to -1 to +1 - for( std::size_t i = 0; i < numControllers; i++) - { - for( std::size_t j = 0; j < numActions; j++) - { - feedback.push_back(actions[i][j] * 2.0 - 1.0); - } - } - - return feedback; -} -