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>
67 std::string resourcePath) :
74 JSONGoalTensionNNW::~JSONGoalTensionNNW()
86 bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
87 if ( !parsingSuccessful )
90 std::cout <<
"Failed to parse configuration\n"
91 << reader.getFormattedErrorMessages();
92 throw std::invalid_argument(
"Bad filename for JSON");
96 Json::Value nodeVals = root.get(
"nodeVals",
"UTF-8");
97 Json::Value edgeVals = root.get(
"edgeVals",
"UTF-8");
99 nodeVals = nodeVals.get(
"params",
"UTF-8");
100 edgeVals = edgeVals.get(
"params",
"UTF-8");
103 array_2D nodeParams = scaleNodeActions(nodeVals);
105 setupCPGs(subject, nodeParams, edgeParams);
107 Json::Value feedbackParams = root.get(
"feedbackVals",
"UTF-8");
108 feedbackParams = feedbackParams.get(
"params",
"UTF-8");
111 m_config.numStates = feedbackParams.get(
"numStates",
"UTF-8").asInt();
112 m_config.numActions = feedbackParams.get(
"numActions",
"UTF-8").asInt();
113 m_config.numHidden = feedbackParams.get(
"numHidden",
"UTF-8").asInt();
115 std::string nnFile = controlFilePath + feedbackParams.get(
"neuralFilename",
"UTF-8").asString();
117 nn =
new neuralNetwork(m_config.numStates, m_config.numHidden, m_config.numActions);
119 nn->loadWeights(nnFile.c_str());
122 Json::Value goalParams = root.get(
"goalVals",
"UTF-8");
123 goalParams = goalParams.get(
"params",
"UTF-8");
126 m_config.goalStates = goalParams.get(
"numStates",
"UTF-8").asInt();
127 m_config.goalActions = goalParams.get(
"numActions",
"UTF-8").asInt();
128 m_config.goalHidden = goalParams.get(
"numHidden",
"UTF-8").asInt();
130 std::string nnFile_goal = controlFilePath + goalParams.get(
"neuralFilename",
"UTF-8").asString();
132 nn_goal =
new neuralNetwork(m_config.goalStates, m_config.goalHidden, m_config.goalActions);
134 nn_goal->loadWeights(nnFile_goal.c_str());
136 initConditions = subject.getSegmentCOM(m_config.segmentNumber);
137 #ifdef LOGGING // Conditional compile for data logging
138 m_dataObserver.
onSetup(subject);
141 #if (0) // Conditional Compile for debug info
142 std::cout << *m_pCPGSys << std::endl;
152 double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];
154 if (m_updateTime >= m_config.controlTime)
158 #if (1) // Goal and cable
160 std::vector<double> desComs = getFeedback(subject);
162 const BaseSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, BaseSpineModelGoal>(subject);
167 std::size_t numControllers = subject.getNumberofMuslces() * 3;
169 double descendingCommand = 0.0;
170 std::vector<double> desComs (numControllers, descendingCommand);
172 const BaseSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, BaseSpineModelGoal>(subject);
179 m_pCPGSys->
update(desComs, m_updateTime);
181 catch (std::runtime_error& e)
188 #ifdef LOGGING // Conditional compile for data logging
189 m_dataObserver.
onStep(subject, m_updateTime);
200 if (currentHeight > 25 || currentHeight < 1.0)
204 throw std::runtime_error(
"Height out of range");
214 std::vector<double> finalConditions = subject.getSegmentCOM(m_config.segmentNumber);
216 const double newX = finalConditions[0];
217 const double newZ = finalConditions[2];
218 const double oldX = initConditions[0];
219 const double oldZ = initConditions[2];
221 const BaseSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, BaseSpineModelGoal>(subject);
223 const double totalDistanceMoved = sqrt((newX-oldX) * (newX-oldX) +
224 (newZ-oldZ) * (newZ-oldZ));
226 const double distanceMoved = calculateDistanceMoved(goalSubject);
230 scores.push_back(-1.0);
234 scores.push_back(distanceMoved);
239 double totalEnergySpent=0;
241 std::vector<tgSpringCableActuator* > tmpStrings = subject.getAllMuscles();
243 for(std::size_t i=0; i<tmpStrings.size(); i++)
249 double dt = m_totalTime / (double)histSize;
257 const double previousLength = stringHist.
restLengths[j-1];
258 const double currentLength = stringHist.
restLengths[j];
260 double motorSpeed = (currentLength-previousLength);
263 const double workDone = previousTension * motorSpeed;
264 totalEnergySpent += workDone;
268 for(std::size_t j=0; j < histSize; j++)
273 const double workDone = previousTension * motorSpeed * dt;
274 totalEnergySpent += workDone;
279 scores.push_back(totalEnergySpent);
281 std::cout <<
"Dist travelled towards goal " << scores[0] <<
" Total Distance Travelled " << totalDistanceMoved ;
282 std::cout <<
" Energy Spent: " << scores[1] << std::endl;
287 bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
288 if ( !parsingSuccessful )
291 std::cout <<
"Failed to parse configuration\n"
292 << reader.getFormattedErrorMessages();
293 throw std::invalid_argument(
"Bad filename for JSON");
296 Json::Value prevScores = root.get(
"scores", Json::nullValue);
298 Json::Value subScores;
299 subScores[
"distance"] = distanceMoved;
300 subScores[
"energy"] = totalEnergySpent;
302 prevScores.append(subScores);
303 root[
"scores"] = prevScores;
306 payloadLog.open(controlFilename.c_str(),ofstream::out);
308 payloadLog << root << std::endl;
313 for(
size_t i = 0; i < m_allControllers.size(); i++)
315 delete m_allControllers[i];
317 m_allControllers.clear();
324 const int nSeg = subject->getSegments() - 1;
330 const btVector3 currentPosVector = subject->getSegmentCOMVector(m_config.segmentNumber);
332 const btVector3 goalPosition = subject->goalBoxPosition();
334 btVector3 desiredHeading = (goalPosition - currentPosVector).normalize();
338 const btVector3 firstSegment = subject->getSegmentCOMVector(0);
339 const btVector3 secondSegment = subject->getSegmentCOMVector(1);
341 btVector3 currentHeading = (firstSegment - secondSegment).normalize();
343 std::vector<double> state;
344 state.push_back(desiredHeading.getX());
345 state.push_back(desiredHeading.getZ());
346 state.push_back(currentHeading.getX());
347 state.push_back(currentHeading.getZ());
349 double *inputs =
new double[m_config.goalStates];
350 for (std::size_t i = 0; i < state.size(); i++)
352 assert(state[i] >= -1.0 && state[i] <= 1.0);
357 double *output = nn_goal->feedForwardPattern(inputs);
359 vector<double> actions;
362 for(
int j=0;j<m_config.goalActions;j++)
364 std::cout << output[j] <<
" ";
366 std::cout << std::endl;
369 for(
int j=0;j<m_config.goalActions;j++)
371 actions.push_back(output[j]);
374 transformFeedbackActions(actions);
376 assert(m_config.goalActions * nSeg == m_allControllers.size());
379 for (
int i = 0; i != nSeg; i++)
381 for(
int j=0;j<m_config.goalActions;j++)
383 m_allControllers[i * m_config.goalActions + j]->updateTensionSetpoint(actions[j] * m_config.tensFeedback + m_config.tensFeedback);
Contains the definition of class ImpedanceControl. $Id$.
void update(std::vector< double > &descCom, double dt)
std::deque< double > tensionHistory
virtual array_4D scaleEdgeActions(Json::Value edgeParam)
virtual void onSetup(BaseSpineModelLearning &subject)
std::deque< double > restLengths
Definition of the tgCPGStringControl observer class.
A controller for the template class BaseSpineModelLearning.
Implementing the tetrahedral complex spine inspired by Tom Flemons.
std::deque< double > lastVelocities
JSONGoalTensionNNW(JSONGoalControl::Config config, std::string args, std::string resourcePath="")
virtual std::vector< double > getGoalFeedback(const BaseSpineModelGoal *subject)
virtual void onStep(BaseSpineModelLearning &subject, double dt)
A template base class for a tensegrity spine.
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
A series of functions to assist with file input/output.
Contains the definition of class tgBasicActuator.
virtual void onStep(tgModel &model, double dt)
Definition of class CPGEquationsFB.
virtual void onSetup(tgModel &model)
void notifyStep(double dt)
virtual void onTeardown(BaseSpineModelLearning &subject)