void JSONCPGControl::onStep(BaseSpineModelLearning& subject, double dt)
{
m_updateTime += dt;
if (m_updateTime >= m_config.controlTime)
{
std::size_t numControllers = subject.getNumberofMuslces();
double descendingCommand = 2.0;
std::vector<double> desComs (numControllers, descendingCommand);
m_pCPGSys->update(desComs, m_updateTime);
#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];
/// @todo add to config
if (currentHeight > 25 || currentHeight < 1.0)
{
/// @todo if bogus, stop trial (reset simulation)
bogus = true;
}
}
void JSONQuadFeedbackControl::onSetup(BaseSpineModelLearning& subject)
{
m_pCPGSys = new CPGEquationsFB(100);
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;
bogus = false;
}
void BaseSpineCPGControl::onSetup(BaseSpineModelLearning& subject)
{
// Maximum number of sub-steps allowed by CPG
m_pCPGSys = new CPGEquations(200);
//Initialize the Learning Adapters
nodeAdapter.initialize(&nodeEvolution,
nodeLearning,
nodeConfigData);
edgeAdapter.initialize(&edgeEvolution,
edgeLearning,
edgeConfigData);
/* Empty vector signifying no state information
* All parameters are stateless parameters, so we can get away with
* only doing this once
*/
std::vector<double> state;
double dt = 0;
array_4D edgeParams = scaleEdgeActions(edgeAdapter.step(dt, state));
array_2D nodeParams = scaleNodeActions(nodeAdapter.step(dt, state));
setupCPGs(subject, nodeParams, edgeParams);
initConditions = subject.getSegmentCOM(m_config.segmentNumber);
#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;
bogus = false;
}
void JSONGoalTensionNNW::onStep(BaseSpineModelLearning& subject, double dt)
{
m_updateTime += dt;
m_totalTime += dt;
double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];
if (m_updateTime >= m_config.controlTime)
{
#if (1) // Goal and cable
std::vector<double> desComs = getFeedback(subject);
const BaseSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, BaseSpineModelGoal>(subject);
getGoalFeedback(goalSubject);
#else // Just goal
std::size_t numControllers = subject.getNumberofMuslces() * 3;
double descendingCommand = 0.0;
std::vector<double> desComs (numControllers, descendingCommand);
const BaseSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, BaseSpineModelGoal>(subject);
getGoalFeedback(goalSubject);
#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;
//std::cout << m_totalTime << " " << currentHeight<< std::endl;
}
#if (0)
/// @todo add to config
if (currentHeight > 25 || currentHeight < 1.0)
{
/// @todo if bogus, stop trial (reset simulation)
bogus = true;
throw std::runtime_error("Height out of range");
}
#endif
}
void LearningSpineSine::onSetup(BaseSpineModelLearning& subject)
{
//Initialize the Learning Adapters
nodeAdapter.initialize(&nodeEvolution,
nodeLearning,
nodeConfigData);
edgeAdapter.initialize(&edgeEvolution,
edgeLearning,
edgeConfigData);
/* Empty vector signifying no state information
* All parameters are stateless parameters, so we can get away with
* only doing this once
*/
std::vector<double> state;
double dt = 0;
array_2D edgeParams = scalePhaseActions(edgeAdapter.step(dt, state));
array_2D nodeParams = scaleNodeActions(nodeAdapter.step(dt, state));
setupWaves(subject, nodeParams, edgeParams);
initConditions = subject.getSegmentCOM(m_config.segmentNumber);
#if (0) // Conditional compile for data logging
m_dataObserver.onSetup(subject);
#endif
m_updateTime = 0.0;
}
void LearningSpineSine::onTeardown(BaseSpineModelLearning& subject)
{
std::vector<double> scores;
// @todo - check to make sure we ran for the right amount of time
std::vector<double> 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));
scores.push_back(distanceMoved);
/// @todo - consolidate with other controller classes.
/// @todo - return length scale as a parameter
double totalEnergySpent=0;
vector<tgSpringCableActuator* > tmpSCAs = subject.getAllMuscles();
vector<tgBasicActuator* > tmpStrings = tgCast::filter<tgSpringCableActuator, tgBasicActuator>(tmpSCAs);
for(int i=0; i<tmpStrings.size(); i++)
{
tgSpringCableActuator::SpringCableActuatorHistory stringHist = tmpStrings[i]->getHistory();
for(int j=1; j<stringHist.tensionHistory.size(); j++)
{
const double previousTension = stringHist.tensionHistory[j-1];
const double previousLength = stringHist.restLengths[j-1];
const double currentLength = stringHist.restLengths[j];
//TODO: examine this assumption - free spinning motor may require more power
double motorSpeed = (currentLength-previousLength);
if(motorSpeed > 0) // Vestigial code
motorSpeed = 0;
const double workDone = previousTension * motorSpeed;
totalEnergySpent += workDone;
}
}
scores.push_back(totalEnergySpent);
edgeAdapter.endEpisode(scores);
nodeAdapter.endEpisode(scores);
for(size_t i = 0; i < m_sineControllers.size(); i++)
{
delete m_sineControllers[i];
}
m_sineControllers.clear();
m_allControllers.clear();
}
void JSONQuadFeedbackControl::onStep(BaseSpineModelLearning& subject, double dt)
{
m_updateTime += dt;
if (m_updateTime >= m_config.controlTime)
{
#if (1)
std::vector<double> desComs = getFeedback(subject);
#else
std::size_t numControllers = subject.getNumberofMuslces() * 3;
double descendingCommand = 0.0;
std::vector<double> 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");
}
}
void JSONCPGControl::onSetup(BaseSpineModelLearning& subject)
{
// Maximum number of sub-steps allowed by CPG
m_pCPGSys = new CPGEquations(200);
//Initialize the Learning Adapters
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");
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);
initConditions = subject.getSegmentCOM(m_config.segmentNumber);
#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;
bogus = false;
}
void JSONCPGControl::onTeardown(BaseSpineModelLearning& subject)
{
scores.clear();
// @todo - check to make sure we ran for the right amount of time
std::vector<double> 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<tgSpringCableActuator* > tmpStrings = subject.getAllMuscles();
for(std::size_t i=0; i<tmpStrings.size(); i++)
{
tgSpringCableActuator::SpringCableActuatorHistory stringHist = tmpStrings[i]->getHistory();
for(std::size_t j=1; j<stringHist.tensionHistory.size(); j++)
{
const double previousTension = stringHist.tensionHistory[j-1];
const double previousLength = stringHist.restLengths[j-1];
const double currentLength = stringHist.restLengths[j];
//TODO: examine this assumption - free spinning motor may require more power
double motorSpeed = (currentLength-previousLength);
if(motorSpeed > 0) // Vestigial code
motorSpeed = 0;
const double workDone = previousTension * motorSpeed;
totalEnergySpent += workDone;
}
}
scores.push_back(totalEnergySpent);
std::cout << "Dist travelled " << scores[0] << std::endl;
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");
}
Json::Value prevScores = root.get("scores", Json::nullValue);
Json::Value subScores;
subScores["distance"] = scores[0];
subScores["energy"] = totalEnergySpent;
prevScores.append(subScores);
root["scores"] = prevScores;
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_allControllers.size(); i++)
{
delete m_allControllers[i];
}
m_allControllers.clear();
}
