27 #include "JSONQuadFeedbackControl.h"
37 #include "examples/learningSpines/tgCPGCableControl.h"
46 #include "examples/learningSpines/tgCPGCableControl.h"
48 #include "neuralNet/Neural Network v2/neuralNetwork.h"
50 #include <json/json.h>
86 JSONCPGControl::Config::Config(ss, tm, om, param, segnum, ct, la, ha,
87 lp, hp, kt, kp, kv, def, cl, lf, hf),
88 freqFeedbackMin(ffMin),
89 freqFeedbackMax(ffMax),
90 ampFeedbackMin(afMin),
91 ampFeedbackMax(afMax),
92 phaseFeedbackMin(pfMin),
93 phaseFeedbackMax(pfMax),
106 std::string resourcePath) :
114 JSONQuadFeedbackControl::~JSONQuadFeedbackControl()
126 bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
127 if ( !parsingSuccessful )
130 std::cout <<
"Failed to parse configuration\n"
131 << reader.getFormattedErrorMessages();
132 throw std::invalid_argument(
"Bad filename for JSON");
136 Json::Value nodeVals = root.get(
"nodeVals",
"UTF-8");
137 Json::Value edgeVals = root.get(
"edgeVals",
"UTF-8");
139 std::cout << nodeVals << std::endl;
141 nodeVals = nodeVals.get(
"params",
"UTF-8");
142 edgeVals = edgeVals.get(
"params",
"UTF-8");
145 array_2D nodeParams = scaleNodeActions(nodeVals);
147 setupCPGs(subject, nodeParams, edgeParams);
149 Json::Value feedbackParams = root.get(
"feedbackVals",
"UTF-8");
150 feedbackParams = feedbackParams.get(
"params",
"UTF-8");
153 m_config.numStates = feedbackParams.get(
"numStates",
"UTF-8").asInt();
154 m_config.numActions = feedbackParams.get(
"numActions",
"UTF-8").asInt();
157 std::string nnFile = controlFilePath + feedbackParams.get(
"neuralFilename",
"UTF-8").asString();
159 nn =
new neuralNetwork(m_config.numStates, m_config.numStates*2, m_config.numActions);
161 nn->loadWeights(nnFile.c_str());
163 initConditions = subject.getSegmentCOM(m_config.segmentNumber);
164 for (
int i = 0; i < initConditions.size(); i++)
166 std::cout << initConditions[i] <<
" ";
168 std::cout << std::endl;
169 #ifdef LOGGING // Conditional compile for data logging
170 m_dataObserver.
onSetup(subject);
173 #if (0) // Conditional Compile for debug info
174 std::cout << *m_pCPGSys << std::endl;
183 if (m_updateTime >= m_config.controlTime)
186 std::vector<double> desComs = getFeedback(subject);
189 std::size_t numControllers = subject.getNumberofMuslces() * 3;
191 double descendingCommand = 0.0;
192 std::vector<double> desComs (numControllers, descendingCommand);
196 m_pCPGSys->
update(desComs, m_updateTime);
198 catch (std::runtime_error& e)
205 #ifdef LOGGING // Conditional compile for data logging
206 m_dataObserver.
onStep(subject, m_updateTime);
212 double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];
215 if (currentHeight > m_config.maxHeight || currentHeight < m_config.minHeight)
219 throw std::runtime_error(
"Height out of range");
228 std::vector<double> finalConditions = subject.getSegmentCOM(m_config.segmentNumber);
230 const double newX = finalConditions[0];
231 const double newZ = finalConditions[2];
232 const double oldX = initConditions[0];
233 const double oldZ = initConditions[2];
235 const double distanceMoved = sqrt((newX-oldX) * (newX-oldX) +
236 (newZ-oldZ) * (newZ-oldZ));
240 scores.push_back(-1.0);
244 scores.push_back(distanceMoved);
249 double totalEnergySpent=0;
253 for(std::size_t i=0; i<tmpStrings.size(); i++)
260 const double previousLength = stringHist.
restLengths[j-1];
261 const double currentLength = stringHist.
restLengths[j];
263 double motorSpeed = (currentLength-previousLength);
266 const double workDone = previousTension * motorSpeed;
267 totalEnergySpent += workDone;
271 scores.push_back(totalEnergySpent);
273 std::cout <<
"Dist travelled " << scores[0] << std::endl;
278 bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
279 if ( !parsingSuccessful )
282 std::cout <<
"Failed to parse configuration\n"
283 << reader.getFormattedErrorMessages();
284 throw std::invalid_argument(
"Bad filename for JSON");
287 Json::Value prevScores = root.get(
"scores", Json::nullValue);
289 Json::Value subScores;
290 subScores[
"distance"] = scores[0];
291 subScores[
"energy"] = totalEnergySpent;
293 prevScores.append(subScores);
294 root[
"scores"] = prevScores;
297 payloadLog.open(controlFilename.c_str(),ofstream::out);
299 payloadLog << root << std::endl;
304 for(
size_t i = 0; i < m_spineControllers.size(); i++)
306 delete m_spineControllers[i];
308 m_spineControllers.clear();
311 void JSONQuadFeedbackControl::setupCPGs(
BaseSpineModelLearning& subject, array_2D nodeActions, array_4D edgeActions)
316 CPGEquationsFB& m_CPGFBSys = *(tgCast::cast<CPGEquations, CPGEquationsFB>(m_pCPGSys));
318 for (std::size_t i = 0; i < spineMuscles.size(); i++)
324 spineMuscles[i]->attach(pStringControl);
329 m_spineControllers.push_back(pStringControl);
333 for (std::size_t i = 0; i < m_spineControllers.size(); i++)
336 assert(pStringInfo != NULL);
343 if (m_config.useDefault)
345 pStringInfo->setupControl(*p_ipc);
349 pStringInfo->setupControl(*p_ipc, m_config.controlLength);
355 array_2D JSONQuadFeedbackControl::scaleNodeActions (Json::Value actions)
357 std::size_t numControllers = actions.size();
358 std::size_t numActions = actions[0].size();
360 array_2D nodeActions(boost::extents[numControllers][numActions]);
362 array_2D limits(boost::extents[2][numActions]);
365 assert(numActions == 5);
367 limits[0][0] = m_config.lowFreq;
368 limits[1][0] = m_config.highFreq;
369 limits[0][1] = m_config.lowAmp;
370 limits[1][1] = m_config.highAmp;
371 limits[0][2] = m_config.freqFeedbackMin;
372 limits[1][2] = m_config.freqFeedbackMax;
373 limits[0][3] = m_config.ampFeedbackMin;
374 limits[1][3] = m_config.ampFeedbackMax;
375 limits[0][4] = m_config.phaseFeedbackMin;
376 limits[1][4] = m_config.phaseFeedbackMax;
378 Json::Value::iterator nodeIt = actions.begin();
381 for( std::size_t i = 0; i < numControllers; i++)
383 Json::Value nodeParam = *nodeIt;
384 for( std::size_t j = 0; j < numActions; j++)
386 nodeActions[i][j] = ( (nodeParam.get(j, 0.0)).asDouble() *
387 (limits[1][j] - limits[0][j])) + limits[0][j];
398 std::vector<double> feedback;
402 double *inputs =
new double[m_config.numStates];
404 std::size_t n = spineCables.size();
405 for(std::size_t i = 0; i != n; i++)
407 std::vector< std::vector<double> > actions;
410 std::vector<double > state = getCableState(cable);
413 for (std::size_t i = 0; i < state.size(); i++)
415 inputs[i]=state[i] / 2.0 + 0.5;
418 double *output =
nn->feedForwardPattern(inputs);
419 vector<double> tmpAct;
420 for(
int j=0;j<m_config.numActions;j++)
422 tmpAct.push_back(output[j]);
424 actions.push_back(tmpAct);
426 std::vector<double> cableFeedback = transformFeedbackActions(actions);
428 feedback.insert(feedback.end(), cableFeedback.begin(), cableFeedback.end());
439 std::vector<double> state;
445 const double maxTension = cable.
getConfig().maxTens;
446 state.push_back((cable.
getTension() - maxTension / 2.0) / maxTension);
451 std::vector<double> JSONQuadFeedbackControl::transformFeedbackActions(std::vector< std::vector<double> >& actions)
454 std::vector<double> feedback;
457 const std::size_t numControllers = 1;
458 const std::size_t numActions = m_config.numActions;
460 assert( actions.size() == numControllers);
461 assert( actions[0].size() == numActions);
464 for( std::size_t i = 0; i < numControllers; i++)
466 for( std::size_t j = 0; j < numActions; j++)
468 feedback.push_back(actions[i][j] * 2.0 - 1.0);
Contains the definition of class ImpedanceControl. $Id$.
virtual array_4D scaleEdgeActions(Json::Value edgeParam)
void update(std::vector< double > &descCom, double dt)
virtual void onTeardown(BaseSpineModelLearning &subject)
std::deque< double > tensionHistory
virtual const double getTension() const
virtual const double getStartLength() const
void setConnectivity(const std::vector< tgCPGActuatorControl * > &allStrings, array_4D edgeParams)
std::deque< double > restLengths
Definition of the tgCPGStringControl observer class.
A template base class for a tensegrity spine.
A class to read a learning configuration from a .ini file.
virtual void onStep(BaseSpineModelLearning &subject, double dt)
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
A series of functions to assist with file input/output.
JSONQuadFeedbackControl(JSONQuadFeedbackControl::Config config, std::string args, std::string resourcePath="")
Contains the definition of class AnnealEvolution. Adapting NeuroEvolution to do Simulated Annealing...
virtual void onSetup(BaseSpineModelLearning &subject)
Contains the definition of class tgBasicActuator.
const Config & getConfig() const
std::vector< T * > find(const tgTagSearch &tagSearch)
virtual void onStep(tgModel &model, double dt)
Definition of class CPGEquationsFB.
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, double minH=1.0, double hffMin=0.0, double hffMax=0.0)
virtual void onSetup(tgModel &model)
virtual const double getCurrentLength() const
void notifyStep(double dt)
void assignNodeNumberFB(CPGEquationsFB &CPGSys, array_2D nodeParams)