NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
OctaCLSine.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 #include "OctaCLSine.h"
29 
30 #include <string>
31 
32 
33 // Should include tgString, but compiler complains since its been
34 // included from TetraSpineLearningModel. Perhaps we should move things
35 // to a cpp over there
36 #include "core/tgBasicActuator.h"
39 
42 
43 #include "dev/btietz/hardwareSineWaves/tgSineStringControl.h"
44 
45 using namespace std;
46 
53  std::string args,
54  std::string resourcePath,
55  std::string ec,
56  std::string nc) :
57 BaseSpineCPGControl(config, args, resourcePath, ec, nc)
58 
59 {
60 }
61 
63 {
64  //Initialize the Learning Adapters
65  nodeAdapter.initialize(&nodeEvolution,
66  nodeLearning,
67  nodeConfigData);
68  edgeAdapter.initialize(&edgeEvolution,
69  edgeLearning,
70  edgeConfigData);
71  /* Empty vector signifying no state information
72  * All parameters are stateless parameters, so we can get away with
73  * only doing this once
74  */
75  std::vector<double> state;
76  double dt = 0;
77 
78  array_2D edgeParams = scalePhaseActions(edgeAdapter.step(dt, state));
79  array_2D nodeParams = scaleNodeActions(nodeAdapter.step(dt, state));
80 
81  setupWaves(subject, nodeParams, edgeParams);
82 
83  initConditions = subject.getSegmentCOM(m_config.segmentNumber);
84 #if (0) // Conditional compile for data logging
85  m_dataObserver.onSetup(subject);
86 #endif
87 
88 
89  m_updateTime = 0.0;
90  bogus = false;
91 
92 }
93 
94 void OctaCLSine::onStep(BaseSpineModelLearning& subject, double dt)
95 {
97  m_updateTime += dt;
98  if (m_updateTime >= m_config.controlTime)
99  {
100 
101 #if (0) // Conditional compile for data logging
102  m_dataObserver.onStep(subject, m_updateTime);
103 #endif
104  notifyStep(m_updateTime);
105  m_updateTime = 0;
106  }
107 
108  double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];
109 
111  if (currentHeight > 25 || currentHeight < 1.0)
112  {
114  bogus = true;
115  }
116 
117 }
118 
120 {
121  std::vector<double> scores;
122  // @todo - check to make sure we ran for the right amount of time
123 
124  std::vector<double> finalConditions = subject.getSegmentCOM(m_config.segmentNumber);
125 
126  const double newX = finalConditions[0];
127  const double newZ = finalConditions[2];
128  const double oldX = initConditions[0];
129  const double oldZ = initConditions[2];
130 
131  const double distanceMoved = sqrt((newX-oldX) * (newX-oldX) +
132  (newZ-oldZ) * (newZ-oldZ));
133 
134  if (bogus)
135  {
136  scores.push_back(-1.0);
137  }
138  else
139  {
140  scores.push_back(distanceMoved);
141  }
142 
145  double totalEnergySpent=0;
146 
147  vector<tgSpringCableActuator* > tmpSCAs = subject.getAllMuscles();
148  vector<tgBasicActuator* > tmpStrings = tgCast::filter<tgSpringCableActuator, tgBasicActuator>(tmpSCAs);
149 
150  for(int i=0; i<tmpStrings.size(); i++)
151  {
152  tgSpringCableActuator::SpringCableActuatorHistory stringHist = tmpStrings[i]->getHistory();
153 
154  for(int j=1; j<stringHist.tensionHistory.size(); j++)
155  {
156  const double previousTension = stringHist.tensionHistory[j-1];
157  const double previousLength = stringHist.restLengths[j-1];
158  const double currentLength = stringHist.restLengths[j];
159  //TODO: examine this assumption - free spinning motor may require more power
160  double motorSpeed = (currentLength-previousLength);
161  if(motorSpeed > 0) // Vestigial code
162  motorSpeed = 0;
163  const double workDone = previousTension * motorSpeed;
164  totalEnergySpent += workDone;
165  }
166  }
167 
168  scores.push_back(totalEnergySpent);
169 
170  edgeAdapter.endEpisode(scores);
171  nodeAdapter.endEpisode(scores);
172 
173  for(size_t i = 0; i < m_sineControllers.size(); i++)
174  {
175  delete m_sineControllers[i];
176  }
177  m_sineControllers.clear();
178  m_allControllers.clear();
179 }
180 
181 void OctaCLSine::setupWaves(BaseSpineModelLearning& subject, array_2D nodeActions, array_2D edgeActions)
182 {
183  std::vector <tgSpringCableActuator*> allMuscles = subject.getAllMuscles();
184 
185  double tension;
186  double kPosition;
187  double kVelocity;
188  double controlLength;
189 
190  for (std::size_t i = 0; i < allMuscles.size(); i++)
191  {
192  tgImpedanceController* p_ipc = new tgImpedanceController( m_config.tension,
193  m_config.kPosition,
194  m_config.kVelocity);
195 
196  controlLength = allMuscles[i]->getStartLength();
197 
198  tgSineStringControl* pStringControl = new tgSineStringControl(m_config.controlTime,
199  p_ipc,
200  nodeActions[0][0],
201  nodeActions[0][1],
202  edgeActions[i][0],
203  0.0, // Repeat learning this too? Unlikely to be helpful
204  controlLength);
205 
206 
207  allMuscles[i]->attach(pStringControl);
208  m_sineControllers.push_back(pStringControl);
209  }
210 
211  assert(m_sineControllers.size() == allMuscles.size());
212 
213 
214 
215 
216 }
217 
219  (vector< vector <double> > actions)
220 {
221  std::size_t numControllers = edgeConfigData.getintvalue("numberOfControllers");
222  std::size_t numActions = edgeConfigData.getintvalue("numberOfActions");
223 
224  assert( actions.size() == numControllers);
225  assert( actions[0].size() == numActions);
226 
227  array_2D edgeActions(boost::extents[numControllers][numActions]);
228 
229  array_2D limits(boost::extents[2][numActions]);
230 
231  // Check if we need to update limits
232  assert(numActions == 1);
233 
234 
235  limits[0][0] = m_config.lowPhase;
236  limits[1][0] = m_config.highPhase;
237 
238  // This one is square
239  for( std::size_t i = 0; i < numControllers; i++)
240  {
241  for( std::size_t j = 0; j < numActions; j++)
242  {
243  edgeActions[i][j] = ( actions[i][j] *
244  (limits[1][j] - limits[0][j])) + limits[0][j];
245  }
246  }
247 
248  return edgeActions;
249 }
250 
251 array_2D OctaCLSine::scaleNodeActions
252  (vector< vector <double> > actions)
253 {
254  std::size_t numControllers = nodeConfigData.getintvalue("numberOfControllers");
255  std::size_t numActions = nodeConfigData.getintvalue("numberOfActions");
256 
257  assert( actions.size() == numControllers);
258  assert( actions[0].size() == numActions);
259 
260  array_2D nodeActions(boost::extents[numControllers][numActions]);
261 
262  array_2D limits(boost::extents[2][numActions]);
263 
264  // Check if we need to update limits
265  assert(numActions == 2);
266 
267 
268  limits[0][0] = m_config.lowAmp;
269  limits[1][0] = m_config.highAmp;
270  limits[1][1] = m_config.lowFreq;
271  limits[1][1] = m_config.highFreq;
272 
273  // This one is square
274  for( std::size_t i = 0; i < numControllers; i++)
275  {
276  for( std::size_t j = 0; j < numActions; j++)
277  {
278  nodeActions[i][j] = ( actions[i][j] *
279  (limits[1][j] - limits[0][j])) + limits[0][j];
280  }
281  }
282 
283  return nodeActions;
284 }
Contains the definition of class ImpedanceControl. $Id$.
OctaCLSine(BaseSpineCPGControl::Config config, std::string args, std::string resourcePath="", std::string ec="edgeConfig.ini", std::string nc="nodeConfig.ini")
Definition: OctaCLSine.cpp:52
virtual void onTeardown(BaseSpineModelLearning &subject)
Definition: OctaCLSine.cpp:119
virtual void onSetup(BaseSpineModelLearning &subject)
Definition: OctaCLSine.cpp:62
Controller for FlemonsSpineLearningModelCL.
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...
virtual void onStep(BaseSpineModelLearning &subject, double dt)
Definition: OctaCLSine.cpp:94
Contains the definition of class AnnealEvolution. Adapting NeuroEvolution to do Simulated Annealing...
Contains the definition of class tgBasicActuator.
virtual array_2D scalePhaseActions(std::vector< std::vector< double > > actions)
Definition: OctaCLSine.cpp:219
virtual void onStep(tgModel &model, double dt)
void initialize(AnnealEvolution *evo, bool isLearning, configuration config)
virtual void onSetup(tgModel &model)