NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
JSONQuadFeedbackControl.cpp
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 
27 #include "JSONQuadFeedbackControl.h"
28 
29 
30 // Should include tgString, but compiler complains since its been
31 // included from BaseSpineModelLearning. Perhaps we should move things
32 // to a cpp over there
33 #include "core/tgSpringCableActuator.h"
34 #include "core/tgBasicActuator.h"
35 #include "controllers/tgImpedanceController.h"
36 #include "examples/learningSpines/tgCPGActuatorControl.h"
37 #include "examples/learningSpines/tgCPGCableControl.h"
38 
39 #include "examples/learningSpines/BaseSpineModelLearning.h"
40 #include "helpers/FileHelpers.h"
41 
42 #include "learning/AnnealEvolution/AnnealEvolution.h"
43 #include "learning/Configuration/configuration.h"
44 
45 #include "util/CPGEquationsFB.h"
46 #include "examples/learningSpines/tgCPGCableControl.h"
47 
48 #include "neuralNet/Neural Network v2/neuralNetwork.h"
49 
50 #include <json/json.h>
51 
52 #include <string>
53 #include <iostream>
54 #include <vector>
55 
56 //#define LOGGING
57 #define USE_KINEMATIC
58 
59 using namespace std;
60 
61 const string strSpineTag = "spine ";
62 const string strLeftShoulderTag = "left shoulder ";
63 const string strLeftHipTag = "left hip ";
64 // Have to use underscores (arbitrary) for some of these because of the way tags work
65 const string strLeftFrontLegTag = "left_front_leg ";
66 const string strLeftHindLegTag = "left_hind_leg ";
67 const string strRightShoulderTag = "right shoulder ";
68 const string strRightHipTag = "right hip ";
69 const string strRightFrontLegTag = "right_front_leg ";
70 const string strRightHindLegTag = "right_hind_leg ";
71 
72 JSONQuadFeedbackControl::Config::Config(int ss,
73  int tm,
74  int om,
75  int param,
76  int segnum,
77  double ct,
78  double la,
79  double ha,
80  double lp,
81  double hp,
82  double kt,
83  double kp,
84  double kv,
85  bool def,
86  double cl,
87  double lf,
88  double hf,
89  double ffMin,
90  double ffMax,
91  double afMin,
92  double afMax,
93  double pfMin,
94  double pfMax,
95  double maxH,
96  double minH,
97  double hffMin,
98  double hffMax) :
99 JSONCPGControl::Config::Config(ss, tm, om, param, segnum, ct, la, ha,
100  lp, hp, kt, kp, kv, def, cl, lf, hf),
101 freqFeedbackMin(ffMin),
102 freqFeedbackMax(ffMax),
103 highFreqFeedbackMin(hffMin),
104 highFreqFeedbackMax(hffMax),
105 ampFeedbackMin(afMin),
106 ampFeedbackMax(afMax),
107 phaseFeedbackMin(pfMin),
108 phaseFeedbackMax(pfMax),
109 maxHeight(maxH),
110 minHeight(minH)
111 {
112 
113 }
119 JSONQuadFeedbackControl::JSONQuadFeedbackControl(JSONQuadFeedbackControl::Config config,
120  std::string args,
121  std::string resourcePath) :
122 JSONCPGControl(config, args, resourcePath),
123 m_config(config)
124 {
125  // Path and filename handled by base class
126 
127 }
128 
129 JSONQuadFeedbackControl::~JSONQuadFeedbackControl()
130 {
131  delete nn;
132 }
133 
134 void JSONQuadFeedbackControl::onSetup(BaseSpineModelLearning& subject)
135 {
136  // spine, 4 legs, 4 hips/shoulers
137  n_bodyParts = 9;
138 
139  m_pCPGSys = new CPGEquationsFB(100);
140 
141  Json::Value root; // will contains the root value after parsing.
142  Json::Reader reader;
143 
144  bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
145  if ( !parsingSuccessful )
146  {
147  // report to the user the failure and their locations in the document.
148  std::cout << "Failed to parse configuration\n"
149  << reader.getFormattedErrorMessages();
150  throw std::invalid_argument("Bad filename for JSON");
151  }
152  // Get the value of the member of root named 'encoding', return 'UTF-8' if there is no
153  // such member.
154  Json::Value nodeVals = root.get("nodeVals", "UTF-8");
155  Json::Value edgeVals = root.get("edgeVals", "UTF-8");
156 
157  std::cout << nodeVals << std::endl;
158 
159  nodeVals = nodeVals.get("params", "UTF-8");
160  edgeVals = edgeVals.get("params", "UTF-8");
161 
162  // parameters for high CPG to high CPG coupling
163  Json::Value highEdgeVals = root.get("highVals", "UTF-8");
164  highEdgeVals = highEdgeVals.get("params", "UTF-8");
165 
166  // parameters for high CPG to low CPG coupling
167  Json::Value highLowEdgeVals = root.get("hLowVals", "UTF-8");
168  highLowEdgeVals = highLowEdgeVals.get("params", "UTF-8");
169 
170  // This is kinda nasty and out of place to put here, but I need to initialize
171  // some important variables.
172  std::vector <tgSpringCableActuator*> spineMuscles = subject.find<tgSpringCableActuator> (strSpineTag);
173  std::vector <tgSpringCableActuator*> leftShoulderMuscles = subject.find<tgSpringCableActuator> (strLeftShoulderTag);
174  std::vector <tgSpringCableActuator*> leftFrontLegMuscles = subject.find<tgSpringCableActuator> (strLeftFrontLegTag);
175  n_muscSpine = spineMuscles.size();
176  n_muscHip = leftShoulderMuscles.size();
177  n_muscLeg = leftFrontLegMuscles.size();
178 
179  array_4D edgeParams = scaleEdgeActions(edgeVals);
180  array_2D nodeParams = scaleNodeActions(nodeVals);
181  array_4D highEdgeParams = scaleHighEdgeActions(highEdgeVals);
182 
183  setupCPGs(subject, nodeParams, edgeParams);
184  setupHighCPGs(nodeParams, highEdgeParams, highLowEdgeVals);
185 
186  Json::Value feedbackParams = root.get("feedbackVals", "UTF-8");
187  feedbackParams = feedbackParams.get("params", "UTF-8");
188 
189  // Setup neural network
190  m_config.numStates = feedbackParams.get("numStates", "UTF-8").asInt();
191  m_config.numActions = feedbackParams.get("numActions", "UTF-8").asInt();
192  //m_config.numHidden = feedbackParams.get("numHidden", "UTF-8").asInt();
193 
194  std::string nnFile = controlFilePath + feedbackParams.get("neuralFilename", "UTF-8").asString();
195 
196  nn = new neuralNetwork(m_config.numStates, m_config.numStates*2, m_config.numActions);
197 
198  nn->loadWeights(nnFile.c_str());
199 
200  initConditions = subject.getSegmentCOM(m_config.segmentNumber);
201  for (int i = 0; i < initConditions.size(); i++)
202  {
203  std::cout << initConditions[i] << " ";
204  }
205  std::cout << std::endl;
206 #ifdef LOGGING // Conditional compile for data logging
207  m_dataObserver.onSetup(subject);
208 #endif
209 
210 #if (0) // Conditional Compile for debug info
211  std::cout << *m_pCPGSys << std::endl;
212 #endif
213  m_updateTime = 0.0;
214  bogus = false;
215 }
216 
217 void JSONQuadFeedbackControl::onStep(BaseSpineModelLearning& subject, double dt)
218 {
219  m_updateTime += dt;
220  if (m_updateTime >= m_config.controlTime)
221  {
222 #if (1)
223  std::vector<double> desComs = getFeedback(subject);
224 
225 #else
226  std::size_t numControllers = subject.getNumberofMuslces() * 3;
227 
228  double descendingCommand = 0.0;
229  std::vector<double> desComs (numControllers, descendingCommand);
230 #endif
231  try
232  {
233  m_pCPGSys->update(desComs, m_updateTime);
234  }
235  catch (std::runtime_error& e)
236  {
237  // Stops the trial immediately, lets teardown know it broke
238  bogus = true;
239  throw (e);
240  }
241 
242 #ifdef LOGGING // Conditional compile for data logging
243  m_dataObserver.onStep(subject, m_updateTime);
244 #endif
245  notifyStep(m_updateTime);
246  m_updateTime = 0;
247  }
248 
249  double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];
250 
252  if (currentHeight > m_config.maxHeight || currentHeight < m_config.minHeight)
253  {
255  bogus = true;
256  throw std::runtime_error("Height out of range");
257  }
258 }
259 
260 void JSONQuadFeedbackControl::onTeardown(BaseSpineModelLearning& subject)
261 {
262  scores.clear();
263  // @todo - check to make sure we ran for the right amount of time
264 
265  std::vector<double> finalConditions = subject.getSegmentCOM(m_config.segmentNumber);
266 
267  const double newX = finalConditions[0];
268  const double newZ = finalConditions[2];
269  const double oldX = initConditions[0];
270  const double oldZ = initConditions[2];
271 
272  const double distanceMoved = sqrt((newX-oldX) * (newX-oldX) +
273  (newZ-oldZ) * (newZ-oldZ));
274  //double diff_of_sqrs = (newX-oldX)*(newX-oldX) - (newZ-oldZ)*(newZ-oldZ);
275  //const double distanceMoved = diff_of_sqrs >= 0 ? sqrt(diff_of_sqrs) : -sqrt(-diff_of_sqrs);
276 
277  if (bogus)
278  {
279  scores.push_back(-1.0);
280  }
281  else
282  {
283  scores.push_back(distanceMoved);
284  }
285 
288  double totalEnergySpent=0;
289 
290  std::vector<tgSpringCableActuator* > tmpStrings = subject.find<tgSpringCableActuator> (strSpineTag);
291 
292  for(std::size_t i=0; i<tmpStrings.size(); i++)
293  {
294  tgSpringCableActuator::SpringCableActuatorHistory stringHist = tmpStrings[i]->getHistory();
295 
296  for(std::size_t j=1; j<stringHist.tensionHistory.size(); j++)
297  {
298  const double previousTension = stringHist.tensionHistory[j-1];
299  const double previousLength = stringHist.restLengths[j-1];
300  const double currentLength = stringHist.restLengths[j];
301  //TODO: examine this assumption - free spinning motor may require more power
302  double motorSpeed = (currentLength-previousLength);
303  if(motorSpeed > 0) // Vestigial code
304  motorSpeed = 0;
305  const double workDone = previousTension * motorSpeed;
306  totalEnergySpent += workDone;
307  }
308  }
309 
310  scores.push_back(totalEnergySpent);
311 
312  std::cout << "Dist travelled " << scores[0] << std::endl;
313 
314  Json::Value root; // will contains the root value after parsing.
315  Json::Reader reader;
316 
317  bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
318  if ( !parsingSuccessful )
319  {
320  // report to the user the failure and their locations in the document.
321  std::cout << "Failed to parse configuration\n"
322  << reader.getFormattedErrorMessages();
323  throw std::invalid_argument("Bad filename for JSON");
324  }
325 
326  Json::Value prevScores = root.get("scores", Json::nullValue);
327 
328  Json::Value subScores;
329  subScores["distance"] = scores[0];
330  subScores["energy"] = totalEnergySpent;
331 
332  prevScores.append(subScores);
333  root["scores"] = prevScores;
334 
335  ofstream payloadLog;
336  payloadLog.open(controlFilename.c_str(),ofstream::out);
337 
338  payloadLog << root << std::endl;
339 
340  delete m_pCPGSys;
341  m_pCPGSys = NULL;
342 
343  for (size_t b = 0; b < 9; b++)
344  {
345  for(size_t i = 0; i < m_controllers[b].size(); i++)
346  {
347  delete m_controllers[b][i];
348  }
349  m_controllers[b].clear();
350  }
351 
352  for (size_t i = 0; i < m_highControllers.size(); i++)
353  {
354  delete m_highControllers[i];
355  }
356  m_highControllers.clear();
357 }
358 
359 void JSONQuadFeedbackControl::setupCPGs(BaseSpineModelLearning& subject, array_2D nodeActions, array_4D edgeActions)
360 {
361  CPGEquationsFB& m_CPGFBSys = *(tgCast::cast<CPGEquations, CPGEquationsFB>(m_pCPGSys));
362 
363  std::vector <tgSpringCableActuator*> spineMuscles = subject.find<tgSpringCableActuator> (strSpineTag);
364  std::vector <tgSpringCableActuator*> leftShoulderMuscles = subject.find<tgSpringCableActuator> (strLeftShoulderTag);
365  std::vector <tgSpringCableActuator*> leftHipMuscles = subject.find<tgSpringCableActuator> (strLeftHipTag);
366  std::vector <tgSpringCableActuator*> leftFrontLegMuscles = subject.find<tgSpringCableActuator> (strLeftFrontLegTag);
367  std::vector <tgSpringCableActuator*> leftHindLegMuscles = subject.find<tgSpringCableActuator> (strLeftHindLegTag);
368  std::vector <tgSpringCableActuator*> rightShoulderMuscles = subject.find<tgSpringCableActuator> (strRightShoulderTag);
369  std::vector <tgSpringCableActuator*> rightHipMuscles = subject.find<tgSpringCableActuator> (strRightHipTag);
370  std::vector <tgSpringCableActuator*> rightFrontLegMuscles = subject.find<tgSpringCableActuator> (strRightFrontLegTag);
371  std::vector <tgSpringCableActuator*> rightHindLegMuscles = subject.find<tgSpringCableActuator> (strRightHindLegTag);
372 
373  std::vector <tgSpringCableActuator*> allMuscles[9] = { spineMuscles, leftShoulderMuscles, leftFrontLegMuscles, rightShoulderMuscles, rightFrontLegMuscles, leftHipMuscles, leftHindLegMuscles, rightHipMuscles, rightHindLegMuscles };
374 
375  // which muscle groups get which node params
376  //const size_t node_assignment[n_bodyParts] = { 0, 1, 1, 2, 2, 3, 3, 4, 4 };
377  for (std::size_t b = 0; b < n_bodyParts; b++)
378  {
379  for (std::size_t i = 0; i < allMuscles[b].size(); i++)
380  {
381 
382  tgPIDController::Config config(20000.0, 0.0, 5.0, true); // Non backdrivable
383  tgCPGCableControl* pStringControl = new tgCPGCableControl(config);
384 
385  allMuscles[b][i]->attach(pStringControl);
386 
387  // need to make a 1 row 2D array so we can pass it into assignNodeNumber.
388  // using nodeActions will not work. (notice subtle name difference) ~B
389  array_2D nodeAction(boost::extents[1][nodeActions.shape()[1]]);
390  for (std::size_t a = 0; a < nodeActions.shape()[1]; a++)
391  nodeAction[0][a] = nodeActions[(b+1)/2 + 5][a];
392  //nodeAction[0][a] = nodeActions[node_assignment[b]+5][a];
393  // Why b+5? Because node params 0-4 have already been assigned to high level CPGs (above)
394 
395  // assign node number
396  pStringControl->assignNodeNumberFB(m_CPGFBSys, nodeAction);
397 
398  m_controllers[b].push_back(pStringControl);
399  }
400  }
401 
402  // The starting index (x and y) for the edgeActions matrix
403  // Remember this 4D matrix contains a block matrix
404  size_t actionsStart = 0;
405  // Then determine connectivity and setup string
406  for (size_t b = 0; b < n_bodyParts; b++)
407  {
408  for (std::size_t i = 0; i < m_controllers[b].size(); i++)
409  {
410  // Have fun understanding this. It was fun to write. :)
411  // Hint: numMuscles is how many muscles exist in body part b
412  size_t numMuscles = b == 0 ? n_muscSpine : (b % 2 ? n_muscHip : n_muscLeg);
413 
414  array_4D edgeActionsForBodyPart(boost::extents[3][numMuscles][numMuscles][edgeActions.shape()[3]]);
415  // I'm doing a nasty copy. boost's documentation for copying a subarray is too confusing.
416  for (size_t x = 0; x < 3; x++) for (size_t y = 0; y < numMuscles; y++) for (size_t z = 0; z < numMuscles; z++) for (size_t w = 0; w < edgeActions.shape()[3]; w++)
417  edgeActionsForBodyPart[x][y][z][w] = edgeActions[x][y+actionsStart][z+actionsStart][w];
418 
419  tgCPGActuatorControl * const pStringInfo = m_controllers[b][i];
420  assert(pStringInfo != NULL);
421  pStringInfo->setConnectivity(m_controllers[b], edgeActionsForBodyPart);
422 
423  //String will own this pointer
424  tgImpedanceController* p_ipc = new tgImpedanceController( m_config.tension,
425  m_config.kPosition,
426  m_config.kVelocity);
427  if (m_config.useDefault)
428  {
429  pStringInfo->setupControl(*p_ipc);
430  }
431  else
432  {
433  pStringInfo->setupControl(*p_ipc, m_config.controlLength);
434  }
435  }
436  // I love one-liners. ~BG
437  actionsStart += b == 0 ? 16 : (b % 2 ? n_muscHip : n_muscLeg);
438  }
439 }
440 
441 void JSONQuadFeedbackControl::setupHighCPGs(array_2D nodeActions, array_4D highEdgeActions, Json::Value highLowEdgeActions)
442 {
443  CPGEquationsFB& m_CPGFBSys = *(tgCast::cast<CPGEquations, CPGEquationsFB>(m_pCPGSys));
444 
445  // Make higher level CPGs, give them node params
446  for (std::size_t b = 5; b < 10; b++)
447  {
448  tgPIDController::Config config(20000.0, 0.0, 5.0, true); // Non backdrivable
449  tgCPGCableControl* pStringControl = new tgCPGCableControl(config);
450 
451  // need to make a 1 row 2D array so we can pass it into assignNodeNumber.
452  // using nodeActions will not work. (notice subtle name difference) ~B
453  array_2D nodeAction(boost::extents[1][nodeActions.shape()[1]]);
454  for (std::size_t a = 0; a < nodeActions.shape()[1]; a++)
455  nodeAction[0][a] = nodeActions[b][a];
456 
457  // must give node number, to make things easier and stuff
458  pStringControl->assignNodeNumberFB(m_CPGFBSys, nodeAction);
459 
460  m_highControllers.push_back(pStringControl);
461  }
462  // So the higher level CPGs are indices 5-9
463 
464 
465 
466  // Determine connectivity between higher level CPGs
467  for (std::size_t i = 0; i < m_highControllers.size(); i++)
468  {
469  tgCPGActuatorControl * const pStringInfo = m_highControllers[i];
470  assert(pStringInfo != NULL);
471  // TODO make sure this works
472  pStringInfo->setConnectivity(m_highControllers, highEdgeActions);
473  }
474 
475 
476 
477 
478  // Now couple high level with low level
479 
480  double lowerLimit = m_config.lowPhase;
481  double upperLimit = m_config.highPhase;
482  double range = upperLimit - lowerLimit;
483 
484  Json::Value::iterator edgeIt = highLowEdgeActions.end();
485 
486  // Number of node to start at (Note that nodes for low-level CPGs are added first)
487  int nodeStart = 0;
488  // iterate through high-level CPGs
489  for (size_t b = 0; b < 5; b++)
490  {
491  edgeIt--;
492 
493  std::vector<int> connectivityList;
494  std::vector<double> weights;
495  std::vector<double> phases;
496 
497  Json::Value param = *edgeIt;
498  assert(param.size() == 2);
499 
500  // n is number of muscles for body part b
501  int n = b == 0 ? n_muscSpine : n_muscHip + n_muscLeg;
502 
503  // !
504  for (size_t i = 0; i < n; i++, nodeStart++)
505  {
506  connectivityList.push_back(nodeStart);
507  weights.push_back(param[0].asDouble());
508  phases.push_back(param[1].asDouble() * (range) + lowerLimit);
509  }
510 
511  int highCPGnode = b + n_muscSpine + n_muscHip*4 + n_muscLeg*4;
512  m_CPGFBSys.defineConnections(highCPGnode, connectivityList, weights, phases);
513  }
514 
515  //assert(nodeStart == 16+10*8+5);
516 
517  // TODO?
518  assert(highLowEdgeActions.begin() == edgeIt);
519 }
520 
521 /*
522 void JSONQuadFeedbackControl::setupHighCouplings(array_4D highEdgeActions)
523 {
524  // Determine connectivity between higher level CPGs
525  for (std::size_t i = 0; i < m_highControllers.size(); i++)
526  {
527  tgCPGActuatorControl * const pStringInfo = m_highControllers[i];
528  assert(pStringInfo != NULL);
529  pStringInfo->setConnectivity(m_highControllers, highEdgeActions);
530 
531  //String will own this pointer
532  tgImpedanceController* p_ipc = new tgImpedanceController( m_config.tension,
533  m_config.kPosition,
534  m_config.kVelocity);
535  if (m_config.useDefault)
536  {
537  pStringInfo->setupControl(*p_ipc);
538  }
539  else
540  {
541  pStringInfo->setupControl(*p_ipc, m_config.controlLength);
542  }
543  }
544 }
545 
546 void JSONQuadFeedbackControl::setupHighLowCouplings(Json::Value highLowEdgeActions)
547 {
548  // Now couple high level with low level
549  // list of other nodenums, eq size list of weights/phases
550  //m_pCPGSystem->defineConnections(m_nodeNumber, connectivityList, weights, phases);
551 
552  double lowerLimit = m_config.lowPhase;
553  double upperLimit = m_config.highPhase;
554  double range = upperLimit - lowerLimit;
555 
556  CPGEquationsFB& m_CPGFBSys = *(tgCast::cast<CPGEquations, CPGEquationsFB>(m_pCPGSys));
557 
558  Json::Value::iterator edgeIt = highLowEdgeActions.end();
559 
560  // Number of node to start at (Note that nodes 0-4 are high CPGs)
561  int nodeStart = 5;
562  for (size_t b = 0; b < 5; b++)
563  {
564  edgeIt--;
565 
566  std::vector<int> connectivityList;
567  std::vector<double> weights;
568  std::vector<double> phases;
569 
570  Json::Value param = *edgeIt;
571  assert(param.size() == 2);
572 
573  // Nasty brute forcing. Don't try this at home. Shield your eyes if necessary.
574  int n = b == 0 ? 16 : 20;
575  // 20 because each hip/shoulder + leg pair has 20 total strings
576  // !
577  for (size_t i = 0; i < n; i++, nodeStart++)
578  {
579  connectivityList.push_back(nodeStart);
580  weights.push_back(param[0].asDouble());
581  phases.push_back(param[1].asDouble());
582  }
583 
584  m_CPGFBSys.defineConnections(b, connectivityList, weights, phases);
585  }
586 
587  assert(nodeStart == 16+10*8+5);
588 
589  // TODO?
590  assert(highLowEdgeActions.begin() == edgeIt);
591 }
592 */
593 
594 array_4D JSONQuadFeedbackControl::scaleEdgeActions
595  (Json::Value edgeParam)
596 {
597  // This creates a gigantic 4D array where the 2nd and 3rd dimensions
598  // form a block matrix (wikipedia it) with blocks along the diagonal.
599  // Each block corresponds to the couplings among muscles in the same body part.
600  // So, the first block is couplings between spine muscles only, the next block
601  // is couplings between left front shoulder muscles only, etc.
602 
603  assert(edgeParam[0].size() == 2);
604 
605  double lowerLimit = m_config.lowPhase;
606  double upperLimit = m_config.highPhase;
607  double range = upperLimit - lowerLimit;
608 
609  array_4D actionList(boost::extents[m_config.segmentSpan]
610  [m_config.theirMuscles]
611  [m_config.ourMuscles]
612  [m_config.params]);
613 
614  /* Horrid while loop to populate upper diagonal of matrix, since
615  * its symmetric and we want to minimze parameters used in learing
616  * note that i==1, j==k will refer to the same muscle
617  * @todo use boost to set up array so storage is only allocated for
618  * elements that are used
619  */
620  int i = 0;
621  int j = 0;
622  int k = 0;
623 
624  // Quirk of the old learning code. Future examples can move forward
625  Json::Value::iterator edgeIt = edgeParam.end();
626 
627  int count = 0;
628 
629  // 16 is the number of muscles per segment in the spine
630  // I don't know how to find this dynamically if this number changes
631  // This might be findable using tags, but we'll leave this as an exercise for the reader
632  int musc = 16;
633  for (size_t b = 0; b < n_bodyParts; b++)
634  {
635  while (i < m_config.segmentSpan)
636  {
637  // Only the spine will have couplings with muscles on other rigid bodies
638  // For legs and hips and stuff, only the muscles on the same rigid body
639  // can couple
640  if (b != 0 && i != 1)
641  {
642  i++;
643  continue;
644  }
645 
646  while(j < musc)
647  {
648  while(k < musc)
649  {
650  //cout << b << " " << i << " " << j << " " << k << " " << musc << endl;
651  if (edgeIt == edgeParam.begin())
652  {
653  std::cout << "ran out before table populated!"
654  << std::endl;
655  cout << count << endl;
656  cout << b << " " << i << " " << j << " " << k << " " << musc << endl;
658  break;
659  }
660  else
661  {
662  if (i == 1 && j == k)
663  {
664  // std::cout << "Skipped identical muscle" << std::endl;
665  //Skip since its the same muscle
666  }
667  else
668  {
669  cout << b << " " << i << " " << j << " " << k << " " << musc << endl;
670  edgeIt--;
671  Json::Value edgeParam1 = *edgeIt;
672  assert(edgeParam1.size() == 2);
673  // Weight from 0 to 1
674  actionList[i][j][k][0] = edgeParam1[0].asDouble();
675  //std::cout << actionList[i][j][k][0] << " ";
676  // Phase offset from -pi to pi
677  actionList[i][j][k][1] = edgeParam1[1].asDouble() *
678  (range) + lowerLimit;
679  //std::cout << actionList[i][j][k][1] << std::endl;
680  count++;
681  }
682  }
683  k++;
684  }
685  j++;
686  k = j;
687 
688  }
689  j = musc - (b > 0 ? (b % 2 ? n_muscHip : n_muscLeg) : 16);
690  k = j;
691  i++;
692  }
693  j = musc;
694  k = musc;
695  i = 0;
696  musc += b % 2 ? n_muscHip : n_muscLeg;
697  }
698 
699  std::cout<< "Params used: " << count << std::endl;
700 
701  assert(edgeParam.begin() == edgeIt);
702 
703  return actionList;
704 }
705 
706 array_4D JSONQuadFeedbackControl::scaleHighEdgeActions (Json::Value highEdgeParam)
707 {
708  assert(highEdgeParam[0].size() == 2);
709 
710  double lowerLimit = m_config.lowPhase;
711  double upperLimit = m_config.highPhase;
712  double range = upperLimit - lowerLimit;
713 
714  // This is a 4d array, but we don't really worry about 1st dimension
715  // since these higher CPGs aren't attached to any rigid bodies
716  // Using 5 by 5 because there are 5 higher level CPGs
717  // Maybe someday someone will program a more elegant solution... Nah, that won't happen.
718  array_4D actionList(boost::extents[m_config.segmentSpan]
719  [5]
720  [5]
721  [m_config.params]);
722 
723  /* Horrid while loop to populate upper diagonal of matrix, since
724  * its symmetric and we want to minimze parameters used in learing
725  * note that i==1, j==k will refer to the same muscle
726  * @todo use boost to set up array so storage is only allocated for
727  * elements that are used
728  */
729  int i = 0;
730  int j = 0;
731  int k = 0;
732 
733  // Quirk of the old learning code. Future examples can move forward
734  Json::Value::iterator highEdgeIt = highEdgeParam.end();
735 
736  int count = 0;
737 
738  while(j < 5)
739  {
740  while(k < 5)
741  {
742  if (j == k)
743  {
744  // std::cout << "Skipped identical muscle" << std::endl;
745  //Skip since its the same muscle
746  }
747  else
748  {
749  if (highEdgeIt == highEdgeParam.begin())
750  {
751  std::cout << "ran out before table populated!"
752  << std::endl;
754  break;
755  }
756  else
757  {
758  //cout << j << " " << k << endl;
759  highEdgeIt--;
760  Json::Value edgeParam1 = *highEdgeIt;
761  assert(edgeParam1.size() == 2);
762  // Weight from 0 to 1
763  while (i < m_config.segmentSpan)
764  {
765  actionList[i][j][k][0] = edgeParam1[0].asDouble();
766  //std::cout << actionList[i][j][k][0] << " ";
767  // Phase offset from -pi to pi
768  actionList[i][j][k][1] = edgeParam1[1].asDouble() *
769  (range) + lowerLimit;
770  //std::cout << actionList[i][j][k][1] << std::endl;
771  i++;
772  }
773  i = 0;
774  count++;
775  }
776  }
777  k++;
778  }
779  j++;
780  k = j;
781  }
782 
783  std::cout<< "Params used: " << count << std::endl;
784 
785  assert(highEdgeParam.begin() == highEdgeIt);
786 
787  return actionList;
788 }
789 
790 array_2D JSONQuadFeedbackControl::scaleNodeActions (Json::Value actions)
791 {
792  std::size_t numControllers = actions.size();
793  std::size_t numActions = actions[0].size();
794 
795  array_2D nodeActions(boost::extents[numControllers][numActions]);
796 
797  array_2D limits(boost::extents[2][numActions]);
798 
799  // Check if we need to update limits
800  assert(numActions == 5);
801 
802  limits[0][0] = m_config.lowFreq;
803  limits[1][0] = m_config.highFreq;
804  limits[0][1] = m_config.lowAmp;
805  limits[1][1] = m_config.highAmp;
806  limits[0][2] = m_config.freqFeedbackMin;
807  limits[1][2] = m_config.freqFeedbackMax;
808  limits[0][3] = m_config.ampFeedbackMin;
809  limits[1][3] = m_config.ampFeedbackMax;
810  limits[0][4] = m_config.phaseFeedbackMin;
811  limits[1][4] = m_config.phaseFeedbackMax;
812 
813  Json::Value::iterator nodeIt = actions.begin();
814 
815  // This one is square
816  for( std::size_t i = 0; i < numControllers; i++)
817  {
818  // Recall that high CPG nodes are added last
819  if (i > n_muscSpine + n_muscHip*4 + n_muscLeg*4)
820  {
821  // We use separate parameters for the frequency of the high
822  // CPGs since they control entire body parts, y'know?
823  limits[0][2] = m_config.highFreqFeedbackMin;
824  limits[1][2] = m_config.highFreqFeedbackMax;
825  }
826  Json::Value nodeParam = *nodeIt;
827  for( std::size_t j = 0; j < numActions; j++)
828  {
829  nodeActions[i][j] = ( (nodeParam.get(j, 0.0)).asDouble() *
830  (limits[1][j] - limits[0][j])) + limits[0][j];
831  }
832  nodeIt++;
833  }
834 
835  return nodeActions;
836 }
837 
838 std::vector<double> JSONQuadFeedbackControl::getFeedback(BaseSpineModelLearning& subject)
839 {
840  // Placeholder
841  std::vector<double> feedback;
842 
843  const std::vector<tgSpringCableActuator*>& spineCables = subject.find<tgSpringCableActuator> (strSpineTag);
844  const std::vector<tgSpringCableActuator*>& leftShoulderCables = subject.find<tgSpringCableActuator> (strLeftShoulderTag);
845  const std::vector<tgSpringCableActuator*>& leftHipCables = subject.find<tgSpringCableActuator> (strLeftHipTag);
846  const std::vector<tgSpringCableActuator*>& leftFrontLegCables = subject.find<tgSpringCableActuator> (strLeftFrontLegTag);
847  const std::vector<tgSpringCableActuator*>& leftHindLegCables = subject.find<tgSpringCableActuator> (strLeftHindLegTag);
848  const std::vector<tgSpringCableActuator*>& rightShoulderCables = subject.find<tgSpringCableActuator> (strRightShoulderTag);
849  const std::vector<tgSpringCableActuator*>& rightHipCables = subject.find<tgSpringCableActuator> (strRightHipTag);
850  const std::vector<tgSpringCableActuator*>& rightFrontLegCables = subject.find<tgSpringCableActuator> (strRightFrontLegTag);
851  const std::vector<tgSpringCableActuator*>& rightHindLegCables = subject.find<tgSpringCableActuator> (strRightHindLegTag);
852 
853  const std::vector <tgSpringCableActuator*> allCables[9] = { spineCables, leftShoulderCables, leftFrontLegCables, rightShoulderCables, rightFrontLegCables, leftHipCables, leftHindLegCables, rightHipCables, rightHindLegCables };
854 
855  double *inputs = new double[m_config.numStates];
856 
857  for (size_t b = 0; b < 9; b++)
858  {
859  std::size_t n = allCables[b].size();
860  for(std::size_t i = 0; i != n; i++)
861  {
862  std::vector< std::vector<double> > actions;
863 
864  const tgSpringCableActuator& cable = *(allCables[b][i]);
865  std::vector<double > state = getCableState(cable);
866 
867  // Rescale to 0 to 1 (consider doing this inside getState
868  for (std::size_t i = 0; i < state.size(); i++)
869  {
870  inputs[i]=state[i] / 2.0 + 0.5;
871  }
872 
873  double *output = nn->feedForwardPattern(inputs);
874  vector<double> tmpAct;
875  for(int j=0;j<m_config.numActions;j++)
876  {
877  tmpAct.push_back(output[j]);
878  }
879  actions.push_back(tmpAct);
880 
881  std::vector<double> cableFeedback = transformFeedbackActions(actions);
882 
883  feedback.insert(feedback.end(), cableFeedback.begin(), cableFeedback.end());
884  }
885  }
886 
887  // inputting 0 for now
888  std::size_t n = m_highControllers.size();
889  for (size_t i = 0; i != n; i++)
890  {
891  std::vector< std::vector<double> > actions;
892 
893  // Rescale to 0 to 1 (consider doing this inside getState
894  for (std::size_t i = 0; i < m_config.numStates; i++)
895  {
896  inputs[i] = 0;
897  }
898 
899  double *output = nn->feedForwardPattern(inputs);
900  vector<double> tmpAct;
901  for(int j=0;j<m_config.numActions;j++)
902  {
903  tmpAct.push_back(output[j]);
904  }
905  actions.push_back(tmpAct);
906 
907  std::vector<double> cableFeedback = transformFeedbackActions(actions);
908 
909  feedback.insert(feedback.end(), cableFeedback.begin(), cableFeedback.end());
910  }
911 
912  return feedback;
913 }
914 
915 std::vector<double> JSONQuadFeedbackControl::getCableState(const tgSpringCableActuator& cable)
916 {
917  // For each string, scale value from -1 to 1 based on initial length or max tension of motor
918 
919  std::vector<double> state;
920 
921  // Scale length by starting length
922  const double startLength = cable.getStartLength();
923  state.push_back((cable.getCurrentLength() - startLength) / startLength);
924 
925  const double maxTension = cable.getConfig().maxTens;
926  state.push_back((cable.getTension() - maxTension / 2.0) / maxTension);
927 
928  return state;
929 }
930 
931 std::vector<double> JSONQuadFeedbackControl::transformFeedbackActions(std::vector< std::vector<double> >& actions)
932 {
933  // Placeholder
934  std::vector<double> feedback;
935 
936  // Leave in place for generalization later
937  const std::size_t numControllers = 1;
938  const std::size_t numActions = m_config.numActions;
939 
940  assert( actions.size() == numControllers);
941  assert( actions[0].size() == numActions);
942 
943  // Scale values back to -1 to +1
944  for( std::size_t i = 0; i < numControllers; i++)
945  {
946  for( std::size_t j = 0; j < numActions; j++)
947  {
948  feedback.push_back(actions[i][j] * 2.0 - 1.0);
949  }
950  }
951 
952  return feedback;
953 }
954 
tgImpedanceController.h
Contains the definition of class ImpedanceControl. $Id$.
JSONQuadFeedbackControl::scaleEdgeActions
virtual array_4D scaleEdgeActions(Json::Value edgeParam)
Definition: JSONQuadFeedbackControl.cpp:595
CPGEquations::update
void update(std::vector< double > &descCom, double dt)
Definition: CPGEquations.cpp:231
JSONQuadFeedbackControl::onTeardown
virtual void onTeardown(BaseSpineModelLearning &subject)
Definition: JSONQuadFeedbackControl.cpp:260
tgSpringCableActuator::getTension
virtual const double getTension() const
Definition: tgSpringCableActuator.cpp:172
tgSpringCableActuator::getStartLength
virtual const double getStartLength() const
Definition: tgSpringCableActuator.cpp:162
tgCPGActuatorControl::setConnectivity
void setConnectivity(const std::vector< tgCPGActuatorControl * > &allStrings, array_4D edgeParams)
Definition: tgCPGActuatorControl.cpp:122
tgCPGActuatorControl.h
Definition of the tgCPGStringControl observer class.
BaseSpineModelLearning.h
A template base class for a tensegrity spine.
configuration.h
A class to read a learning configuration from a .ini file.
JSONQuadFeedbackControl::onStep
virtual void onStep(BaseSpineModelLearning &subject, double dt)
Definition: JSONQuadFeedbackControl.cpp:217
tgSpringCableActuator.h
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
FileHelpers.h
A series of functions to assist with file input/output.
JSONQuadFeedbackControl::JSONQuadFeedbackControl
JSONQuadFeedbackControl(JSONQuadFeedbackControl::Config config, std::string args, std::string resourcePath="")
Definition: JSONQuadFeedbackControl.cpp:119
AnnealEvolution.h
Contains the definition of class AnnealEvolution. Adapting NeuroEvolution to do Simulated Annealing...
JSONQuadFeedbackControl::onSetup
virtual void onSetup(BaseSpineModelLearning &subject)
Definition: JSONQuadFeedbackControl.cpp:134
tgBasicActuator.h
Contains the definition of class tgBasicActuator.
JSONQuadFeedbackControl::scaleHighEdgeActions
virtual array_4D scaleHighEdgeActions(Json::Value highEdgeParam)
Definition: JSONQuadFeedbackControl.cpp:706
tgSpringCableActuator::getConfig
const Config & getConfig() const
Definition: tgSpringCableActuator.h:263
tgModel::find
std::vector< T * > find(const tgTagSearch &tagSearch)
Definition: tgModel.h:128
tgDataObserver::onStep
virtual void onStep(tgModel &model, double dt)
Definition: tgDataObserver.cpp:159
CPGEquationsFB.h
Definition of class CPGEquationsFB.
JSONQuadFeedbackControl::Config::Config
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)
Definition: JSONQuadFeedbackControl.cpp:72
tgDataObserver::onSetup
virtual void onSetup(tgModel &model)
Definition: tgDataObserver.cpp:60
tgSpringCableActuator::getCurrentLength
virtual const double getCurrentLength() const
Definition: tgSpringCableActuator.cpp:167
tgSubject< JSONCPGControl >::notifyStep
void notifyStep(double dt)
tgCPGCableControl::assignNodeNumberFB
void assignNodeNumberFB(CPGEquationsFB &CPGSys, array_2D nodeParams)
Definition: tgCPGCableControl.cpp:99