38 #include "examples/learningSpines/tgCPGCableControl.h"
40 #include "dev/dhustigschultz/BigPuppy_SpineOnly_Stats/BaseQuadModelLearning.h"
47 #include "examples/learningSpines/tgCPGCableControl.h"
49 #include "neuralNet/Neural Network v2/neuralNetwork.h"
51 #include <json/json.h>
92 lp, hp, kt, kp, kv, def, cl, lf, hf),
93 freqFeedbackMin(ffMin),
94 freqFeedbackMax(ffMax),
95 ampFeedbackMin(afMin),
96 ampFeedbackMax(afMax),
97 phaseFeedbackMin(pfMin),
98 phaseFeedbackMax(pfMax),
102 theirHipMuscles(thm),
115 std::string resourcePath) :
123 JSONSegmentsFeedbackControl::~JSONSegmentsFeedbackControl()
135 bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
136 if ( !parsingSuccessful )
139 std::cout <<
"Failed to parse configuration\n"
140 << reader.getFormattedErrorMessages();
141 throw std::invalid_argument(
"Bad filename for JSON");
145 Json::Value nodeVals = root.get(
"nodeVals",
"UTF-8");
146 Json::Value edgeVals = root.get(
"edgeVals",
"UTF-8");
148 Json::Value hipEdgeVals = root.get(
"hipEdgeVals",
"UTF-8");
149 Json::Value legEdgeVals = root.get(
"legEdgeVals",
"UTF-8");
151 std::cout << nodeVals << std::endl;
153 nodeVals = nodeVals.get(
"params",
"UTF-8");
154 edgeVals = edgeVals.get(
"params",
"UTF-8");
156 hipEdgeVals = hipEdgeVals.get(
"params",
"UTF-8");
157 legEdgeVals = legEdgeVals.get(
"params",
"UTF-8");
159 array_4D edgeParams =
scaleEdgeActions(edgeVals,m_config.theirMuscles,m_config.ourMuscles);
161 array_4D hipEdgeParams =
scaleEdgeActions(hipEdgeVals,m_config.theirHipMuscles,m_config.ourHipMuscles);
162 array_4D legEdgeParams =
scaleEdgeActions(legEdgeVals,m_config.theirLegMuscles,m_config.ourLegMuscles);
164 array_2D nodeParams = scaleNodeActions(nodeVals);
166 setupCPGs(subject, nodeParams, edgeParams, hipEdgeParams, legEdgeParams);
168 Json::Value feedbackParams = root.get(
"feedbackVals",
"UTF-8");
169 feedbackParams = feedbackParams.get(
"params",
"UTF-8");
172 m_config.numStates = feedbackParams.get(
"numStates",
"UTF-8").asInt();
173 m_config.numActions = feedbackParams.get(
"numActions",
"UTF-8").asInt();
176 std::string nnFile = controlFilePath + feedbackParams.get(
"neuralFilename",
"UTF-8").asString();
178 nn =
new neuralNetwork(m_config.numStates, m_config.numStates*2, m_config.numActions);
180 nn->loadWeights(nnFile.c_str());
182 initConditions = subject.getSegmentCOM(m_config.segmentNumber);
183 for (
int i = 0; i < initConditions.size(); i++)
185 std::cout << initConditions[i] <<
" ";
187 std::cout << std::endl;
188 #ifdef LOGGING // Conditional compile for data logging
189 m_dataObserver.
onSetup(subject);
192 #if (0) // Conditional Compile for debug info
193 std::cout << *m_pCPGSys << std::endl;
202 std::vector<double> structureCOM = subject.getCOM(m_config.segmentNumber);
204 for(std::size_t i=0; i<3; i++)
206 metrics.push_back(structureCOM[i]);
211 Json::Value prevMetrics = root.get(
"metrics", Json::nullValue);
213 Json::Value subMetrics;
214 subMetrics[
"initial COM x"] = metrics[0];
215 subMetrics[
"initial COM y"] = metrics[1];
216 subMetrics[
"initial COM z"] = metrics[2];
218 prevMetrics.append(subMetrics);
219 root[
"metrics"] = prevMetrics;
222 payloadLog.open(controlFilename.c_str(),ofstream::out);
224 payloadLog << root << std::endl;
231 if (m_updateTime >= m_config.controlTime)
234 std::vector<double> desComs = getFeedback(subject);
237 std::size_t numControllers = subject.getNumberofMuslces() * 3;
239 double descendingCommand = 0.0;
240 std::vector<double> desComs (numControllers, descendingCommand);
244 m_pCPGSys->
update(desComs, m_updateTime);
246 catch (std::runtime_error& e)
253 #ifdef LOGGING // Conditional compile for data logging
254 m_dataObserver.
onStep(subject, m_updateTime);
260 double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];
263 if (currentHeight > m_config.maxHeight || currentHeight < m_config.minHeight)
267 throw std::runtime_error(
"Height out of range");
271 static int count = 0;
273 std::cout << m_totalTime << std::endl;
276 std::vector<double> structureCOM = subject.getCOM(m_config.segmentNumber);
277 std::cout <<
"COM: " << structureCOM[0] <<
" " << structureCOM[1] <<
" " << structureCOM[2] <<
" ";
278 std::cout << std::endl;
285 for(std::size_t i=0; i<tmpStrings.size(); i++)
287 std::cout <<
"Spine Muscle Tension " << i <<
": " << tmpStrings[i]->getTension() << std::endl;
289 std::cout << std::endl;
291 for(std::size_t i=0; i<tmpStrings.size(); i++)
293 std::cout <<
"Spine Muscle Length " << i <<
": " << tmpStrings[i]->getCurrentLength() << std::endl;
295 std::cout << std::endl;
300 for(std::size_t i=0; i<tmpHipStrings.size(); i++)
302 std::cout <<
"Hip Muscle Tension " << i <<
": " << tmpHipStrings[i]->getTension() << std::endl;
304 std::cout << std::endl;
306 for(std::size_t i=0; i<tmpHipStrings.size(); i++)
308 std::cout <<
"Hip Muscle Length " << i <<
": " << tmpHipStrings[i]->getCurrentLength() << std::endl;
310 std::cout << std::endl;
315 for(std::size_t i=0; i<tmpLegStrings.size(); i++)
317 std::cout <<
"Leg Muscle Tension " << i <<
": " << tmpLegStrings[i]->getTension() << std::endl;
319 std::cout << std::endl;
321 for(std::size_t i=0; i<tmpLegStrings.size(); i++)
323 std::cout <<
"Leg Muscle Length " << i <<
": " << tmpLegStrings[i]->getCurrentLength() << std::endl;
325 std::cout << std::endl;
341 std::vector<double> finalConditions = subject.getSegmentCOM(m_config.segmentNumber);
343 const double newX = finalConditions[0];
344 const double newZ = finalConditions[2];
345 const double oldX = initConditions[0];
346 const double oldZ = initConditions[2];
348 const double distanceMoved = sqrt((newX-oldX) * (newX-oldX) +
349 (newZ-oldZ) * (newZ-oldZ));
353 scores.push_back(-1.0);
357 scores.push_back(distanceMoved);
362 double totalEnergySpent=0;
367 for(std::size_t i=0; i<tmpStrings.size(); i++)
374 const double previousLength = stringHist.
restLengths[j-1];
375 const double currentLength = stringHist.
restLengths[j];
377 double motorSpeed = (currentLength-previousLength);
380 const double workDone = previousTension * motorSpeed;
381 totalEnergySpent += workDone;
388 for(std::size_t i=0; i<tmpHipStrings.size(); i++)
395 const double previousLength = stringHipHist.
restLengths[j-1];
396 const double currentLength = stringHipHist.
restLengths[j];
398 double motorSpeed = (currentLength-previousLength);
401 const double workDone = previousTension * motorSpeed;
402 totalEnergySpent += workDone;
409 for(std::size_t i=0; i<tmpLegStrings.size(); i++)
416 const double previousLength = stringLegHist.
restLengths[j-1];
417 const double currentLength = stringLegHist.
restLengths[j];
419 double motorSpeed = (currentLength-previousLength);
422 const double workDone = previousTension * motorSpeed;
423 totalEnergySpent += workDone;
427 scores.push_back(totalEnergySpent);
430 std::vector<double> structureCOM = subject.getCOM(m_config.segmentNumber);
432 for(std::size_t i=0; i<3; i++)
434 metrics.push_back(structureCOM[i]);
437 std::cout <<
"Dist travelled " << scores[0] << std::endl;
442 bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
443 if ( !parsingSuccessful )
446 std::cout <<
"Failed to parse configuration\n"
447 << reader.getFormattedErrorMessages();
448 throw std::invalid_argument(
"Bad filename for JSON");
451 Json::Value prevScores = root.get(
"scores", Json::nullValue);
452 Json::Value prevMetrics = root.get(
"metrics", Json::nullValue);
454 Json::Value subScores;
455 subScores[
"distance"] = scores[0];
456 subScores[
"energy"] = scores[1];
458 Json::Value subMetrics;
459 subMetrics[
"final COM x"] = metrics[0];
460 subMetrics[
"final COM y"] = metrics[1];
461 subMetrics[
"final COM z"] = metrics[2];
463 prevScores.append(subScores);
464 prevMetrics.append(subMetrics);
466 root[
"scores"] = prevScores;
467 root[
"metrics"] = prevMetrics;
470 payloadLog.open(controlFilename.c_str(),ofstream::out);
472 payloadLog << root << std::endl;
477 for(
size_t i = 0; i < m_spineControllers.size(); i++)
479 delete m_spineControllers[i];
481 m_spineControllers.clear();
484 void JSONSegmentsFeedbackControl::setupCPGs(
BaseQuadModelLearning& subject, array_2D nodeActions, array_4D edgeActions, array_4D hipEdgeActions, array_4D legEdgeActions)
491 CPGEquationsFB& m_CPGFBSys = *(tgCast::cast<CPGEquations, CPGEquationsFB>(m_pCPGSys));
493 for (std::size_t i = 0; i < spineMuscles.size(); i++)
499 spineMuscles[i]->attach(pStringControl);
504 m_spineControllers.push_back(pStringControl);
508 for (std::size_t i = 0; i < m_spineControllers.size(); i++)
511 assert(pStringInfo != NULL);
518 if (m_config.useDefault)
520 pStringInfo->setupControl(*p_ipc);
524 pStringInfo->setupControl(*p_ipc, m_config.controlLength);
528 for (std::size_t i = 0; i < hipMuscles.size(); i++)
534 hipMuscles[i]->attach(pStringControl);
539 m_hipControllers.push_back(pStringControl);
543 for (std::size_t i = 0; i < m_hipControllers.size(); i++)
546 assert(pStringInfo != NULL);
553 if (m_config.useDefault)
555 pStringInfo->setupControl(*p_ipc);
559 pStringInfo->setupControl(*p_ipc, m_config.controlLength);
601 (Json::Value edgeParam,
int theirMuscles,
int ourMuscles)
603 assert(edgeParam[0].size() == 2);
605 double lowerLimit = m_config.lowPhase;
606 double upperLimit = m_config.highPhase;
607 double range = upperLimit - lowerLimit;
609 array_4D actionList(boost::extents[m_config.segmentSpan][theirMuscles][ourMuscles][m_config.params]);
622 Json::Value::iterator edgeIt = edgeParam.end();
626 while (i < m_config.segmentSpan)
628 while(j < theirMuscles)
630 while(k < ourMuscles)
632 if (edgeIt == edgeParam.begin())
634 std::cout <<
"ran out before table populated!"
641 if (i == 1 && j == k)
649 Json::Value edgeParam = *edgeIt;
650 assert(edgeParam.size() == 2);
652 actionList[i][j][k][0] = edgeParam[0].asDouble();
655 actionList[i][j][k][1] = edgeParam[1].asDouble() *
656 (range) + lowerLimit;
672 std::cout<<
"Params used: " << count << std::endl;
674 assert(edgeParam.begin() == edgeIt);
679 array_2D JSONSegmentsFeedbackControl::scaleNodeActions (Json::Value actions)
681 std::size_t numControllers = actions.size();
682 std::size_t numActions = actions[0].size();
684 array_2D nodeActions(boost::extents[numControllers][numActions]);
686 array_2D limits(boost::extents[2][numActions]);
689 assert(numActions == 5);
691 limits[0][0] = m_config.lowFreq;
692 limits[1][0] = m_config.highFreq;
693 limits[0][1] = m_config.lowAmp;
694 limits[1][1] = m_config.highAmp;
695 limits[0][2] = m_config.freqFeedbackMin;
696 limits[1][2] = m_config.freqFeedbackMax;
697 limits[0][3] = m_config.ampFeedbackMin;
698 limits[1][3] = m_config.ampFeedbackMax;
699 limits[0][4] = m_config.phaseFeedbackMin;
700 limits[1][4] = m_config.phaseFeedbackMax;
702 Json::Value::iterator nodeIt = actions.begin();
705 for( std::size_t i = 0; i < numControllers; i++)
707 Json::Value nodeParam = *nodeIt;
708 for( std::size_t j = 0; j < numActions; j++)
710 nodeActions[i][j] = ( (nodeParam.get(j, 0.0)).asDouble() *
711 (limits[1][j] - limits[0][j])) + limits[0][j];
722 std::vector<double> feedback;
726 double *inputs =
new double[m_config.numStates];
728 std::size_t n = spineCables.size();
729 for(std::size_t i = 0; i != n; i++)
731 std::vector< std::vector<double> > actions;
734 std::vector<double > state = getCableState(cable);
737 for (std::size_t i = 0; i < state.size(); i++)
739 inputs[i]=state[i] / 2.0 + 0.5;
742 double *output =
nn->feedForwardPattern(inputs);
743 vector<double> tmpAct;
744 for(
int j=0;j<m_config.numActions;j++)
746 tmpAct.push_back(output[j]);
748 actions.push_back(tmpAct);
750 std::vector<double> cableFeedback = transformFeedbackActions(actions);
752 feedback.insert(feedback.end(), cableFeedback.begin(), cableFeedback.end());
758 std::size_t n2 = hipCables.size();
759 for(std::size_t i = 0; i != n2; i++)
761 std::vector< std::vector<double> > actions;
764 std::vector<double > state = getCableState(cable);
767 for (std::size_t i = 0; i < state.size(); i++)
769 inputs[i]=state[i] / 2.0 + 0.5;
772 double *output =
nn->feedForwardPattern(inputs);
773 vector<double> tmpAct;
774 for(
int j=0;j<m_config.numActions;j++)
776 tmpAct.push_back(output[j]);
778 actions.push_back(tmpAct);
780 std::vector<double> cableFeedback = transformFeedbackActions(actions);
782 feedback.insert(feedback.end(), cableFeedback.begin(), cableFeedback.end());
822 std::vector<double> state;
828 const double maxTension = cable.
getConfig().maxTens;
829 state.push_back((cable.
getTension() - maxTension / 2.0) / maxTension);
834 std::vector<double> JSONSegmentsFeedbackControl::transformFeedbackActions(std::vector< std::vector<double> >& actions)
837 std::vector<double> feedback;
840 const std::size_t numControllers = 1;
841 const std::size_t numActions = m_config.numActions;
843 assert( actions.size() == numControllers);
844 assert( actions[0].size() == numActions);
847 for( std::size_t i = 0; i < numControllers; i++)
849 for( std::size_t j = 0; j < numActions; j++)
851 feedback.push_back(actions[i][j] * 2.0 - 1.0);
Contains the definition of class ImpedanceControl. $Id$.
void update(std::vector< double > &descCom, double dt)
virtual void onSetup(BaseQuadModelLearning &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
JSONSegmentsFeedbackControl(JSONSegmentsFeedbackControl::Config config, std::string args, std::string resourcePath="")
Definition of the tgCPGStringControl observer class.
virtual array_4D scaleEdgeActions(Json::Value actions, int theirMuscles, int ourMuscles)
A class to read a learning configuration from a .ini file.
A controller for the template class BaseSpineModelLearning more metrics, such as center of mass of e...
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
A series of functions to assist with file input/output.
virtual void onStep(BaseQuadModelLearning &subject, double dt)
Contains the definition of class AnnealEvolution. Adapting NeuroEvolution to do Simulated Annealing...
virtual void onTeardown(BaseQuadModelLearning &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, int ohm=10, int thm=10, int olm=10, int tlm=10)
virtual void onSetup(tgModel &model)
virtual const double getCurrentLength() const
void notifyStep(double dt)
void assignNodeNumberFB(CPGEquationsFB &CPGSys, array_2D nodeParams)