37 #include "examples/learningSpines/tgCPGCableControl.h"
38 #include "examples/learningSpines/tgSCASineControl.h"
45 #include "examples/learningSpines/tgCPGCableControl.h"
47 #include "neuralNet/Neural Network v2/neuralNetwork.h"
49 #include <json/json.h>
85 lp, hp, kt, kp, kv, def, cl, lf, hf),
86 freqFeedbackMin(ffMin),
87 freqFeedbackMax(ffMax),
88 ampFeedbackMin(afMin),
89 ampFeedbackMax(afMax),
90 phaseFeedbackMin(pfMin),
91 phaseFeedbackMax(pfMax),
103 std::string resourcePath) :
113 JSONGoalControl::~JSONGoalControl()
124 bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
125 if ( !parsingSuccessful )
128 std::cout <<
"Failed to parse configuration\n"
129 << reader.getFormattedErrorMessages();
130 throw std::invalid_argument(
"Bad filename for JSON");
134 Json::Value nodeVals = root.get(
"nodeVals",
"UTF-8");
135 Json::Value edgeVals = root.get(
"edgeVals",
"UTF-8");
137 nodeVals = nodeVals.get(
"params",
"UTF-8");
138 edgeVals = edgeVals.get(
"params",
"UTF-8");
141 array_2D nodeParams = scaleNodeActions(nodeVals);
143 setupCPGs(subject, nodeParams, edgeParams);
146 Json::Value feedbackParams = root.get(
"feedbackVals",
"UTF-8");
147 feedbackParams = feedbackParams.get(
"params",
"UTF-8");
150 m_config.numStates = feedbackParams.get(
"numStates",
"UTF-8").asInt();
151 m_config.numActions = feedbackParams.get(
"numActions",
"UTF-8").asInt();
152 m_config.numHidden = feedbackParams.get(
"numHidden",
"UTF-8").asInt();
154 std::string nnFile = controlFilePath + feedbackParams.get(
"neuralFilename",
"UTF-8").asString();
156 nn =
new neuralNetwork(m_config.numStates, m_config.numHidden, m_config.numActions);
158 nn->loadWeights(nnFile.c_str());
160 Json::Value goalParams = root.get(
"goalVals",
"UTF-8");
161 goalParams = goalParams.get(
"params",
"UTF-8");
164 m_config.goalStates = goalParams.get(
"numStates",
"UTF-8").asInt();
165 m_config.goalActions = goalParams.get(
"numActions",
"UTF-8").asInt();
166 m_config.goalHidden = goalParams.get(
"numHidden",
"UTF-8").asInt();
168 std::string nnFile_goal = controlFilePath + goalParams.get(
"neuralFilename",
"UTF-8").asString();
170 nn_goal =
new neuralNetwork(m_config.goalStates, m_config.goalHidden, m_config.goalActions);
172 nn_goal->loadWeights(nnFile_goal.c_str());
174 initConditions = subject.getSegmentCOM(m_config.segmentNumber);
175 #ifdef LOGGING // Conditional compile for data logging
176 m_dataObserver.
onSetup(subject);
179 #if (0) // Conditional Compile for debug info
180 std::cout << *m_pCPGSys << std::endl;
189 if (m_updateTime >= m_config.controlTime)
191 #if (1) // Goal and cable
193 std::vector<double> desComs = getFeedback(subject);
195 const BaseSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, BaseSpineModelGoal>(subject);
198 std::size_t n = desComs.size();
199 assert(n == desComsSet2.size());
201 for (std::size_t i = 0; i < n; i++)
203 desComs[i] += desComsSet2[i];
206 #elif (1) // Just goal
207 const BaseSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, BaseSpineModelGoal>(subject);
210 std::size_t numControllers = subject.getNumberofMuslces() * 3;
212 double descendingCommand = 0.0;
213 std::vector<double> desComs (numControllers, descendingCommand);
217 m_pCPGSys->
update(desComs, m_updateTime);
219 catch (std::runtime_error& e)
226 #ifdef LOGGING // Conditional compile for data logging
227 m_dataObserver.
onStep(subject, m_updateTime);
233 double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];
236 if (currentHeight > 25 || currentHeight < 1.0)
240 throw std::runtime_error(
"Height out of range");
249 std::vector<double> finalConditions = subject.getSegmentCOM(m_config.segmentNumber);
251 const double newX = finalConditions[0];
252 const double newZ = finalConditions[2];
253 const double oldX = initConditions[0];
254 const double oldZ = initConditions[2];
256 const BaseSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, BaseSpineModelGoal>(subject);
258 const double distanceMoved = calculateDistanceMoved(goalSubject);
262 scores.push_back(-1.0);
266 scores.push_back(distanceMoved);
271 double totalEnergySpent=0;
273 std::vector<tgSpringCableActuator* > tmpStrings = subject.getAllMuscles();
275 for(std::size_t i=0; i<tmpStrings.size(); i++)
282 const double previousLength = stringHist.
restLengths[j-1];
283 const double currentLength = stringHist.
restLengths[j];
285 double motorSpeed = (currentLength-previousLength);
288 const double workDone = previousTension * motorSpeed;
289 totalEnergySpent += workDone;
293 scores.push_back(totalEnergySpent);
295 std::cout <<
"Dist travelled " << scores[0] << std::endl;
300 bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
301 if ( !parsingSuccessful )
304 std::cout <<
"Failed to parse configuration\n"
305 << reader.getFormattedErrorMessages();
306 throw std::invalid_argument(
"Bad filename for JSON");
309 Json::Value prevScores = root.get(
"scores", Json::nullValue);
311 Json::Value subScores;
312 subScores[
"distance"] = scores[0];
313 subScores[
"energy"] = totalEnergySpent;
315 prevScores.append(subScores);
316 root[
"scores"] = prevScores;
319 payloadLog.open(controlFilename.c_str(),ofstream::out);
321 payloadLog << root << std::endl;
326 for(
size_t i = 0; i < m_allControllers.size(); i++)
328 delete m_allControllers[i];
330 m_allControllers.clear();
336 void JSONGoalControl::setupCPGs(
BaseSpineModelLearning& subject, array_2D nodeActions, array_4D edgeActions)
339 std::vector <tgSpringCableActuator*> allMuscles = subject.getAllMuscles();
341 CPGEquationsFB& m_CPGFBSys = *(tgCast::cast<CPGEquations, CPGEquationsFB>(m_pCPGSys));
343 for (std::size_t i = 0; i < allMuscles.size(); i++)
349 allMuscles[i]->attach(pStringControl);
354 m_allControllers.push_back(pStringControl);
358 for (std::size_t i = 0; i < m_allControllers.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 JSONGoalControl::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];
422 const int nSeg = subject->getSegments() - 1;
428 const btVector3 currentPosVector = subject->getSegmentCOMVector(m_config.segmentNumber);
430 const btVector3 goalPosition = subject->goalBoxPosition();
432 btVector3 desiredHeading = (goalPosition - currentPosVector).normalize();
436 const btVector3 firstSegment = subject->getSegmentCOMVector(0);
437 const btVector3 secondSegment = subject->getSegmentCOMVector(1);
439 btVector3 currentHeading = (firstSegment - secondSegment).normalize();
441 std::vector<double> state;
442 state.push_back(desiredHeading.getX());
443 state.push_back(desiredHeading.getZ());
444 state.push_back(currentHeading.getX());
445 state.push_back(currentHeading.getZ());
447 double *inputs =
new double[m_config.goalStates];
448 for (std::size_t i = 0; i < state.size(); i++)
450 assert(state[i] >= -1.0 && state[i] <= 1.0);
455 double *output = nn_goal->feedForwardPattern(inputs);
457 vector<double> actions;
460 for(
int j=0;j<m_config.goalActions;j++)
462 std::cout << output[j] <<
" ";
464 std::cout << std::endl;
468 for (
int i = 0; i != nSeg; i++)
470 for(
int j=0;j<m_config.goalActions;j++)
472 actions.push_back(output[j]);
477 transformFeedbackActions(actions);
485 std::vector<double> feedback;
487 const std::vector<tgSpringCableActuator*>& allCables = subject.getAllMuscles();
489 double *inputs =
new double[m_config.numStates];
491 std::size_t n = allCables.size();
492 for(std::size_t i = 0; i != n; i++)
495 std::vector<double > state = getCableState(cable);
498 for (std::size_t i = 0; i < state.size(); i++)
503 double *output =
nn->feedForwardPattern(inputs);
504 vector<double> actions;
505 for(
int j=0;j<m_config.numActions;j++)
507 actions.push_back(output[j]);
510 transformFeedbackActions(actions);
512 feedback.insert(feedback.end(), actions.begin(), actions.end());
523 std::vector<double> state;
529 const double maxTension = cable.
getConfig().maxTens;
530 state.push_back((cable.
getTension() - maxTension / 2.0) / maxTension);
535 void JSONGoalControl::transformFeedbackActions(std::vector<double> & actions)
537 #if (0) // Only true if actions are applied to all segments
538 assert( actions.size() == numActions);
541 for( std::size_t i = 0; i < actions.size(); i++)
543 actions[i] = actions[i] * 2.0 - 1.0;
547 double JSONGoalControl::calculateDistanceMoved(
const BaseSpineModelGoal* subject)
const
549 std::vector<double> finalConditions = subject->getSegmentCOM(m_config.segmentNumber);
551 const btVector3 goalPos = subject->goalBoxPosition();
553 std::cout << goalPos << std::endl;
555 double x= finalConditions[0] - goalPos.getX();
556 double z= finalConditions[2] - goalPos.getZ();
557 double distanceNew=sqrt(x*x + z*z);
558 double xx=initConditions[0]-goalPos.getX();
559 double zz=initConditions[2]-goalPos.getZ();
560 double distanceOld=sqrt(xx*xx + zz*zz);
561 double distanceMoved=distanceOld-distanceNew;
566 return distanceMoved;
Contains the definition of class ImpedanceControl. $Id$.
void update(std::vector< double > &descCom, double dt)
std::deque< double > tensionHistory
A controller for the template class BaseSpineModelLearning.
virtual const double getTension() const
virtual void onSetup(BaseSpineModelLearning &subject)
virtual const double getStartLength() const
virtual array_4D scaleEdgeActions(Json::Value edgeParam)
void setConnectivity(const std::vector< tgCPGActuatorControl * > &allStrings, array_4D edgeParams)
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 tf=0.0)
std::deque< double > restLengths
JSONGoalControl(JSONGoalControl::Config config, std::string args, std::string resourcePath="")
Definition of the tgCPGStringControl observer class.
Implementing the tetrahedral complex spine inspired by Tom Flemons.
A template base class for a tensegrity spine.
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
virtual void onTeardown(BaseSpineModelLearning &subject)
std::vector< double > getGoalFeedback(const BaseSpineModelGoal *subject)
A series of functions to assist with file input/output.
Contains the definition of class tgBasicActuator.
virtual void onStep(BaseSpineModelLearning &subject, double dt)
const Config & getConfig() const
virtual void onStep(tgModel &model, double dt)
Definition of class CPGEquationsFB.
virtual void onSetup(tgModel &model)
virtual const double getCurrentLength() const
void notifyStep(double dt)
void assignNodeNumberFB(CPGEquationsFB &CPGSys, array_2D nodeParams)