46 # define M_PI 3.14159265358979323846
52 EscapeController::EscapeController(
const double initialLength,
54 std::string resourcePath,
56 m_initialLengths(initialLength),
58 maxStringLengthFactor(0.50),
62 configPath(resourcePath),
65 clusters.resize(nClusters);
66 for (
int i=0; i<nClusters; i++) {
67 clusters[i].resize(musclesPerCluster);
78 const std::vector<tgBasicActuator*> muscles = subject.
getAllMuscles();
79 for (
size_t i = 0; i < muscles.size(); ++i) {
81 assert(pMuscle != NULL);
85 populateClusters(subject);
88 initializeSineWaves();
93 actions = evolutionAdapter.step(dt,state);
105 throw std::invalid_argument(
"dt is not positive");
109 setPreferredMuscleLengths(subject, dt);
110 const std::vector<tgBasicActuator*> muscles = subject.
getAllMuscles();
113 for (
size_t i = 0; i < muscles.size(); ++i)
116 assert(pMuscle != NULL);
124 std::vector<double> scores;
125 double distance = displacement(subject);
126 double energySpent = totalEnergySpent(subject);
129 scores.push_back(distance);
130 scores.push_back(energySpent);
132 std::cout <<
"Tearing down" << std::endl;
133 evolutionAdapter.endEpisode(scores);
146 bool usingManualParams =
false;
147 vector <double> manualParams(4 * nClusters, 1);
148 if (usingManualParams) {
149 std::cout <<
"Using manually set parameters\n";
150 string filename =
"logs/paramSortedBestTrials.dat";
152 manualParams = readManualParams(lineNumber, filename);
155 double pretension = 0.90;
157 double mins[4] = {m_initialLengths * (pretension - maxStringLengthFactor),
163 double maxes[4] = {m_initialLengths * (pretension + maxStringLengthFactor),
167 double ranges[4] = {maxes[0]-mins[0], maxes[1]-mins[1], maxes[2]-mins[2], maxes[3]-mins[3]};
169 for(
int i=0;i<actions.size();i++) {
170 for (
int j=0; j<actions[i].size(); j++) {
171 if (usingManualParams) {
172 actions[i][j] = manualParams[i*actions[i].size() + j]*(ranges[j])+mins[j];
174 actions[i][j] = actions[i][j]*(ranges[j])+mins[j];
186 assert(actions.size() == clusters.size());
189 for (
size_t cluster = 0; cluster < clusters.size(); cluster++) {
190 amplitude[cluster] = actions[cluster][0];
191 angularFrequency[cluster] = actions[cluster][1];
192 phaseChange[cluster] = actions[cluster][2];
193 dcOffset[cluster] = actions[cluster][3];
194 dcOffset[cluster] = 1;
199 void EscapeController::setupAdapter() {
203 if (configPath !=
"")
205 path = FileHelpers::getResourcePath(configPath);
212 string configAnnealEvolution = path + configName;
214 bool isLearning =
true;
216 configEvolutionAdapter.readFile(configAnnealEvolution);
218 evolutionAdapter.
initialize(evo, isLearning, configEvolutionAdapter);
221 double EscapeController::totalEnergySpent(
EscapeModel& subject) {
222 double totalEnergySpent=0;
224 vector<tgBasicActuator* > tmpStrings = subject.
getAllMuscles();
225 for(
int i=0; i<tmpStrings.size(); i++)
232 const double previousLength = stringHist.
restLengths[j-1];
233 const double currentLength = stringHist.
restLengths[j];
235 double motorSpeed = (currentLength-previousLength);
238 const double workDone = previousTension * motorSpeed;
239 totalEnergySpent += workDone;
242 return totalEnergySpent;
247 void EscapeController::setPreferredMuscleLengths(
EscapeModel& subject,
double dt) {
254 for(
int iMuscle=0; iMuscle < nMuscles; iMuscle++) {
256 const vector<tgBasicActuator*> muscles = subject.
getAllMuscles();
259 assert(pMuscle != NULL);
262 oldCluster = cluster;
265 }
else if (iMuscle < 8) {
267 }
else if (iMuscle < 12) {
269 }
else if (iMuscle < 16 ) {
271 }
else if (iMuscle < 18) {
273 }
else if (iMuscle < 20) {
275 }
else if (iMuscle < 22) {
281 double newLength = amplitude[cluster] * sin(angularFrequency[cluster] * m_totalTime + phase) + dcOffset[cluster];
282 double minLength = m_initialLengths * (1-maxStringLengthFactor);
283 double maxLength = m_initialLengths * (1+maxStringLengthFactor);
284 if (newLength <= minLength) {
285 newLength = minLength;
286 }
else if (newLength >= maxLength) {
287 newLength = maxLength;
290 if (oldCluster != cluster) {
291 phase += phaseChange[cluster];
314 void EscapeController::populateClusters(
EscapeModel& subject) {
315 for(
int cluster=0; cluster < nClusters; cluster++) {
318 string suffix = ss.str();
319 std::vector <tgBasicActuator*> musclesInThisCluster = subject.
find<
tgBasicActuator>(
"muscle cluster" + suffix);
320 clusters[cluster] = std::vector<tgBasicActuator*>(musclesInThisCluster);
324 void EscapeController::initializeSineWaves() {
325 amplitude =
new double[nClusters];
326 angularFrequency =
new double[nClusters];
327 phaseChange =
new double[nClusters];
328 dcOffset =
new double[nClusters];
331 double EscapeController::displacement(
EscapeModel& subject) {
332 vector<double> finalPosition = subject.
getBallCOM();
337 const double newX = finalPosition[0];
338 const double newZ = finalPosition[2];
339 const double oldX = initPosition[0];
340 const double oldZ = initPosition[2];
342 const double distanceMoved = sqrt((newX-oldX) * (newX-oldX) +
343 (newZ-oldZ) * (newZ-oldZ));
344 return distanceMoved;
347 std::vector<double> EscapeController::readManualParams(
int lineNumber,
string filename) {
348 assert(lineNumber > 0);
349 vector<double> result(32, 1.0);
351 ifstream infile(filename.c_str(), ifstream::in);
354 if (infile.is_open()) {
355 cout <<
"OPENED FILE\n";
356 for (
int i=0; i<lineNumber; i++) {
357 getline(infile, line);
361 cerr <<
"Error: Manual Parameters file not found\n";
368 stringstream lineStream(line);
371 while(getline(lineStream,cell,
',')) {
372 result[iCell] = atof(cell.c_str());
376 bool tweaking =
false;
379 for (
int i=0; i < result.size(); i++) {
380 std::cout<<
"Entered Cell " << i <<
": " << result[i] <<
"\n";
381 double seed = ((double) (rand() % 100)) / 100;
382 result[i] += (0.01 * seed) - 0.005;
385 cerr <<
"WARNING: Not changing manual input parameters\n";
391 void EscapeController::printSineParams() {
392 for (
size_t cluster = 0; cluster < clusters.size(); cluster++) {
393 std::cout <<
"amplitude[" << cluster <<
"]: " << amplitude[cluster] << std::endl;
394 std::cout <<
"angularFrequency[" << cluster <<
"]: " << angularFrequency[cluster] << std::endl;
395 std::cout <<
"phaseChange[" << cluster <<
"]: " << phaseChange[cluster] << std::endl;
396 std::cout <<
"dcOffset[" << cluster <<
"]: " << dcOffset[cluster] << std::endl;
Contains the definition of class EscapeModel. $Id$.
virtual void moveMotors(double dt)
std::deque< double > tensionHistory
virtual void onSetup(EscapeModel &subject)
virtual void onTeardown(EscapeModel &subject)
virtual void applyActions(EscapeModel &subject, vector< vector< double > > act)
std::deque< double > restLengths
virtual void setControlInput(double input)
A class to read a learning configuration from a .ini file.
A series of functions to assist with file input/output.
Contains the definition of class AnnealEvolution. Adapting NeuroEvolution to do Simulated Annealing...
Contains the definition of class tgBasicActuator.
virtual vector< vector< double > > transformActions(vector< vector< double > > act)
const std::vector< tgBasicActuator * > & getAllMuscles() const
std::vector< T * > find(const tgTagSearch &tagSearch)
Defines a class AnnealAdapter to pass parameters from AnnealEvolution to a controller. Adapting NeuroEvolution to do Simulated Annealing.
virtual void onStep(EscapeModel &subject, double dt)
void initialize(AnnealEvolution *evo, bool isLearning, configuration config)
Contains the definition of class EscapeController.
std::vector< double > getBallCOM()