NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
JSONMGFeedbackControlFM1.cpp
Go to the documentation of this file.
1 /*
2  * Copyright © 2012, United States Government, as represented by the
3  * Administrator of the National Aeronautics and Space Administration.
4  * All rights reserved.
5  *
6  * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed
7  * under the Apache License, Version 2.0 (the "License");
8  * you may not use this file except in compliance with the License.
9  * You may obtain a copy of the License at
10  * http://www.apache.org/licenses/LICENSE-2.0.
11  *
12  * Unless required by applicable law or agreed to in writing,
13  * software distributed under the License is distributed on an
14  * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
15  * either express or implied. See the License for the specific language
16  * governing permissions and limitations under the License.
17 */
18 
29 
30 
31 // Should include tgString, but compiler complains since its been
32 // included from BaseQuadModelLearning. Perhaps we should move things
33 // to a cpp over there
35 #include "core/tgBasicActuator.h"
38 #include "examples/learningSpines/tgCPGCableControl.h"
39 
40 #include "dev/dhustigschultz/BigPuppy_SpineOnly_Stats/BaseQuadModelLearning.h"
41 #include "helpers/FileHelpers.h"
42 
45 
46 #include "util/CPGEquationsFB.h"
47 #include "examples/learningSpines/tgCPGCableControl.h"
48 
49 #include "neuralNet/Neural Network v2/neuralNetwork.h"
50 
51 #include <json/json.h>
52 
53 #include <string>
54 #include <iostream>
55 #include <vector>
56 
57 //#define LOGGING
58 //#define USE_KINEMATIC
59 
60 using namespace std;
61 
63  int tm,
64  int om,
65  int param,
66  int segnum,
67  double ct,
68  double la,
69  double ha,
70  double lp,
71  double hp,
72  double kt,
73  double kp,
74  double kv,
75  bool def,
76  double cl,
77  double lf,
78  double hf,
79  double ffMin,
80  double ffMax,
81  double afMin,
82  double afMax,
83  double pfMin,
84  double pfMax,
85  double maxH,
86  double minH) :
87 JSONQuadCPGControl::Config::Config(ss, tm, om, param, segnum, ct, la, ha,
88  lp, hp, kt, kp, kv, def, cl, lf, hf),
89 freqFeedbackMin(ffMin),
90 freqFeedbackMax(ffMax),
91 ampFeedbackMin(afMin),
92 ampFeedbackMax(afMax),
93 phaseFeedbackMin(pfMin),
94 phaseFeedbackMax(pfMax),
95 maxHeight(maxH),
96 minHeight(minH)
97 {
98 
99 }
106  std::string args,
107  std::string resourcePath) :
108 JSONQuadCPGControl(config, args, resourcePath),
109 m_config(config)
110 {
111  // Path and filename handled by base class
112 
113 }
114 
115 JSONMGFeedbackControlFM1::~JSONMGFeedbackControlFM1()
116 {
117  delete nn;
118 }
119 
121 {
122  m_pCPGSys = new CPGEquationsFB(100);
123 
124  Json::Value root; // will contains the root value after parsing.
125  Json::Reader reader;
126 
127  bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
128  if ( !parsingSuccessful )
129  {
130  // report to the user the failure and their locations in the document.
131  std::cout << "Failed to parse configuration\n"
132  << reader.getFormattedErrorMessages();
133  throw std::invalid_argument("Bad filename for JSON");
134  }
135  // Get the value of the member of root named 'encoding', return 'UTF-8' if there is no
136  // such member.
137  Json::Value nodeVals = root.get("nodeVals", "UTF-8");
138  Json::Value edgeVals = root.get("edgeVals", "UTF-8");
139 
140  std::cout << nodeVals << std::endl;
141 
142  nodeVals = nodeVals.get("params", "UTF-8");
143  edgeVals = edgeVals.get("params", "UTF-8");
144 
145  array_4D edgeParams = scaleEdgeActions(edgeVals);
146  array_2D nodeParams = scaleNodeActions(nodeVals);
147 
148  setupCPGs(subject, nodeParams, edgeParams);
149 
150  Json::Value feedbackParams = root.get("feedbackVals", "UTF-8");
151  feedbackParams = feedbackParams.get("params", "UTF-8");
152 
153  // Setup neural network
154  m_config.numStates = feedbackParams.get("numStates", "UTF-8").asInt();
155  m_config.numActions = feedbackParams.get("numActions", "UTF-8").asInt();
156  //m_config.numHidden = feedbackParams.get("numHidden", "UTF-8").asInt();
157 
158  std::string nnFile = controlFilePath + feedbackParams.get("neuralFilename", "UTF-8").asString();
159 
160  nn = new neuralNetwork(m_config.numStates, m_config.numStates*2, m_config.numActions);
161 
162  nn->loadWeights(nnFile.c_str());
163 
164  initConditions = subject.getSegmentCOM(m_config.segmentNumber);
165  for (int i = 0; i < initConditions.size(); i++)
166  {
167  std::cout << initConditions[i] << " ";
168  }
169  std::cout << std::endl;
170 #ifdef LOGGING // Conditional compile for data logging
171  m_dataObserver.onSetup(subject);
172 #endif
173 
174 #if (0) // Conditional Compile for debug info
175  std::cout << *m_pCPGSys << std::endl;
176 #endif
177  m_updateTime = 0.0;
178  m_totalTime = 0.0; //For metrics.
179  bogus = false;
180 }
181 
183 {
184  m_updateTime += dt;
185  m_totalTime += dt;
186  if (m_updateTime >= m_config.controlTime)
187  {
188 #if (1)
189  std::vector<double> desComs = getFeedback(subject);
190 
191 #else
192  std::size_t numControllers = subject.getNumberofMuslces() * 3;
193 
194  double descendingCommand = 0.0;
195  std::vector<double> desComs (numControllers, descendingCommand);
196 #endif
197  try
198  {
199  m_pCPGSys->update(desComs, m_updateTime);
200  }
201  catch (std::runtime_error& e)
202  {
203  // Stops the trial immediately, lets teardown know it broke
204  bogus = true;
205  throw (e);
206  }
207 
208 #ifdef LOGGING // Conditional compile for data logging
209  m_dataObserver.onStep(subject, m_updateTime);
210 #endif
211  notifyStep(m_updateTime);
212  m_updateTime = 0;
213  }
214 
215  double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];
216  double currentHeightRear = subject.getSegmentCOM(6)[1];
217 
218  // Max and min heights added to config
219  if (currentHeight > m_config.maxHeight || currentHeight < m_config.minHeight || currentHeightRear > m_config.maxHeight || currentHeightRear < m_config.minHeight)
220  {
222  bogus = true;
223  throw std::runtime_error("Height out of range");
224  }
225 
226  //every 100 steps, get the COM and tensions of active muscles and store them in the JSON file.
227  if(0){
228  static int count = 0;
229  if(count > 100) {
230  std::cout << m_totalTime << std::endl;
231 
232  //Getting the center of mass of the entire structure:
233  std::vector<double> newConditions = subject.getSegmentCOM(m_config.segmentNumber);
234 
235  std::cout << "COM: " << newConditions[0] << " " << newConditions[1] << " " << newConditions[2] << " ";
236  std::cout << std::endl;
237 
238  count = 0;
239  }
240  else {
241  count++;
242  }
243  }
244 }
245 
247 {
248  scores.clear();
249  // @todo - check to make sure we ran for the right amount of time
250 
251  std::vector<double> finalConditions = subject.getSegmentCOM(m_config.segmentNumber);
252 
253  const double newX = finalConditions[0];
254  const double newZ = finalConditions[2];
255  const double oldX = initConditions[0];
256  const double oldZ = initConditions[2];
257 
258  const double distanceMoved = sqrt((newX-oldX) * (newX-oldX) +
259  (newZ-oldZ) * (newZ-oldZ));
260 
261  if (bogus)
262  {
263  scores.push_back(-1.0);
264  }
265  else
266  {
267  scores.push_back(distanceMoved);
268  }
269 
272  double totalEnergySpent=0;
273 
274  std::vector<tgSpringCableActuator* > tmpStrings = subject.find<tgSpringCableActuator> ("spiral");
275 
276  for(std::size_t i=0; i<tmpStrings.size(); i++)
277  {
278  tgSpringCableActuator::SpringCableActuatorHistory stringHist = tmpStrings[i]->getHistory();
279 
280  for(std::size_t j=1; j<stringHist.tensionHistory.size(); j++)
281  {
282  const double previousTension = stringHist.tensionHistory[j-1];
283  const double previousLength = stringHist.restLengths[j-1];
284  const double currentLength = stringHist.restLengths[j];
285  //TODO: examine this assumption - free spinning motor may require more power
286  double motorSpeed = (currentLength-previousLength);
287  if(motorSpeed > 0) // Vestigial code
288  motorSpeed = 0;
289  const double workDone = previousTension * motorSpeed;
290  totalEnergySpent += workDone;
291  }
292  }
293 
294  scores.push_back(totalEnergySpent);
295 
296  std::cout << "Dist travelled " << scores[0] << std::endl;
297 
298  Json::Value root; // will contains the root value after parsing.
299  Json::Reader reader;
300 
301  bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
302  if ( !parsingSuccessful )
303  {
304  // report to the user the failure and their locations in the document.
305  std::cout << "Failed to parse configuration\n"
306  << reader.getFormattedErrorMessages();
307  throw std::invalid_argument("Bad filename for JSON");
308  }
309 
310  Json::Value prevScores = root.get("scores", Json::nullValue);
311 
312  Json::Value subScores;
313  subScores["distance"] = scores[0];
314  subScores["energy"] = scores[1];
315 
316  prevScores.append(subScores);
317  root["scores"] = prevScores;
318 
319  ofstream payloadLog;
320  payloadLog.open(controlFilename.c_str(),ofstream::out);
321 
322  payloadLog << root << std::endl;
323 
324  delete m_pCPGSys;
325  m_pCPGSys = NULL;
326 
327  for(size_t i = 0; i < m_spineControllers.size(); i++)
328  {
329  delete m_spineControllers[i];
330  }
331  m_spineControllers.clear();
332 }
333 
334 void JSONMGFeedbackControlFM1::setupCPGs(BaseQuadModelLearning& subject, array_2D nodeActions, array_4D edgeActions)
335 {
336 
337  std::vector <tgSpringCableActuator*> spineMuscles = subject.find<tgSpringCableActuator> ("spiral");
338 
339  CPGEquationsFB& m_CPGFBSys = *(tgCast::cast<CPGEquations, CPGEquationsFB>(m_pCPGSys));
340 
341  for (std::size_t i = 0; i < spineMuscles.size(); i++)
342  {
343 
344  tgPIDController::Config config(20000.0, 0.0, 5.0, true); // Non backdrivable
345  tgCPGCableControl* pStringControl = new tgCPGCableControl(config);
346 
347  spineMuscles[i]->attach(pStringControl);
348 
349  // First assign node numbers
350  pStringControl->assignNodeNumberFB(m_CPGFBSys, nodeActions);
351 
352  m_spineControllers.push_back(pStringControl);
353  }
354 
355  // Then determine connectivity and setup string
356  for (std::size_t i = 0; i < m_spineControllers.size(); i++)
357  {
358  tgCPGActuatorControl * const pStringInfo = m_spineControllers[i];
359  assert(pStringInfo != NULL);
360  pStringInfo->setConnectivity(m_spineControllers, edgeActions);
361 
362  //String will own this pointer
363  tgImpedanceController* p_ipc = new tgImpedanceController( m_config.tension,
364  m_config.kPosition,
365  m_config.kVelocity);
366  if (m_config.useDefault)
367  {
368  pStringInfo->setupControl(*p_ipc);
369  }
370  else
371  {
372  pStringInfo->setupControl(*p_ipc, m_config.controlLength);
373  }
374  }
375 
376 }
377 
378 array_2D JSONMGFeedbackControlFM1::scaleNodeActions (Json::Value actions)
379 {
380  std::size_t numControllers = actions.size();
381  std::size_t numActions = actions[0].size();
382 
383  array_2D nodeActions(boost::extents[numControllers][numActions]);
384 
385  array_2D limits(boost::extents[2][numActions]);
386 
387  // Check if we need to update limits
388  assert(numActions == 5);
389 
390  limits[0][0] = m_config.lowFreq;
391  limits[1][0] = m_config.highFreq;
392  limits[0][1] = m_config.lowAmp;
393  limits[1][1] = m_config.highAmp;
394  limits[0][2] = m_config.freqFeedbackMin;
395  limits[1][2] = m_config.freqFeedbackMax;
396  limits[0][3] = m_config.ampFeedbackMin;
397  limits[1][3] = m_config.ampFeedbackMax;
398  limits[0][4] = m_config.phaseFeedbackMin;
399  limits[1][4] = m_config.phaseFeedbackMax;
400 
401  Json::Value::iterator nodeIt = actions.begin();
402 
403  // This one is square
404  for( std::size_t i = 0; i < numControllers; i++)
405  {
406  Json::Value nodeParam = *nodeIt;
407  for( std::size_t j = 0; j < numActions; j++)
408  {
409  nodeActions[i][j] = ( (nodeParam.get(j, 0.0)).asDouble() *
410  (limits[1][j] - limits[0][j])) + limits[0][j];
411  }
412  nodeIt++;
413  }
414 
415  return nodeActions;
416 }
417 
418 std::vector<double> JSONMGFeedbackControlFM1::getFeedback(BaseQuadModelLearning& subject)
419 {
420  // Placeholder
421  std::vector<double> feedback;
422 
423  const std::vector<tgSpringCableActuator*>& spineCables = subject.find<tgSpringCableActuator> ("spiral");
424 
425  double *inputs = new double[m_config.numStates];
426 
427  std::size_t n = spineCables.size();
428  for(std::size_t i = 0; i != n; i++)
429  {
430  std::vector< std::vector<double> > actions;
431 
432  const tgSpringCableActuator& cable = *(spineCables[i]);
433  std::vector<double > state = getCableState(cable);
434 
435  // Rescale to 0 to 1 (consider doing this inside getState
436  for (std::size_t i = 0; i < state.size(); i++)
437  {
438  inputs[i]=state[i] / 2.0 + 0.5;
439  }
440 
441  double *output = nn->feedForwardPattern(inputs);
442  vector<double> tmpAct;
443  for(int j=0;j<m_config.numActions;j++)
444  {
445  tmpAct.push_back(output[j]);
446  }
447  actions.push_back(tmpAct);
448 
449  std::vector<double> cableFeedback = transformFeedbackActions(actions);
450 
451  feedback.insert(feedback.end(), cableFeedback.begin(), cableFeedback.end());
452  }
453 
454 
455  return feedback;
456 }
457 
458 std::vector<double> JSONMGFeedbackControlFM1::getCableState(const tgSpringCableActuator& cable)
459 {
460  // For each string, scale value from -1 to 1 based on initial length or max tension of motor
461 
462  std::vector<double> state;
463 
464  // Scale length by starting length
465  const double startLength = cable.getStartLength();
466  state.push_back((cable.getCurrentLength() - startLength) / startLength);
467 
468  const double maxTension = cable.getConfig().maxTens;
469  state.push_back((cable.getTension() - maxTension / 2.0) / maxTension);
470 
471  return state;
472 }
473 
474 std::vector<double> JSONMGFeedbackControlFM1::transformFeedbackActions(std::vector< std::vector<double> >& actions)
475 {
476  // Placeholder
477  std::vector<double> feedback;
478 
479  // Leave in place for generalization later
480  const std::size_t numControllers = 1;
481  const std::size_t numActions = m_config.numActions;
482 
483  assert( actions.size() == numControllers);
484  assert( actions[0].size() == numActions);
485 
486  // Scale values back to -1 to +1
487  for( std::size_t i = 0; i < numControllers; i++)
488  {
489  for( std::size_t j = 0; j < numActions; j++)
490  {
491  feedback.push_back(actions[i][j] * 2.0 - 1.0);
492  }
493  }
494 
495  return feedback;
496 }
Contains the definition of class ImpedanceControl. $Id$.
void update(std::vector< double > &descCom, double dt)
virtual const double getTension() const
virtual void onStep(BaseQuadModelLearning &subject, double dt)
virtual const double getStartLength() const
void setConnectivity(const std::vector< tgCPGActuatorControl * > &allStrings, array_4D edgeParams)
virtual void onSetup(BaseQuadModelLearning &subject)
A controller for the template class BaseQuadModelLearning, modified for use on a quadruped.
Definition of the tgCPGStringControl observer class.
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)
A class to read a learning configuration from a .ini file.
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 AnnealEvolution. Adapting NeuroEvolution to do Simulated Annealing...
Contains the definition of class tgBasicActuator.
const Config & getConfig() const
std::vector< T * > find(const tgTagSearch &tagSearch)
Definition: tgModel.h:128
virtual void onStep(tgModel &model, double dt)
virtual array_4D scaleEdgeActions(Json::Value edgeParam)
Definition of class CPGEquationsFB.
virtual void onTeardown(BaseQuadModelLearning &subject)
virtual void onSetup(tgModel &model)
JSONMGFeedbackControlFM1(JSONMGFeedbackControlFM1::Config config, std::string args, std::string resourcePath="")
virtual const double getCurrentLength() const
void assignNodeNumberFB(CPGEquationsFB &CPGSys, array_2D nodeParams)