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>
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;
180 Json::Value PVal = root.get(
"PropVals",
"UTF-8");
181 Json::Value DVal = root.get(
"DerVals",
"UTF-8");
183 cout << PVal << endl;
186 PVal = PVal.get(
"params",
"UTF-8");
187 DVal = DVal.get(
"params",
"UTF-8");
189 if (PVal[0].isArray())
193 if (DVal[0].isArray())
199 P = (PVal.get(j, 0.0)).asDouble();
200 D = (DVal.get(j, 0.0)).asDouble();
207 if (m_updateTime >= m_config.controlTime)
210 std::vector<double> desComs = getFeedback(subject);
213 std::size_t numControllers = subject.getNumberofMuslces() * 3;
215 double descendingCommand = 0.0;
216 std::vector<double> desComs (numControllers, descendingCommand);
220 m_pCPGSys->
update(desComs, m_updateTime);
222 catch (std::runtime_error& e)
229 #ifdef LOGGING // Conditional compile for data logging
230 m_dataObserver.
onStep(subject, m_updateTime);
236 double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];
237 double currentHeightTail = subject.getSegmentCOM(6)[1];
239 if (currentHeight > m_config.maxHeight || currentHeight < m_config.minHeight
240 || currentHeightTail > m_config.maxHeight || currentHeightTail < m_config.minHeight)
244 throw std::runtime_error(
"Height out of range");
253 std::vector<double> finalConditions = subject.getSegmentCOM(m_config.segmentNumber);
255 const double newX = finalConditions[0];
256 const double newZ = finalConditions[2];
257 const double oldX = initConditions[0];
258 const double oldZ = initConditions[2];
260 const double distanceMoved = sqrt((newX-oldX) * (newX-oldX) +
261 (newZ-oldZ) * (newZ-oldZ));
265 scores.push_back(-1.0);
269 scores.push_back(distanceMoved);
274 double totalEnergySpent=0;
278 for(std::size_t i=0; i<tmpStrings.size(); i++)
285 const double previousLength = stringHist.
restLengths[j-1];
286 const double currentLength = stringHist.
restLengths[j];
288 double motorSpeed = (currentLength-previousLength);
291 const double workDone = previousTension * motorSpeed;
292 totalEnergySpent += workDone;
296 scores.push_back(totalEnergySpent);
298 std::cout <<
"Dist travelled " << scores[0] << std::endl;
303 bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
304 if ( !parsingSuccessful )
307 std::cout <<
"Failed to parse configuration\n"
308 << reader.getFormattedErrorMessages();
309 throw std::invalid_argument(
"Bad filename for JSON");
312 Json::Value prevScores = root.get(
"scores", Json::nullValue);
314 Json::Value subScores;
315 subScores[
"distance"] = scores[0];
316 subScores[
"energy"] = totalEnergySpent;
318 prevScores.append(subScores);
319 root[
"scores"] = prevScores;
322 payloadLog.open(controlFilename.c_str(),ofstream::out);
324 payloadLog << root << std::endl;
329 for(
size_t i = 0; i < m_spineControllers.size(); i++)
331 delete m_spineControllers[i];
333 m_spineControllers.clear();
336 void JSONQuadFeedbackControl::setupCPGs(
BaseSpineModelLearning& subject, array_2D nodeActions, array_4D edgeActions)
340 CPGEquationsFB& m_CPGFBSys = *(tgCast::cast<CPGEquations, CPGEquationsFB>(m_pCPGSys));
342 for (std::size_t i = 0; i < spineMuscles.size(); i++)
349 spineMuscles[i]->attach(pStringControl);
354 m_spineControllers.push_back(pStringControl);
358 for (std::size_t i = 0; i < m_spineControllers.size(); i++)
361 assert(pStringInfo != NULL);
368 if (m_config.useDefault)
370 pStringInfo->setupControl(*p_ipc);
374 pStringInfo->setupControl(*p_ipc, m_config.controlLength);
380 array_2D JSONQuadFeedbackControl::scaleNodeActions (Json::Value actions)
382 std::size_t numControllers = actions.size();
383 std::size_t numActions = actions[0].size();
385 array_2D nodeActions(boost::extents[numControllers][numActions]);
387 array_2D limits(boost::extents[2][numActions]);
390 assert(numActions == 5);
392 limits[0][0] = m_config.lowFreq;
393 limits[1][0] = m_config.highFreq;
394 limits[0][1] = m_config.lowAmp;
395 limits[1][1] = m_config.highAmp;
396 limits[0][2] = m_config.freqFeedbackMin;
397 limits[1][2] = m_config.freqFeedbackMax;
398 limits[0][3] = m_config.ampFeedbackMin;
399 limits[1][3] = m_config.ampFeedbackMax;
400 limits[0][4] = m_config.phaseFeedbackMin;
401 limits[1][4] = m_config.phaseFeedbackMax;
403 Json::Value::iterator nodeIt = actions.begin();
406 for( std::size_t i = 0; i < numControllers; i++)
408 Json::Value nodeParam = *nodeIt;
409 for( std::size_t j = 0; j < numActions; j++)
411 nodeActions[i][j] = ( (nodeParam.get(j, 0.0)).asDouble() *
412 (limits[1][j] - limits[0][j])) + limits[0][j];
423 std::vector<double> feedback;
427 double *inputs =
new double[m_config.numStates];
429 std::size_t n = spineCables.size();
430 for(std::size_t i = 0; i != n; i++)
432 std::vector< std::vector<double> > actions;
435 std::vector<double > state = getCableState(cable);
438 for (std::size_t i = 0; i < state.size(); i++)
440 inputs[i]=state[i] / 2.0 + 0.5;
443 double *output =
nn->feedForwardPattern(inputs);
444 vector<double> tmpAct;
445 for(
int j=0;j<m_config.numActions;j++)
447 tmpAct.push_back(output[j]);
449 actions.push_back(tmpAct);
451 std::vector<double> cableFeedback = transformFeedbackActions(actions);
453 feedback.insert(feedback.end(), cableFeedback.begin(), cableFeedback.end());
464 std::vector<double> state;
470 const double maxTension = cable.
getConfig().maxTens;
471 state.push_back((cable.
getTension() - maxTension / 2.0) / maxTension);
476 std::vector<double> JSONQuadFeedbackControl::transformFeedbackActions(std::vector< std::vector<double> >& actions)
479 std::vector<double> feedback;
482 const std::size_t numControllers = 1;
483 const std::size_t numActions = m_config.numActions;
485 assert( actions.size() == numControllers);
486 assert( actions[0].size() == numActions);
489 for( std::size_t i = 0; i < numControllers; i++)
491 for( std::size_t j = 0; j < numActions; j++)
493 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)