NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
OctahedralGoalControl.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 "OctahedralGoalControl.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
34 #include "core/tgBasicActuator.h"
37 #include "examples/learningSpines/tgCPGCableControl.h"
38 #include "examples/learningSpines/tgSCASineControl.h"
39 
42 #include "helpers/FileHelpers.h"
43 
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 
68  std::string args,
69  std::string resourcePath) :
70 JSONGoalControl(config, args, resourcePath)
71 {
72  // Path and filename handled by base class
73 
74 }
75 
76 OctahedralGoalControl::~OctahedralGoalControl()
77 {
78 }
79 
81 {
82  m_pCPGSys = new CPGEquationsFB(200);
83 
84  Json::Value root; // will contains the root value after parsing.
85  Json::Reader reader;
86 
87  bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
88  if ( !parsingSuccessful )
89  {
90  // report to the user the failure and their locations in the document.
91  std::cout << "Failed to parse configuration\n"
92  << reader.getFormattedErrorMessages();
93  throw std::invalid_argument("Bad filename for JSON");
94  }
95  // Get the value of the member of root named 'encoding', return 'UTF-8' if there is no
96  // such member.
97  Json::Value nodeVals = root.get("nodeVals", "UTF-8");
98  Json::Value edgeVals = root.get("edgeVals", "UTF-8");
99 
100  nodeVals = nodeVals.get("params", "UTF-8");
101  edgeVals = edgeVals.get("params", "UTF-8");
102 
103  array_4D edgeParams = scaleEdgeActions(edgeVals);
104  array_2D nodeParams = scaleNodeActions(nodeVals);
105 
106  setupCPGs(subject, nodeParams, edgeParams);
107 
108  Json::Value feedbackParams = root.get("feedbackVals", "UTF-8");
109  feedbackParams = feedbackParams.get("params", "UTF-8");
110 
111  // Setup neural network
112  m_config.numStates = feedbackParams.get("numStates", "UTF-8").asInt();
113  m_config.numActions = feedbackParams.get("numActions", "UTF-8").asInt();
114  m_config.numHidden = feedbackParams.get("numHidden", "UTF-8").asInt();
115 
116  std::string nnFile = controlFilePath + feedbackParams.get("neuralFilename", "UTF-8").asString();
117 
118  nn = new neuralNetwork(m_config.numStates, m_config.numHidden, m_config.numActions);
119 
120  nn->loadWeights(nnFile.c_str());
121 
122  const OctahedralComplex* ocSubject = tgCast::cast<BaseSpineModelLearning, OctahedralComplex>(subject);
123  setupSaddleControllers(ocSubject);
124 
125  initConditions = subject.getSegmentCOM(m_config.segmentNumber);
126 #ifdef LOGGING // Conditional compile for data logging
127  m_dataObserver.onSetup(subject);
128 #endif
129 
130 #if (0) // Conditional Compile for debug info
131  std::cout << *m_pCPGSys << std::endl;
132 #endif
133  m_updateTime = 0.0;
134  bogus = false;
135 }
136 
138 {
139  m_updateTime += dt;
140  if (m_updateTime >= m_config.controlTime)
141  {
142 #if (1)
143  #if (0)
144  std::vector<double> desComs = getFeedback(subject);
145  #else
146  const BaseSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, BaseSpineModelGoal>(subject);
147  std::vector<double> desComs = getGoalFeedback(goalSubject);
148  #endif // Terrain feedback vs goal feedback
149 #else
150  #if (1)
151  const BaseSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, BaseSpineModelGoal>(subject);
152  setGoalTensions(goalSubject);
153  #endif
154  std::size_t numControllers = subject.getNumberofMuslces() * 3;
155 
156  double descendingCommand = 0.0;
157  std::vector<double> desComs (numControllers, descendingCommand);
158 #endif
159  try
160  {
161  m_pCPGSys->update(desComs, m_updateTime);
162  }
163  catch (std::runtime_error& e)
164  {
165  // Stops the trial immediately, lets teardown know it broke
166  bogus = true;
167  throw (e);
168  }
169 
170 #ifdef LOGGING // Conditional compile for data logging
171  m_dataObserver.onStep(subject, m_updateTime);
172 #endif
173  notifyStep(m_updateTime);
174  m_updateTime = 0;
175  }
176 
177  double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];
178 
180  if (currentHeight > 25 || currentHeight < 1.0)
181  {
183  bogus = true;
184  throw std::runtime_error("Height out of range");
185  }
186 }
187 
189 {
190  scores.clear();
191  // @todo - check to make sure we ran for the right amount of time
192 
193  std::vector<double> finalConditions = subject.getSegmentCOM(m_config.segmentNumber);
194 
195  const double newX = finalConditions[0];
196  const double newZ = finalConditions[2];
197  const double oldX = initConditions[0];
198  const double oldZ = initConditions[2];
199 
200  const BaseSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, BaseSpineModelGoal>(subject);
201 
202  const double distanceMoved = calculateDistanceMoved(goalSubject);
203 
204  if (bogus)
205  {
206  scores.push_back(-1.0);
207  }
208  else
209  {
210  scores.push_back(distanceMoved);
211  }
212 
215  double totalEnergySpent=0;
216 
217  std::vector<tgSpringCableActuator* > tmpStrings = subject.getAllMuscles();
218 
219  for(std::size_t i=0; i<tmpStrings.size(); i++)
220  {
221  tgSpringCableActuator::SpringCableActuatorHistory stringHist = tmpStrings[i]->getHistory();
222 
223  for(std::size_t j=1; j<stringHist.tensionHistory.size(); j++)
224  {
225  const double previousTension = stringHist.tensionHistory[j-1];
226  const double previousLength = stringHist.restLengths[j-1];
227  const double currentLength = stringHist.restLengths[j];
228  //TODO: examine this assumption - free spinning motor may require more power
229  double motorSpeed = (currentLength-previousLength);
230  if(motorSpeed > 0) // Vestigial code
231  motorSpeed = 0;
232  const double workDone = previousTension * motorSpeed;
233  totalEnergySpent += workDone;
234  }
235  }
236 
237  scores.push_back(totalEnergySpent);
238 
239  std::cout << "Dist travelled " << scores[0] << std::endl;
240 
241  Json::Value root; // will contains the root value after parsing.
242  Json::Reader reader;
243 
244  bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
245  if ( !parsingSuccessful )
246  {
247  // report to the user the failure and their locations in the document.
248  std::cout << "Failed to parse configuration\n"
249  << reader.getFormattedErrorMessages();
250  throw std::invalid_argument("Bad filename for JSON");
251  }
252 
253  Json::Value prevScores = root.get("scores", Json::nullValue);
254 
255  Json::Value subScores;
256  subScores["distance"] = scores[0];
257  subScores["energy"] = totalEnergySpent;
258 
259  prevScores.append(subScores);
260  root["scores"] = prevScores;
261 
262  ofstream payloadLog;
263  payloadLog.open(controlFilename.c_str(),ofstream::out);
264 
265  payloadLog << root << std::endl;
266 
267  delete m_pCPGSys;
268  m_pCPGSys = NULL;
269 
270  for(size_t i = 0; i < m_allControllers.size(); i++)
271  {
272  delete m_allControllers[i];
273  }
274  m_allControllers.clear();
275 
276  for(size_t i = 0; i < m_allControllers.size(); i++)
277  {
278  delete m_saddleControllers[i];
279  }
280  m_saddleControllers.clear();
281 }
282 
283 void OctahedralGoalControl::setupSaddleControllers(const OctahedralComplex* subject)
284 {
285 
286  const std::vector <tgSpringCableActuator*> allSaddleMuscles = subject->getSaddleMuscles();
287 
288  for (std::size_t i = 0; i < allSaddleMuscles.size(); i++)
289  {
290 
291  tgPIDController::Config config(20000.0, 0.0, 5.0, true); // Non backdrivable
292  tgImpedanceController* p_ipc = new tgImpedanceController( m_config.tensFeedback,
293  m_config.kPosition,
294  0.0);
295 
296  // Impedance control only, no sine waves
297  tgSCASineControl* pStringControl = new tgSCASineControl(m_config.controlTime,
298  p_ipc,
299  config,
300  0.0,
301  0.0,
302  0.0,
303  0.0,
304  allSaddleMuscles[i]->getCurrentLength());
305 
306  allSaddleMuscles[i]->attach(pStringControl);
307 
308  m_saddleControllers.push_back(pStringControl);
309  }
310 
311 }
312 
313 
314 void OctahedralGoalControl::setGoalTensions(const BaseSpineModelGoal* subject)
315 {
316  // Get heading and generate feedback vector
317  std::vector<double> currentPosition = subject->getSegmentCOM(m_config.segmentNumber);
318 
319  assert(currentPosition.size() == 3);
320 
321  btVector3 currentPosVector(currentPosition[0], currentPosition[1], currentPosition[2]);
322 
323  btVector3 goalPosition = subject->goalBoxPosition();
324 
325  btVector3 desiredHeading = (goalPosition - currentPosVector).normalize();
326 
327  std::vector<double> state;
328  state.push_back(desiredHeading.getX());
329  state.push_back(desiredHeading.getZ());
330 
331  assert(state[0] >= -1.0 && state[0] <= 1.0);
332  assert(state[1] >= -1.0 && state[1] <= 1.0);
333 
334  double *inputs = new double[m_config.numStates];
335 
336  // Rescale to 0 to 1 (consider doing this inside getState
337  for (std::size_t i = 0; i < state.size(); i++)
338  {
339  inputs[i]=state[i] / 2.0 + 0.5;
340  }
341 
342  const int nSeg = subject->getSegments() - 1;
343 
344  double *output = nn->feedForwardPattern(inputs);
345 
346  vector<double> actions;
347  for(int j=0;j<m_config.numActions;j++)
348  {
349  actions.push_back(output[j]);
350  }
351 
352  transformFeedbackActions(actions);
353 
354  assert(m_config.numActions == m_saddleControllers.size() + m_allControllers.size());
355  assert(m_saddleControllers.size() == m_allControllers.size());
356 
357  const OctahedralComplex* octaSubject = tgCast::cast<BaseSpineModelLearning, OctahedralComplex>(subject);
358  const std::vector <tgSpringCableActuator*> allSaddleMuscles = octaSubject->getSaddleMuscles();
359  const std::vector <tgSpringCableActuator*> allCPGMuscles = octaSubject->getAllMuscles();
360 
361 
362  for (int j = 0; j < m_saddleControllers.size(); j++)
363  {
364  double startLength = allSaddleMuscles[j]->getStartLength();
365  m_saddleControllers[j]->updateControlLength(actions[j] * startLength + startLength);
366  }
367 
368  for (int i = 0; i < m_allControllers.size(); i++)
369  {
370  double startLength = allCPGMuscles[i]->getStartLength();
371 
372  tgCPGCableControl* mCPGController = tgCast::cast<tgCPGActuatorControl, tgCPGCableControl>(m_allControllers[i]);
373  mCPGController->updateControlLength(actions[i] *startLength + startLength);
374  }
375 }
Contains the definition of class ImpedanceControl. $Id$.
virtual void onTeardown(BaseSpineModelLearning &subject)
void update(std::vector< double > &descCom, double dt)
Implementing the cross-linked octahedral complex spine inspired by Tom Flemons.
OctahedralGoalControl(JSONGoalControl::Config config, std::string args, std::string resourcePath="")
virtual array_4D scaleEdgeActions(Json::Value edgeParam)
virtual void onSetup(BaseSpineModelLearning &subject)
Definition of the tgCPGStringControl observer class.
virtual void onStep(BaseSpineModelLearning &subject, double dt)
Implementing the tetrahedral complex spine inspired by Tom Flemons.
neuralNetwork * nn
A controller for the template class BaseSpineModelLearning.
A template base class for a tensegrity spine.
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
std::vector< double > getGoalFeedback(const BaseSpineModelGoal *subject)
A series of functions to assist with file input/output.
void updateControlLength(double newControlLength)
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)