NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
OctahedralTensionControl.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 
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 JSONGoalTension(config, args, resourcePath)
71 {
72  // Path and filename handled by base class
73 
74 }
75 
76 OctahedralTensionControl::~OctahedralTensionControl()
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  Json::Value goalParams = root.get("goalVals", "UTF-8");
117  goalParams = goalParams.get("params", "UTF-8");
118 
119  // Setup neural network
120  m_config.goalStates = goalParams.get("numStates", "UTF-8").asInt();
121  m_config.goalActions = goalParams.get("numActions", "UTF-8").asInt();
122  m_config.goalHidden = goalParams.get("numHidden", "UTF-8").asInt();
123 
124  std::string nnFile_goal = controlFilePath + goalParams.get("neuralFilename", "UTF-8").asString();
125 
126  nn_goal = new neuralNetwork(m_config.goalStates, m_config.goalHidden, m_config.goalActions);
127 
128  nn_goal->loadWeights(nnFile_goal.c_str());
129 
130  const OctahedralComplex* ocSubject = tgCast::cast<BaseSpineModelLearning, OctahedralComplex>(subject);
131  setupSaddleControllers(ocSubject);
132 
133  initConditions = subject.getSegmentCOM(m_config.segmentNumber);
134 #ifdef LOGGING // Conditional compile for data logging
135  m_dataObserver.onSetup(subject);
136 #endif
137 
138 #if (0) // Conditional Compile for debug info
139  std::cout << *m_pCPGSys << std::endl;
140 #endif
141  m_updateTime = 0.0;
142  bogus = false;
143 }
144 
146 {
147  m_updateTime += dt;
148  if (m_updateTime >= m_config.controlTime)
149  {
150 #if (0)
151  std::vector<double> desComs = getFeedback(subject);
152 
153  const BaseSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, BaseSpineModelGoal>(subject);
154 
155  getGoalFeedback(goalSubject);
156 #else
157 
158  std::size_t numControllers = subject.getNumberofMuslces() * 3;
159 
160  double descendingCommand = 0.0;
161  std::vector<double> desComs (numControllers, descendingCommand);
162 #endif
163  try
164  {
165  m_pCPGSys->update(desComs, m_updateTime);
166  }
167  catch (std::runtime_error& e)
168  {
169  // Stops the trial immediately, lets teardown know it broke
170  bogus = true;
171  throw (e);
172  }
173 
174 #ifdef LOGGING // Conditional compile for data logging
175  m_dataObserver.onStep(subject, m_updateTime);
176 #endif
177  notifyStep(m_updateTime);
178  m_updateTime = 0;
179  }
180 
181  double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];
182 
184  if (currentHeight > 25 || currentHeight < 1.0)
185  {
187  bogus = true;
188  throw std::runtime_error("Height out of range");
189  }
190 }
191 
193 {
194  scores.clear();
195  // @todo - check to make sure we ran for the right amount of time
196 
197  std::vector<double> finalConditions = subject.getSegmentCOM(m_config.segmentNumber);
198 
199  const double newX = finalConditions[0];
200  const double newZ = finalConditions[2];
201  const double oldX = initConditions[0];
202  const double oldZ = initConditions[2];
203 
204  const BaseSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, BaseSpineModelGoal>(subject);
205 
206  const double distanceMoved = calculateDistanceMoved(goalSubject);
207 
208  if (bogus)
209  {
210  scores.push_back(-1.0);
211  }
212  else
213  {
214  scores.push_back(distanceMoved);
215  }
216 
219  double totalEnergySpent=0;
220 
221  std::vector<tgSpringCableActuator* > tmpStrings = subject.getAllMuscles();
222 
223  for(std::size_t i=0; i<tmpStrings.size(); i++)
224  {
225  tgSpringCableActuator::SpringCableActuatorHistory stringHist = tmpStrings[i]->getHistory();
226 
227  for(std::size_t j=1; j<stringHist.tensionHistory.size(); j++)
228  {
229  const double previousTension = stringHist.tensionHistory[j-1];
230  const double previousLength = stringHist.restLengths[j-1];
231  const double currentLength = stringHist.restLengths[j];
232  //TODO: examine this assumption - free spinning motor may require more power
233  double motorSpeed = (currentLength-previousLength);
234  if(motorSpeed > 0) // Vestigial code
235  motorSpeed = 0;
236  const double workDone = previousTension * motorSpeed;
237  totalEnergySpent += workDone;
238  }
239  }
240 
241  scores.push_back(totalEnergySpent);
242 
243  std::cout << "Dist travelled " << scores[0] << std::endl;
244 
245  Json::Value root; // will contains the root value after parsing.
246  Json::Reader reader;
247 
248  bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
249  if ( !parsingSuccessful )
250  {
251  // report to the user the failure and their locations in the document.
252  std::cout << "Failed to parse configuration\n"
253  << reader.getFormattedErrorMessages();
254  throw std::invalid_argument("Bad filename for JSON");
255  }
256 
257  Json::Value prevScores = root.get("scores", Json::nullValue);
258 
259  Json::Value subScores;
260  subScores["distance"] = scores[0];
261  subScores["energy"] = totalEnergySpent;
262 
263  prevScores.append(subScores);
264  root["scores"] = prevScores;
265 
266  ofstream payloadLog;
267  payloadLog.open(controlFilename.c_str(),ofstream::out);
268 
269  payloadLog << root << std::endl;
270 
271  delete m_pCPGSys;
272  m_pCPGSys = NULL;
273 
274  for(size_t i = 0; i < m_allControllers.size(); i++)
275  {
276  delete m_allControllers[i];
277  }
278  m_allControllers.clear();
279 
280  for(size_t i = 0; i < m_allControllers.size(); i++)
281  {
282  delete m_saddleControllers[i];
283  }
284  m_saddleControllers.clear();
285 
286  delete nn_goal;
287 }
288 
289 void OctahedralTensionControl::setupSaddleControllers(const OctahedralComplex* subject)
290 {
291 
292  const std::vector <tgSpringCableActuator*> allSaddleMuscles = subject->getSaddleMuscles();
293 
294  for (std::size_t i = 0; i < allSaddleMuscles.size(); i++)
295  {
296 
297  tgPIDController::Config config(20000.0, 0.0, 5.0, true); // Non backdrivable
298  tgImpedanceController* p_ipc = new tgImpedanceController( m_config.tensFeedback,
299  m_config.kPosition,
300  0.0);
301 
302  // Impedance control only, no sine waves
303  tgSCASineControl* pStringControl = new tgSCASineControl(m_config.controlTime,
304  p_ipc,
305  config,
306  0.0,
307  0.0,
308  0.0,
309  0.0,
310  allSaddleMuscles[i]->getCurrentLength());
311 
312  allSaddleMuscles[i]->attach(pStringControl);
313 
314  m_saddleControllers.push_back(pStringControl);
315  }
316 
317 }
318 
319 
321 {
322  const int nSeg = subject->getSegments() - 1;
323 
324  // Only one segment has no actuators, and means we can't get heading.
325  assert(nSeg > 0);
326 
327  // Get heading and generate feedback vector
328  const btVector3 currentPosVector = subject->getSegmentCOMVector(m_config.segmentNumber);
329 
330  const btVector3 goalPosition = subject->goalBoxPosition();
331 
332  btVector3 desiredHeading = (goalPosition - currentPosVector).normalize();
333 
334  // Get current orientation
336  const btVector3 firstSegment = subject->getSegmentCOMVector(0);
337  const btVector3 secondSegment = subject->getSegmentCOMVector(1);
338 
339  btVector3 currentHeading = (firstSegment - secondSegment).normalize();
340 
341  std::vector<double> state;
342  state.push_back(desiredHeading.getX());
343  state.push_back(desiredHeading.getZ());
344  state.push_back(currentHeading.getX());
345  state.push_back(currentHeading.getZ());
346 
347  double *inputs = new double[m_config.goalStates];
348  for (std::size_t i = 0; i < state.size(); i++)
349  {
350  assert(state[i] >= -1.0 && state[i] <= 1.0);
351  // Don't scale! Sigmoid can handle this range
352  inputs[i]=state[i];
353  }
354 
355  double *output = nn_goal->feedForwardPattern(inputs);
356 
357  vector<double> actions;
358 
359 #if (0)
360  for(int j=0;j<m_config.goalActions;j++)
361  {
362  std::cout << output[j] << " ";
363  }
364  std::cout << std::endl;
365 #endif
366 
367  for(int j=0;j<m_config.goalActions;j++)
368  {
369  actions.push_back(output[j]);
370  }
371 
372  transformFeedbackActions(actions);
373 
374  assert(m_config.goalActions * nSeg == m_allControllers.size() + m_saddleControllers.size());
375 
376  assert(m_config.goalActions %2 == 0);
377 
378  // Duplicate the actions across segments
379  for (int i = 0; i != nSeg; i++)
380  {
381  for(int j=0;j<m_config.goalActions;j++)
382  {
383  if (j < m_config.goalActions / 2)
384  {
385  m_allControllers[i * m_config.goalActions / 2 + j]->updateTensionSetpoint(actions[j] * m_config.tensFeedback + m_config.tensFeedback);
386  }
387  else
388  {
389  m_saddleControllers[i * m_config.goalActions /2 + j - m_config.goalActions / 2]->updateTensionSetpoint(actions[j] * m_config.tensFeedback + m_config.tensFeedback);
390  }
391  }
392  }
393 
394 
395 
396  return actions;
397 }
Contains the definition of class ImpedanceControl. $Id$.
void update(std::vector< double > &descCom, double dt)
virtual std::vector< double > getGoalFeedback(const BaseSpineModelGoal *subject)
Implementing the cross-linked octahedral complex spine inspired by Tom Flemons.
virtual void onStep(BaseSpineModelLearning &subject, double dt)
A controller for the template class BaseSpineModelLearning.
virtual array_4D scaleEdgeActions(Json::Value edgeParam)
Definition of the tgCPGStringControl observer class.
Implementing the tetrahedral complex spine inspired by Tom Flemons.
A template base class for a tensegrity spine.
virtual void onTeardown(BaseSpineModelLearning &subject)
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 onSetup(BaseSpineModelLearning &subject)
virtual void onStep(tgModel &model, double dt)
Definition of class CPGEquationsFB.
OctahedralTensionControl(JSONGoalControl::Config config, std::string args, std::string resourcePath="")
virtual void onSetup(tgModel &model)
void notifyStep(double dt)