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);
67 const std::vector<tgBasicActuator*> muscles = subject.
getAllMuscles();
68 for (
size_t i = 0; i < muscles.size(); ++i) {
70 assert(pMuscle != NULL);
74 populateClusters(subject);
77 initializeSineWaves();
82 actions = evolutionAdapter.step(dt,state);
94 throw std::invalid_argument(
"dt is not positive");
98 setPreferredMuscleLengths(subject, dt);
99 const std::vector<tgBasicActuator*> muscles = subject.
getAllMuscles();
102 for (
size_t i = 0; i < muscles.size(); ++i)
105 assert(pMuscle != NULL);
110 for(
int i=0; i<muscles.size(); i++)
117 actions.push_back(tmp);
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 =
true;
147 vector <double> manualParams(4 * nClusters, 1);
148 if (usingManualParams) {
149 std::cout <<
"Using manually set parameters\n";
151 string filename =
"logs/trial_8/bestParamsSorted.dat";
153 manualParams = readManualParams(lineNumber, filename);
156 double pretension = 0.90;
158 double mins[4] = {m_initialLengths * (pretension - maxStringLengthFactor),
164 double maxes[4] = {m_initialLengths * (pretension + maxStringLengthFactor),
168 double ranges[4] = {maxes[0]-mins[0], maxes[1]-mins[1], maxes[2]-mins[2], maxes[3]-mins[3]};
170 for(
int i=0;i<actions.size();i++) {
171 for (
int j=0; j<actions[i].size(); j++) {
172 if (usingManualParams) {
173 actions[i][j] = manualParams[i*actions[i].size() + j]*(ranges[j])+mins[j];
175 actions[i][j] = actions[i][j]*(ranges[j])+mins[j];
187 assert(actions.size() == clusters.size());
190 for (
size_t cluster = 0; cluster < clusters.size(); cluster++) {
191 amplitude[cluster] = actions[cluster][0];
192 angularFrequency[cluster] = actions[cluster][1];
193 phaseChange[cluster] = actions[cluster][2];
194 dcOffset[cluster] = actions[cluster][3];
199 void Escape_T6Controller::setupAdapter() {
200 string suffix =
"_Escape";
201 string configAnnealEvolution =
"Config.ini";
203 bool isLearning =
true;
205 configEvolutionAdapter.readFile(configAnnealEvolution);
207 evolutionAdapter.
initialize(evo, isLearning, configEvolutionAdapter);
211 double Escape_T6Controller::totalEnergySpent(
Escape_T6Model& subject) {
212 double totalEnergySpent=0;
214 vector<tgBasicActuator* > tmpStrings = subject.
getAllMuscles();
215 for(
int i=0; i<tmpStrings.size(); i++)
217 tgBaseString::BaseStringHistory stringHist = tmpStrings[i]->getHistory();
219 for(
int j=1; j<stringHist.tensionHistory.size(); j++)
221 const double previousTension = stringHist.tensionHistory[j-1];
222 const double previousLength = stringHist.restLengths[j-1];
223 const double currentLength = stringHist.restLengths[j];
225 double motorSpeed = (currentLength-previousLength);
228 const double workDone = previousTension * motorSpeed;
229 totalEnergySpent += workDone;
232 return totalEnergySpent;
237 void Escape_T6Controller::setPreferredMuscleLengths(
Escape_T6Model& subject,
double dt) {
240 for(
int cluster=0; cluster<nClusters; cluster++) {
241 for(
int node=0; node<musclesPerCluster; node++) {
243 assert(pMuscle != NULL);
244 double newLength = amplitude[cluster] * sin(angularFrequency[cluster] * m_totalTime + phase) + dcOffset[cluster];
245 double minLength = m_initialLengths * (1-maxStringLengthFactor);
246 double maxLength = m_initialLengths * (1+maxStringLengthFactor);
247 if (newLength <= minLength) {
248 newLength = minLength;
249 }
else if (newLength >= maxLength) {
250 newLength = maxLength;
254 phase += phaseChange[cluster];
258 void Escape_T6Controller::populateClusters(
Escape_T6Model& subject) {
259 for(
int cluster=0; cluster < nClusters; cluster++) {
262 string suffix = ss.str();
263 std::vector <tgBasicActuator*> musclesInThisCluster = subject.
find<
tgBasicActuator>(
"muscle cluster" + suffix);
264 clusters[cluster] = std::vector<tgBasicActuator*>(musclesInThisCluster);
268 void Escape_T6Controller::initializeSineWaves() {
269 amplitude =
new double[nClusters];
270 angularFrequency =
new double[nClusters];
271 phaseChange =
new double[nClusters];
272 dcOffset =
new double[nClusters];
275 double Escape_T6Controller::displacement(
Escape_T6Model& subject) {
276 vector<double> finalPosition = subject.
getBallCOM();
281 const double newX = finalPosition[0];
282 const double newZ = finalPosition[2];
283 const double oldX = initPosition[0];
284 const double oldZ = initPosition[2];
286 const double distanceMoved = sqrt((newX-oldX) * (newX-oldX) +
287 (newZ-oldZ) * (newZ-oldZ));
288 return distanceMoved;
291 std::vector<double> Escape_T6Controller::readManualParams(
int lineNumber,
string filename) {
292 assert(lineNumber > 0);
293 vector<double> result(32, 1.0);
295 ifstream infile(filename.c_str(), ifstream::in);
298 if (infile.is_open()) {
299 for (
int i=0; i<lineNumber; i++) {
300 getline(infile, line);
308 stringstream lineStream(line);
311 while(getline(lineStream,cell,
',')) {
312 result[iCell] = atof(cell.c_str());
317 for (
int i=0; i < result.size(); i++) {
319 double seed = ((double) (rand() % 100)) / 100;
320 result[i] += (0.01 * seed) - 0.005;
327 void Escape_T6Controller::printSineParams() {
328 for (
size_t cluster = 0; cluster < clusters.size(); cluster++) {
329 std::cout <<
"amplitude[" << cluster <<
"]: " << amplitude[cluster] << std::endl;
330 std::cout <<
"angularFrequency[" << cluster <<
"]: " << angularFrequency[cluster] << std::endl;
331 std::cout <<
"phaseChange[" << cluster <<
"]: " << phaseChange[cluster] << std::endl;
332 std::cout <<
"dcOffset[" << cluster <<
"]: " << dcOffset[cluster] << std::endl;
virtual void moveMotors(double dt)
virtual vector< vector< double > > transformActions(vector< vector< double > > act)
const std::vector< tgBasicActuator * > & getAllMuscles() const
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)