43 #include "dev/btietz/hardwareSineWaves/tgSineStringControl.h"
54 std::string resourcePath,
75 std::vector<double> state;
79 array_2D nodeParams = scaleNodeActions(nodeAdapter.step(dt, state));
81 setupWaves(subject, nodeParams, edgeParams);
83 initConditions = subject.getSegmentCOM(m_config.segmentNumber);
84 #if (0) // Conditional compile for data logging
85 m_dataObserver.
onSetup(subject);
98 if (m_updateTime >= m_config.controlTime)
101 #if (0) // Conditional compile for data logging
102 m_dataObserver.
onStep(subject, m_updateTime);
108 double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];
111 if (currentHeight > 25 || currentHeight < 1.0)
121 std::vector<double> scores;
124 std::vector<double> finalConditions = subject.getSegmentCOM(m_config.segmentNumber);
126 const double newX = finalConditions[0];
127 const double newZ = finalConditions[2];
128 const double oldX = initConditions[0];
129 const double oldZ = initConditions[2];
131 const double distanceMoved = sqrt((newX-oldX) * (newX-oldX) +
132 (newZ-oldZ) * (newZ-oldZ));
136 scores.push_back(-1.0);
140 scores.push_back(distanceMoved);
145 double totalEnergySpent=0;
147 vector<tgSpringCableActuator* > tmpSCAs = subject.getAllMuscles();
148 vector<tgBasicActuator* > tmpStrings = tgCast::filter<tgSpringCableActuator, tgBasicActuator>(tmpSCAs);
150 for(
int i=0; i<tmpStrings.size(); i++)
157 const double previousLength = stringHist.
restLengths[j-1];
158 const double currentLength = stringHist.
restLengths[j];
160 double motorSpeed = (currentLength-previousLength);
163 const double workDone = previousTension * motorSpeed;
164 totalEnergySpent += workDone;
168 scores.push_back(totalEnergySpent);
170 edgeAdapter.endEpisode(scores);
171 nodeAdapter.endEpisode(scores);
173 for(
size_t i = 0; i < m_sineControllers.size(); i++)
175 delete m_sineControllers[i];
177 m_sineControllers.clear();
178 m_allControllers.clear();
183 std::vector <tgSpringCableActuator*> allMuscles = subject.getAllMuscles();
188 double controlLength;
190 for (std::size_t i = 0; i < allMuscles.size(); i++)
196 controlLength = allMuscles[i]->getStartLength();
207 allMuscles[i]->attach(pStringControl);
208 m_sineControllers.push_back(pStringControl);
211 assert(m_sineControllers.size() == allMuscles.size());
219 (vector< vector <double> > actions)
221 std::size_t numControllers = edgeConfigData.getintvalue(
"numberOfControllers");
222 std::size_t numActions = edgeConfigData.getintvalue(
"numberOfActions");
224 assert( actions.size() == numControllers);
225 assert( actions[0].size() == numActions);
227 array_2D edgeActions(boost::extents[numControllers][numActions]);
229 array_2D limits(boost::extents[2][numActions]);
232 assert(numActions == 1);
235 limits[0][0] = m_config.lowPhase;
236 limits[1][0] = m_config.highPhase;
239 for( std::size_t i = 0; i < numControllers; i++)
241 for( std::size_t j = 0; j < numActions; j++)
243 edgeActions[i][j] = ( actions[i][j] *
244 (limits[1][j] - limits[0][j])) + limits[0][j];
251 array_2D OctaCLSine::scaleNodeActions
252 (vector< vector <double> > actions)
254 std::size_t numControllers = nodeConfigData.getintvalue(
"numberOfControllers");
255 std::size_t numActions = nodeConfigData.getintvalue(
"numberOfActions");
257 assert( actions.size() == numControllers);
258 assert( actions[0].size() == numActions);
260 array_2D nodeActions(boost::extents[numControllers][numActions]);
262 array_2D limits(boost::extents[2][numActions]);
265 assert(numActions == 2);
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;
274 for( std::size_t i = 0; i < numControllers; i++)
276 for( std::size_t j = 0; j < numActions; j++)
278 nodeActions[i][j] = ( actions[i][j] *
279 (limits[1][j] - limits[0][j])) + limits[0][j];
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")
std::deque< double > tensionHistory
virtual void onTeardown(BaseSpineModelLearning &subject)
std::deque< double > restLengths
virtual void onSetup(BaseSpineModelLearning &subject)
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)
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)
virtual void onStep(tgModel &model, double dt)
void initialize(AnnealEvolution *evo, bool isLearning, configuration config)
virtual void onSetup(tgModel &model)
void notifyStep(double dt)