NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
BaseSpineCPGControl.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 "BaseSpineCPGControl.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
37 #include "tgCPGActuatorControl.h"
38 
39 #include "helpers/FileHelpers.h"
40 
43 
44 #include "util/CPGEquations.h"
45 #include "util/CPGNode.h"
46 
47 //#define LOGGING
48 
49 using namespace std;
50 
52  int tm,
53  int om,
54  int param,
55  int segnum,
56  double ct,
57  double la,
58  double ha,
59  double lp,
60  double hp,
61  double kt,
62  double kp,
63  double kv,
64  bool def,
65  double cl,
66  double lf,
67  double hf) :
68  segmentSpan(ss),
69  theirMuscles(tm),
70  ourMuscles(om),
71  params(param),
72  segmentNumber(segnum),
73  controlTime(ct),
74  lowAmp(la),
75  highAmp(ha),
76  lowPhase(lp),
77  highPhase(hp),
78  tension(kt),
79  kPosition(kp),
80  kVelocity(kv),
81  useDefault(def),
82  controlLength(cl),
83  lowFreq(lf),
84  highFreq(hf)
85 {
86  if (ss <= 0)
87  {
88  throw std::invalid_argument("segmentSpan parameter is negative.");
89  }
90  else if (tm <= 0)
91  {
92  throw std::invalid_argument("theirMuscles parameter is negative.");
93  }
94  else if (om <= 0)
95  {
96  throw std::invalid_argument("Our Muscles parameter is negative.");
97  }
98  else if (param <= 0)
99  {
100  throw std::invalid_argument("Edge parameters is negative.");
101  }
102  else if (segnum < 0)
103  {
104  throw std::invalid_argument("Segment number is negative.");
105  }
106  else if (ct < 0.0)
107  {
108  throw std::invalid_argument("control time is negative.");
109  }
110  else if (kt < 0.0)
111  {
112  throw std::invalid_argument("impedance control tension is negative.");
113  }
114  else if (kp < 0.0)
115  {
116  throw std::invalid_argument("impedance control position is negative.");
117  }
118  else if (kv < 0.0)
119  {
120  throw std::invalid_argument("impedance control velocity is negative.");
121  }
122  else if (cl < 0.0)
123  {
124  throw std::invalid_argument("Control Length is negative.");
125  }
126 }
127 
134  std::string args,
135  std::string resourcePath,
136  std::string ec,
137  std::string nc) :
138 m_config(config),
139 edgeConfigFilename(ec),
140 nodeConfigFilename(nc),
141 // Evolution assumes no pre-processing was done on these names
142 edgeEvolution(args + "_edge", ec, resourcePath),
143 // Can't have identical args or they'll overwrite each other
144 nodeEvolution(args + "_node", nc, resourcePath),
145 // Will be overwritten by configuration data
146 nodeLearning(false),
147 edgeLearning(false),
148 m_dataObserver("logs/TCData"),
149 m_pCPGSys(NULL),
150 m_updateTime(0.0),
151 bogus(false)
152 {
153  std::string path;
154  if (resourcePath != "")
155  {
156  path = FileHelpers::getResourcePath(resourcePath);
157  }
158  else
159  {
160  path = "";
161  }
162 
163  nodeConfigData.readFile(path + nodeConfigFilename);
164  edgeConfigData.readFile(path + edgeConfigFilename);
165  nodeLearning = nodeConfigData.getintvalue("learning");
166  edgeLearning = edgeConfigData.getintvalue("learning");
167 
168 }
169 
170 BaseSpineCPGControl::~BaseSpineCPGControl()
171 {
172  scores.clear();
173 }
174 
176 {
177  // Maximum number of sub-steps allowed by CPG
178  m_pCPGSys = new CPGEquations(200);
179  //Initialize the Learning Adapters
180  nodeAdapter.initialize(&nodeEvolution,
181  nodeLearning,
182  nodeConfigData);
183  edgeAdapter.initialize(&edgeEvolution,
184  edgeLearning,
185  edgeConfigData);
186  /* Empty vector signifying no state information
187  * All parameters are stateless parameters, so we can get away with
188  * only doing this once
189  */
190  std::vector<double> state;
191  double dt = 0;
192 
193  array_4D edgeParams = scaleEdgeActions(edgeAdapter.step(dt, state));
194  array_2D nodeParams = scaleNodeActions(nodeAdapter.step(dt, state));
195 
196  setupCPGs(subject, nodeParams, edgeParams);
197 
198  initConditions = subject.getSegmentCOM(m_config.segmentNumber);
199 #ifdef LOGGING // Conditional compile for data logging
200  m_dataObserver.onSetup(subject);
201 #endif
202 
203 #if (0) // Conditional Compile for debug info
204  std::cout << *m_pCPGSys << std::endl;
205 #endif
206  m_updateTime = 0.0;
207  bogus = false;
208 }
209 
210 void BaseSpineCPGControl::setupCPGs(BaseSpineModelLearning& subject, array_2D nodeActions, array_4D edgeActions)
211 {
212 
213  std::vector <tgSpringCableActuator*> allMuscles = subject.getAllMuscles();
214 
215  for (std::size_t i = 0; i < allMuscles.size(); i++)
216  {
217  tgCPGActuatorControl* pStringControl = new tgCPGActuatorControl();
218  allMuscles[i]->attach(pStringControl);
219 
220  m_allControllers.push_back(pStringControl);
221  }
222 
224  // First assign node numbers to the info Classes
225  for (std::size_t i = 0; i < m_allControllers.size(); i++)
226  {
227  m_allControllers[i]->assignNodeNumber(*m_pCPGSys, nodeActions);
228  }
229 
230  // Then determine connectivity and setup string
231  for (std::size_t i = 0; i < m_allControllers.size(); i++)
232  {
233  tgCPGActuatorControl * const pStringInfo = m_allControllers[i];
234  assert(pStringInfo != NULL);
235  pStringInfo->setConnectivity(m_allControllers, edgeActions);
236 
237  //String will own this pointer
238  tgImpedanceController* p_ipc = new tgImpedanceController( m_config.tension,
239  m_config.kPosition,
240  m_config.kVelocity);
241  if (m_config.useDefault)
242  {
243  pStringInfo->setupControl(*p_ipc);
244  }
245  else
246  {
247  pStringInfo->setupControl(*p_ipc, m_config.controlLength);
248  }
249  }
250 
251 }
252 
254 {
255  m_updateTime += dt;
256  if (m_updateTime >= m_config.controlTime)
257  {
258  std::size_t numControllers = subject.getNumberofMuslces();
259 
260  double descendingCommand = 2.0;
261  std::vector<double> desComs (numControllers, descendingCommand);
262 
263  m_pCPGSys->update(desComs, m_updateTime);
264 #ifdef LOGGING // Conditional compile for data logging
265  m_dataObserver.onStep(subject, m_updateTime);
266 #endif
267  notifyStep(m_updateTime);
268  m_updateTime = 0;
269  }
270 
271  double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];
272 
274  if (currentHeight > 25 || currentHeight < 1.0)
275  {
277  bogus = true;
278  }
279 }
280 
282 {
283  scores.clear();
284  // @todo - check to make sure we ran for the right amount of time
285 
286  std::vector<double> finalConditions = subject.getSegmentCOM(m_config.segmentNumber);
287 
288  const double newX = finalConditions[0];
289  const double newZ = finalConditions[2];
290  const double oldX = initConditions[0];
291  const double oldZ = initConditions[2];
292 
293  const double distanceMoved = sqrt((newX-oldX) * (newX-oldX) +
294  (newZ-oldZ) * (newZ-oldZ));
295 
296  if (bogus)
297  {
298  scores.push_back(-1.0);
299  }
300  else
301  {
302  scores.push_back(distanceMoved);
303  }
304 
307  double totalEnergySpent=0;
308 
309  std::vector<tgSpringCableActuator* > tmpStrings = subject.getAllMuscles();
310 
311  for(int i=0; i<tmpStrings.size(); i++)
312  {
313  tgSpringCableActuator::SpringCableActuatorHistory stringHist = tmpStrings[i]->getHistory();
314 
315  for(int j=1; j<stringHist.tensionHistory.size(); j++)
316  {
317  const double previousTension = stringHist.tensionHistory[j-1];
318  const double previousLength = stringHist.restLengths[j-1];
319  const double currentLength = stringHist.restLengths[j];
320  //TODO: examine this assumption - free spinning motor may require more power
321  double motorSpeed = (currentLength-previousLength);
322  if(motorSpeed > 0) // Vestigial code
323  motorSpeed = 0;
324  const double workDone = previousTension * motorSpeed;
325  totalEnergySpent += workDone;
326  }
327  }
328 
329  scores.push_back(totalEnergySpent);
330 
331  edgeAdapter.endEpisode(scores);
332  nodeAdapter.endEpisode(scores);
333 
334  delete m_pCPGSys;
335  m_pCPGSys = NULL;
336 
337  for(size_t i = 0; i < m_allControllers.size(); i++)
338  {
339  delete m_allControllers[i];
340  }
341  m_allControllers.clear();
342 }
343 
344 const double BaseSpineCPGControl::getCPGValue(std::size_t i) const
345 {
346  // Error handling on input done in CPG_Equations
347  return (*m_pCPGSys)[i];
348 }
349 
350 double BaseSpineCPGControl::getScore() const
351 {
352  if (scores.size() == 2)
353  {
354  return scores[0];
355  }
356  else
357  {
358  throw std::runtime_error("Called before scores were obtained!");
359  }
360 }
361 
362 
364  (vector< vector <double> > actions)
365 {
366  std::size_t numControllers = edgeConfigData.getintvalue("numberOfControllers");
367 
368  // Ensure reading from the same file
369  assert(numControllers == actions.size());
370  assert(actions[0].size() == 2);
371 
372  double lowerLimit = m_config.lowPhase;
373  double upperLimit = m_config.highPhase;
374  double range = upperLimit - lowerLimit;
375 
376  array_4D actionList(boost::extents[m_config.segmentSpan]
377  [m_config.theirMuscles]
378  [m_config.ourMuscles]
379  [m_config.params]);
380 
381  /* Horrid while loop to populate upper diagonal of matrix, since
382  * its symmetric and we want to minimze parameters used in learing
383  * note that i==1, j==k will refer to the same muscle
384  * @todo use boost to set up array so storage is only allocated for
385  * elements that are used
386  */
387  int i = 0;
388  int j = 0;
389  int k = 0;
390 
391  while (i < m_config.segmentSpan)
392  {
393  while(j < m_config.theirMuscles)
394  {
395  while(k < m_config.ourMuscles)
396  {
397  if (actions.empty())
398  {
399  std::cout << "ran out before table populated!"
400  << std::endl;
401  break;
402  }
403  else
404  {
405  if (i == 1 && j == k)
406  {
407  // std::cout << "Skipped identical muscle" << std::endl;
408  //Skip since its the same muscle
409  }
410  else
411  {
412  std::vector<double> edgeParam = actions.back();
413  // Weight from 0 to 1
414  actionList[i][j][k][0] = edgeParam[0];
415  // Phase offset from -pi to pi
416  actionList[i][j][k][1] = edgeParam[1] *
417  (range) + lowerLimit;
418  actions.pop_back();
419  }
420  }
421  k++;
422  }
423  j++;
424  k = j;
425 
426  }
427  j = 0;
428  k = 0;
429  i++;
430  }
431 
432  assert(actions.empty());
433 
434  return actionList;
435 }
436 array_2D BaseSpineCPGControl::scaleNodeActions
437  (vector< vector <double> > actions)
438 {
439  std::size_t numControllers = nodeConfigData.getintvalue("numberOfControllers");
440  std::size_t numActions = nodeConfigData.getintvalue("numberOfActions");
441 
442  assert( actions.size() == numControllers);
443  assert( actions[0].size() == numActions);
444 
445  array_2D nodeActions(boost::extents[numControllers][numActions]);
446 
447  array_2D limits(boost::extents[2][numActions]);
448 
449  // Check if we need to update limits
450  assert(numActions == 2);
451 
452  limits[0][0] = m_config.lowFreq;
453  limits[1][0] = m_config.highFreq;
454  limits[0][1] = m_config.lowAmp;
455  limits[1][1] = m_config.highAmp;
456 
457  // This one is square
458  for( std::size_t i = 0; i < numControllers; i++)
459  {
460  for( std::size_t j = 0; j < numActions; j++)
461  {
462  nodeActions[i][j] = ( actions[i][j] *
463  (limits[1][j] - limits[0][j])) + limits[0][j];
464  }
465  }
466 
467  return nodeActions;
468 }
Contains the definition of class ImpedanceControl. $Id$.
virtual void onSetup(BaseSpineModelLearning &subject)
virtual void onStep(BaseSpineModelLearning &subject, double dt)
void update(std::vector< double > &descCom, double dt)
virtual void onTeardown(BaseSpineModelLearning &subject)
virtual void setupCPGs(BaseSpineModelLearning &subject, array_2D nodeActions, array_4D edgeActions)
void setConnectivity(const std::vector< tgCPGActuatorControl * > &allStrings, array_4D edgeParams)
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)
Definition of the tgCPGStringControl observer class.
BaseSpineCPGControl(BaseSpineCPGControl::Config config, std::string args, std::string resourcePath="", std::string ec="edgeConfig.ini", std::string nc="nodeConfig.ini")
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...
virtual array_4D scaleEdgeActions(std::vector< std::vector< double > > actions)
A controller for the template class BaseSpineModelLearning.
Definition of class CPGEquations.
virtual void onStep(tgModel &model, double dt)
Definition of class CPGNode.
void initialize(AnnealEvolution *evo, bool isLearning, configuration config)
virtual void onSetup(tgModel &model)