NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
SpineFeedbackControl.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 
27 #include "SpineFeedbackControl.h"
28 
29 #include <string>
30 
31 
32 // Should include tgString, but compiler complains since its been
33 // included from BaseSpineModelLearning. Perhaps we should move things
34 // to a cpp over there
36 #include "core/tgBasicActuator.h"
39 #include "tgCPGCableControl.h"
40 
41 #include "helpers/FileHelpers.h"
42 
45 
46 #include "CPGEquationsFB.h"
47 #include "CPGNodeFB.h"
48 
49 //#define LOGGING
50 #define USE_KINEMATIC
51 
52 using namespace std;
53 
55  int tm,
56  int om,
57  int param,
58  int segnum,
59  double ct,
60  double la,
61  double ha,
62  double lp,
63  double hp,
64  double kt,
65  double kp,
66  double kv,
67  bool def,
68  double cl,
69  double lf,
70  double hf,
71  double ffMin,
72  double ffMax,
73  double afMin,
74  double afMax,
75  double pfMin,
76  double pfMax) :
77 BaseSpineCPGControl::Config::Config(ss, tm, om, param, segnum, ct, la, ha,
78  lp, hp, kt, kp, kv, def, cl, lf, hf),
79 freqFeedbackMin(ffMin),
80 freqFeedbackMax(ffMax),
81 ampFeedbackMin(afMin),
82 ampFeedbackMax(afMax),
83 phaseFeedbackMin(pfMin),
84 phaseFeedbackMax(pfMax)
85 {
86 
87 }
94  std::string args,
95  std::string resourcePath,
96  std::string ec,
97  std::string nc,
98  std::string fc) :
99 BaseSpineCPGControl(config, args, resourcePath, ec, nc),
100 m_config(config),
101 feedbackConfigFilename(fc),
102 // Evolution assumes no pre-processing was done on these names
103 feedbackEvolution(args + "_fb", fc, resourcePath),
104 // Will be overwritten by configuration data
105 feedbackLearning(false)
106 {
107  std::string path;
108  if (resourcePath != "")
109  {
110  path = FileHelpers::getResourcePath(resourcePath);
111  }
112  else
113  {
114  path = "";
115  }
116 
117  feedbackConfigData.readFile(path + feedbackConfigFilename);
118  feedbackLearning = feedbackConfigData.getintvalue("learning");
119 
120 }
121 
123 {
124  m_pCPGSys = new CPGEquationsFB(100);
125  //Initialize the Learning Adapters
126  nodeAdapter.initialize(&nodeEvolution,
127  nodeLearning,
128  nodeConfigData);
129  edgeAdapter.initialize(&edgeEvolution,
130  edgeLearning,
131  edgeConfigData);
132  feedbackAdapter.initialize(&feedbackEvolution,
133  feedbackLearning,
134  feedbackConfigData);
135  /* Empty vector signifying no state information
136  * All parameters are stateless parameters, so we can get away with
137  * only doing this once
138  */
139  std::vector<double> state;
140  double dt = 0;
141 
142  array_4D edgeParams = scaleEdgeActions(edgeAdapter.step(dt, state));
143  array_2D nodeParams = scaleNodeActions(nodeAdapter.step(dt, state));
144 
145  setupCPGs(subject, nodeParams, edgeParams);
146 
147  initConditions = subject.getSegmentCOM(m_config.segmentNumber);
148 #ifdef LOGGING // Conditional compile for data logging
149  m_dataObserver.onSetup(subject);
150 #endif
151 
152 #if (0) // Conditional Compile for debug info
153  std::cout << *m_pCPGSys << std::endl;
154 #endif
155  m_updateTime = 0.0;
156  bogus = false;
157 }
158 
160 {
161  m_updateTime += dt;
162  if (m_updateTime >= m_config.controlTime)
163  {
164 #if (1)
165  std::vector<double> desComs = getFeedback(subject);
166 
167 #else
168  std::size_t numControllers = subject.getNumberofMuslces() * 3;
169 
170  double descendingCommand = 0.0;
171  std::vector<double> desComs (numControllers, descendingCommand);
172 #endif
173  try
174  {
175  m_pCPGSys->update(desComs, m_updateTime);
176  }
177  catch (std::runtime_error& e)
178  {
179  // Stops the trial immediately, lets teardown know it broke
180  bogus = true;
181  throw (e);
182  }
183 
184 #ifdef LOGGING // Conditional compile for data logging
185  m_dataObserver.onStep(subject, m_updateTime);
186 #endif
187  notifyStep(m_updateTime);
188  m_updateTime = 0;
189  }
190 
191  double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];
192 
194  if (currentHeight > 25 || currentHeight < 1.0)
195  {
197  bogus = true;
198  throw std::runtime_error("Height out of range");
199  }
200 }
201 
203 {
204  scores.clear();
205  // @todo - check to make sure we ran for the right amount of time
206 
207  std::vector<double> finalConditions = subject.getSegmentCOM(m_config.segmentNumber);
208 
209  const double newX = finalConditions[0];
210  const double newZ = finalConditions[2];
211  const double oldX = initConditions[0];
212  const double oldZ = initConditions[2];
213 
214  const double distanceMoved = sqrt((newX-oldX) * (newX-oldX) +
215  (newZ-oldZ) * (newZ-oldZ));
216 
217  if (bogus)
218  {
219  scores.push_back(-1.0);
220  }
221  else
222  {
223  scores.push_back(distanceMoved);
224  }
225 
228  double totalEnergySpent=0;
229 
230  std::vector<tgSpringCableActuator* > tmpStrings = subject.getAllMuscles();
231 
232  for(int i=0; i<tmpStrings.size(); i++)
233  {
234  tgSpringCableActuator::SpringCableActuatorHistory stringHist = tmpStrings[i]->getHistory();
235 
236  for(int j=1; j<stringHist.tensionHistory.size(); j++)
237  {
238  const double previousTension = stringHist.tensionHistory[j-1];
239  const double previousLength = stringHist.restLengths[j-1];
240  const double currentLength = stringHist.restLengths[j];
241  //TODO: examine this assumption - free spinning motor may require more power
242  double motorSpeed = (currentLength-previousLength);
243  if(motorSpeed > 0) // Vestigial code
244  motorSpeed = 0;
245  const double workDone = previousTension * motorSpeed;
246  totalEnergySpent += workDone;
247  }
248  }
249 
250  scores.push_back(totalEnergySpent);
251 
252  edgeAdapter.endEpisode(scores);
253  nodeAdapter.endEpisode(scores);
254  feedbackAdapter.endEpisode(scores);
255 
256  delete m_pCPGSys;
257  m_pCPGSys = NULL;
258 
259  for(size_t i = 0; i < m_allControllers.size(); i++)
260  {
261  delete m_allControllers[i];
262  }
263  m_allControllers.clear();
264 }
265 
266 void SpineFeedbackControl::setupCPGs(BaseSpineModelLearning& subject, array_2D nodeActions, array_4D edgeActions)
267 {
268 
269  std::vector <tgSpringCableActuator*> allMuscles = subject.getAllMuscles();
270 
271  CPGEquationsFB& m_CPGFBSys = *(tgCast::cast<CPGEquations, CPGEquationsFB>(m_pCPGSys));
272 
273  for (std::size_t i = 0; i < allMuscles.size(); i++)
274  {
275 
276  tgPIDController::Config config(20000.0, 0.0, 5.0, true); // Non backdrivable
277  tgCPGCableControl* pStringControl = new tgCPGCableControl(config);
278 
279  allMuscles[i]->attach(pStringControl);
280 
281  // First assign node numbers
282  pStringControl->assignNodeNumberFB(m_CPGFBSys, nodeActions);
283 
284  m_allControllers.push_back(pStringControl);
285  }
286 
287  // Then determine connectivity and setup string
288  for (std::size_t i = 0; i < m_allControllers.size(); i++)
289  {
290  tgCPGActuatorControl * const pStringInfo = m_allControllers[i];
291  assert(pStringInfo != NULL);
292  pStringInfo->setConnectivity(m_allControllers, edgeActions);
293 
294  //String will own this pointer
295  tgImpedanceController* p_ipc = new tgImpedanceController( m_config.tension,
296  m_config.kPosition,
297  m_config.kVelocity);
298  if (m_config.useDefault)
299  {
300  pStringInfo->setupControl(*p_ipc);
301  }
302  else
303  {
304  pStringInfo->setupControl(*p_ipc, m_config.controlLength);
305  }
306  }
307 
308 }
309 
310 array_2D SpineFeedbackControl::scaleNodeActions
311  (vector< vector <double> > actions)
312 {
313  std::size_t numControllers = nodeConfigData.getintvalue("numberOfControllers");
314  std::size_t numActions = nodeConfigData.getintvalue("numberOfActions");
315 
316  assert( actions.size() == numControllers);
317  assert( actions[0].size() == numActions);
318 
319  array_2D nodeActions(boost::extents[numControllers][numActions]);
320 
321  array_2D limits(boost::extents[2][numActions]);
322 
323  // Check if we need to update limits
324  assert(numActions == 5);
325 
326  limits[0][0] = m_config.lowFreq;
327  limits[1][0] = m_config.highFreq;
328  limits[0][1] = m_config.lowAmp;
329  limits[1][1] = m_config.highAmp;
330  limits[0][2] = m_config.freqFeedbackMin;
331  limits[1][2] = m_config.freqFeedbackMax;
332  limits[0][3] = m_config.ampFeedbackMin;
333  limits[1][3] = m_config.ampFeedbackMax;
334  limits[0][4] = m_config.phaseFeedbackMin;
335  limits[1][4] = m_config.phaseFeedbackMax;
336 
337  // This one is square
338  for( std::size_t i = 0; i < numControllers; i++)
339  {
340  for( std::size_t j = 0; j < numActions; j++)
341  {
342  nodeActions[i][j] = ( actions[i][j] *
343  (limits[1][j] - limits[0][j])) + limits[0][j];
344  }
345  }
346 
347  return nodeActions;
348 }
349 
350 std::vector<double> SpineFeedbackControl::getFeedback(BaseSpineModelLearning& subject)
351 {
352  // Placeholder
353  std:vector<double> feedback;
354  // Adapter doesn't use this anyway, so just do zero here for now (will trigger errors if it starts to use it =) )
355  const double dt = 0;
356 
357  const std::vector<tgSpringCableActuator*>& allCables = subject.getAllMuscles();
358 
359  std::size_t n = allCables.size();
360  for(std::size_t i = 0; i != n; i++)
361  {
362  const tgSpringCableActuator& cable = *(allCables[i]);
363  std::vector<double > state = getCableState(cable);
364  std::vector< std::vector<double> > actions = feedbackAdapter.step(m_updateTime, state);
365  std::vector<double> cableFeedback = transformFeedbackActions(actions);
366 
367  feedback.insert(feedback.end(), cableFeedback.begin(), cableFeedback.end());
368  }
369 
370 
371  return feedback;
372 }
373 
374 std::vector<double> SpineFeedbackControl::getCableState(const tgSpringCableActuator& cable)
375 {
376  // For each string, scale value from -1 to 1 based on initial length or max tension of motor
377 
378  std::vector<double> state;
379 
380  // Scale length by starting length
381  const double startLength = cable.getStartLength();
382  state.push_back((cable.getCurrentLength() - startLength) / startLength);
383 
384  const double maxTension = cable.getConfig().maxTens;
385  state.push_back((cable.getTension() - maxTension / 2.0) / maxTension);
386 
387  return state;
388 }
389 
390 std::vector<double> SpineFeedbackControl::transformFeedbackActions(std::vector< std::vector<double> >& actions)
391 {
392  // Placeholder
393  std:vector<double> feedback;
394 
395  std::size_t numControllers = feedbackConfigData.getintvalue("numberOfControllers");
396  std::size_t numActions = feedbackConfigData.getintvalue("numberOfActions");
397 
398  assert( actions.size() == numControllers);
399  assert( actions[0].size() == numActions);
400 
401  // Scale values back to -1 to +1
402  for( std::size_t i = 0; i < numControllers; i++)
403  {
404  for( std::size_t j = 0; j < numActions; j++)
405  {
406  feedback.push_back(actions[i][j] * 2.0 - 1.0);
407  }
408  }
409 
410  return feedback;
411 }
Contains the definition of class ImpedanceControl. $Id$.
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)
void initialize(NeuroEvolution *evo, bool isLearning, configuration config)
A controller for the template class BaseSpineModelLearning.
void update(std::vector< double > &descCom, double dt)
virtual const double getTension() const
virtual const double getStartLength() const
virtual void onStep(BaseSpineModelLearning &subject, double dt)
SpineFeedbackControl(SpineFeedbackControl::Config config, std::string args, std::string resourcePath="", std::string ec="edgeConfig.ini", std::string nc="nodeConfig.ini", std::string fc="feedbackConfig.ini")
void setConnectivity(const std::vector< tgCPGActuatorControl * > &allStrings, array_4D edgeParams)
Definition of the tgCPGStringControl observer class.
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.
Definition of class CPGNodeFB.
Contains the definition of class AnnealEvolution. Adapting NeuroEvolution to do Simulated Annealing...
Contains the definition of class tgBasicActuator.
const Config & getConfig() const
virtual array_4D scaleEdgeActions(std::vector< std::vector< double > > actions)
virtual void onStep(tgModel &model, double dt)
Definition of class CPGEquationsFB.
void initialize(AnnealEvolution *evo, bool isLearning, configuration config)
virtual void onSetup(tgModel &model)
virtual void onSetup(BaseSpineModelLearning &subject)
virtual const double getCurrentLength() const
void assignNodeNumberFB(CPGEquationsFB &CPGSys, array_2D nodeParams)
virtual void onTeardown(BaseSpineModelLearning &subject)