28 #include "Escape_T6Controller.h"
42 # define M_PI 3.14159265358979323846
48 Escape_T6Controller::Escape_T6Controller(
const double initialLength) :
49 m_initialLengths(initialLength),
51 maxStringLengthFactor(0.50),
55 clusters.resize(nClusters);
56 for (
int i=0; i<nClusters; i++) {
57 clusters[i].resize(musclesPerCluster);
68 const std::vector<tgBasicActuator*> muscles = subject.
getAllMuscles();
69 for (
size_t i = 0; i < muscles.size(); ++i) {
71 assert(pMuscle != NULL);
75 populateClusters(subject);
78 initializeSineWaves();
83 actions = evolutionAdapter.step(dt,state);
95 throw std::invalid_argument(
"dt is not positive");
99 setPreferredMuscleLengths(subject, dt);
100 const std::vector<tgBasicActuator*> muscles = subject.
getAllMuscles();
103 for (
size_t i = 0; i < muscles.size(); ++i)
106 assert(pMuscle != NULL);
111 for(
int i=0; i<muscles.size(); i++)
118 actions.push_back(tmp);
122 static int count = 0;
126 for(
size_t i=0; i<muscles.size(); i++)
128 std::cout << (muscles[i]->getTension())/10 <<
"\t";
131 for(
size_t i=0; i<muscles.size(); i++)
133 std::cout << (muscles[i]->getCurrentLength())/10 <<
"\t";
147 std::vector<double> scores;
148 double distance = displacement(subject);
149 double energySpent = totalEnergySpent(subject);
152 scores.push_back(distance);
153 scores.push_back(energySpent);
155 std::cout <<
"Tearing down" << std::endl;
156 evolutionAdapter.endEpisode(scores);
169 bool usingManualParams =
false;
170 vector <double> manualParams(4 * nClusters, 1);
171 if (usingManualParams) {
172 std::cout <<
"Using manually set parameters\n";
173 string filename =
"logs/paramSortedBestTrials.dat";
175 manualParams = readManualParams(lineNumber, filename);
178 double pretension = 0.90;
180 double mins[4] = {m_initialLengths * (pretension - maxStringLengthFactor),
186 double maxes[4] = {m_initialLengths * (pretension + maxStringLengthFactor),
190 double ranges[4] = {maxes[0]-mins[0], maxes[1]-mins[1], maxes[2]-mins[2], maxes[3]-mins[3]};
192 for(
int i=0;i<actions.size();i++) {
193 for (
int j=0; j<actions[i].size(); j++) {
194 if (usingManualParams) {
195 actions[i][j] = manualParams[i*actions[i].size() + j]*(ranges[j])+mins[j];
197 actions[i][j] = actions[i][j]*(ranges[j])+mins[j];
209 assert(actions.size() == clusters.size());
212 for (
size_t cluster = 0; cluster < clusters.size(); cluster++) {
213 amplitude[cluster] = actions[cluster][0];
214 angularFrequency[cluster] = actions[cluster][1];
215 phaseChange[cluster] = actions[cluster][2];
216 dcOffset[cluster] = actions[cluster][3];
217 dcOffset[cluster] = 1;
222 void Escape_T6Controller::setupAdapter() {
223 string suffix =
"_Escape";
224 string configAnnealEvolution =
"Config.ini";
226 bool isLearning =
true;
228 configEvolutionAdapter.readFile(configAnnealEvolution);
230 evolutionAdapter.
initialize(evo, isLearning, configEvolutionAdapter);
234 double Escape_T6Controller::totalEnergySpent(
Escape_T6Model& subject) {
235 double totalEnergySpent=0;
237 vector<tgBasicActuator* > tmpStrings = subject.
getAllMuscles();
238 for(
int i=0; i<tmpStrings.size(); i++)
245 const double previousLength = stringHist.
restLengths[j-1];
246 const double currentLength = stringHist.
restLengths[j];
248 double motorSpeed = (currentLength-previousLength);
251 const double workDone = previousTension * motorSpeed;
252 totalEnergySpent += workDone;
255 return totalEnergySpent;
260 void Escape_T6Controller::setPreferredMuscleLengths(
Escape_T6Model& subject,
double dt) {
267 for(
int iMuscle=0; iMuscle < nMuscles; iMuscle++) {
269 const vector<tgBasicActuator*> muscles = subject.
getAllMuscles();
272 assert(pMuscle != NULL);
275 oldCluster = cluster;
278 }
else if (iMuscle < 8) {
280 }
else if (iMuscle < 12) {
282 }
else if (iMuscle < 16 ) {
284 }
else if (iMuscle < 18) {
286 }
else if (iMuscle < 20) {
288 }
else if (iMuscle < 22) {
294 double newLength = amplitude[cluster] * sin(angularFrequency[cluster] * m_totalTime + phase) + dcOffset[cluster];
295 double minLength = m_initialLengths * (1-maxStringLengthFactor);
296 double maxLength = m_initialLengths * (1+maxStringLengthFactor);
297 if (newLength <= minLength) {
298 newLength = minLength;
299 }
else if (newLength >= maxLength) {
300 newLength = maxLength;
303 if (oldCluster != cluster) {
304 phase += phaseChange[cluster];
327 void Escape_T6Controller::populateClusters(
Escape_T6Model& subject) {
328 for(
int cluster=0; cluster < nClusters; cluster++) {
331 string suffix = ss.str();
332 std::vector <tgBasicActuator*> musclesInThisCluster = subject.
find<
tgBasicActuator>(
"muscle cluster" + suffix);
333 clusters[cluster] = std::vector<tgBasicActuator*>(musclesInThisCluster);
337 void Escape_T6Controller::initializeSineWaves() {
338 amplitude =
new double[nClusters];
339 angularFrequency =
new double[nClusters];
340 phaseChange =
new double[nClusters];
341 dcOffset =
new double[nClusters];
344 double Escape_T6Controller::displacement(
Escape_T6Model& subject) {
345 vector<double> finalPosition = subject.
getBallCOM();
350 const double newX = finalPosition[0];
351 const double newZ = finalPosition[2];
352 const double oldX = initPosition[0];
353 const double oldZ = initPosition[2];
355 const double distanceMoved = sqrt((newX-oldX) * (newX-oldX) +
356 (newZ-oldZ) * (newZ-oldZ));
357 return distanceMoved;
360 std::vector<double> Escape_T6Controller::readManualParams(
int lineNumber,
string filename) {
361 assert(lineNumber > 0);
362 vector<double> result(32, 1.0);
364 ifstream infile(filename.c_str(), ifstream::in);
367 if (infile.is_open()) {
368 cout <<
"OPENED FILE\n";
369 for (
int i=0; i<lineNumber; i++) {
370 getline(infile, line);
374 cerr <<
"Error: Manual Parameters file not found\n";
381 stringstream lineStream(line);
384 while(getline(lineStream,cell,
',')) {
385 result[iCell] = atof(cell.c_str());
389 bool tweaking =
false;
392 for (
int i=0; i < result.size(); i++) {
393 std::cout<<
"Entered Cell " << i <<
": " << result[i] <<
"\n";
394 double seed = ((double) (rand() % 100)) / 100;
395 result[i] += (0.01 * seed) - 0.005;
399 cerr <<
"WARNING: Not changing manual input parameters\n";
405 void Escape_T6Controller::printSineParams() {
406 for (
size_t cluster = 0; cluster < clusters.size(); cluster++) {
407 std::cout <<
"amplitude[" << cluster <<
"]: " << amplitude[cluster] << std::endl;
408 std::cout <<
"angularFrequency[" << cluster <<
"]: " << angularFrequency[cluster] << std::endl;
409 std::cout <<
"phaseChange[" << cluster <<
"]: " << phaseChange[cluster] << std::endl;
410 std::cout <<
"dcOffset[" << cluster <<
"]: " << dcOffset[cluster] << std::endl;
virtual void moveMotors(double dt)
std::deque< double > tensionHistory
virtual vector< vector< double > > transformActions(vector< vector< double > > act)
const std::vector< tgBasicActuator * > & getAllMuscles() const
std::deque< double > restLengths
virtual void setControlInput(double input)
Contains the definition of class Escape_T6Model. $Id$.
A class to read a learning configuration from a .ini file.
virtual void onSetup(Escape_T6Model &subject)
Contains the definition of class tgBasicActuator.
std::vector< double > getBallCOM()
std::vector< T * > find(const tgTagSearch &tagSearch)
virtual void applyActions(Escape_T6Model &subject, vector< vector< double > > act)
virtual void onStep(Escape_T6Model &subject, double dt)
void initialize(AnnealEvolution *evo, bool isLearning, configuration config)
virtual void onTeardown(Escape_T6Model &subject)