NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
SuperBallLearningController.cpp
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 
27 // This module
28 #include "SuperBallLearningController.h"
29 // This application
30 #include "SuperBallModel.h"
31 // This library
32 #include "core/tgBasicActuator.h"
33 // The C++ Standard Library
34 #include <cassert>
35 #include <stdexcept>
36 #include <vector>
37 
38 using namespace std;
39 
40 //Constructor using the model subject and a single pref length for all muscles.
41 SuperBallPrefLengthController::SuperBallPrefLengthController(const double initialLength)
42 {
43  this->m_initialLengths=initialLength;
44  this->m_totalTime=0.0;
45 
46  evolution = new AnnealEvolution("superball");
47 
48 }
49 
50 //Fetch all the muscles and set their preferred length
51 void SuperBallPrefLengthController::onSetup(SuperBallModel& subject)
52 {
53  m_totalTime=0;
54  const std::vector<tgBasicActuator*> muscles = subject.getAllMuscles();
55  for (size_t i = 0; i < muscles.size(); ++i)
56  {
57  tgBasicActuator * const pMuscle = muscles[i];
58  assert(pMuscle != NULL);
59  pMuscle->setControlInput(this->m_initialLengths,0.0001);
60  }
61 
62  btTransform boxTr;
63  boxTr.setIdentity();
64  double randomAngle=((rand() / (double)RAND_MAX) - 0.5) * 3.1415;
65  randomAngle=3.1415/2.0;
66  randomAngle=0.0;
67 
68  btDynamicsWorld *btworld=subject.getWorld();
69  boxTr.setOrigin(btVector3(1000*cos(randomAngle),50,1000*sin(randomAngle)));
70  btDefaultMotionState* motSt = new btDefaultMotionState(boxTr);
71  btRigidBody::btRigidBodyConstructionInfo const rbInfo1(1,motSt, new btBoxShape(btVector3(1.0,1.0,1.0)));
72  goalPoint = new btRigidBody(rbInfo1);
73  btworld->addRigidBody(goalPoint);
74 
75  boxTr.setIdentity();
76  boxTr.setOrigin(btVector3(0,1.1,0));
77  btDefaultMotionState* motSt2 = new btDefaultMotionState(boxTr);
78  btRigidBody::btRigidBodyConstructionInfo const rbInfo2(0.0,motSt2,new btBoxShape(btVector3(500.0,0.1,500.0)));
79  btRigidBody *groundBox = new btRigidBody(rbInfo2);
80  btworld->addRigidBody(groundBox);
81 
82  m_subject = &subject;
83 
84 }
85 
86 void SuperBallPrefLengthController::onStep(SuperBallModel& subject, double dt)
87 {
88  if (dt <= 0.0)
89  {
90  throw std::invalid_argument("dt is not positive");
91  }
92  m_totalTime+=dt;
93 
94  //Store the initial position if the first step
95  if(initialPosition.empty())
96  {
97  btVector3 pos= subject.getCenter();
98  initialPosition.push_back(pos[0]);
99  initialPosition.push_back(pos[2]);
100  }
101 
102  //Move motors for all the muscles
103  const std::vector<tgBasicActuator*> muscles = subject.getAllMuscles();
104  for (size_t i = 0; i < muscles.size(); ++i)
105  {
106  tgBasicActuator * const pMuscle = muscles[i];
107  assert(pMuscle != NULL);
108  pMuscle->moveMotors(dt);
109  }
110 
111  vector<double> state=subject.getSensorInfo();
112 
114 // std::cout<<m_totalTime;
115 // for(int i=0;i<state.size();i++)
116 // {
117 // std::cout<<","<<state[i];
118 // }
119 // std::cout<<endl;
120 
121 
122  vector< vector< double> > actions = receiveActionsFromEvolution();
123 
124  //transform them from 0-1 to the size of the structure
125  actions = transformActions(actions);
126 
127  //apply these actions to the appropriate muscles according to the sensor values
128  applyActions(subject,actions,state);
129 
130 }
131 
132 //Scale actions according to Min and Max length of muscles.
133 vector< vector <double> > SuperBallPrefLengthController::transformActions(vector< vector <double> > actions)
134 {
135  double min=6;
136  double max=11;
137  double range=max-min;
138  double scaledAct;
139  for(int i=0;i<actions.size();i++)
140  {
141  for(int j=0;j<actions[i].size();j++)
142  {
143  scaledAct=actions[i][j]*(range)+min;
144  actions[i][j]=scaledAct;
145  }
146  }
147  return actions;
148 }
149 
150 
151 typedef std::pair<int,double> mypair;
152 bool comparator ( const mypair& l, const mypair& r)
153  { return l.second < r.second; }
154 
155 
156 //Pick particular muscles (according to the structure's state) and apply the given actions one by one
157 void SuperBallPrefLengthController::applyActions(SuperBallModel& subject, vector< vector <double> > actions, vector<double> nodeRayDistances)
158 {
159  vector< btVector3 > nodePositions=subject.getSensorPositions();
160  vector< btVector3 > nodeDirections=subject.getSensorOrientations();
161 
162  vector< mypair > nodeHeightWithIndex;
163  for(int i=0;i<nodePositions.size();i++)
164  {
165  //use height of each node (closeness to the ground is not working yet.)
166  nodeHeightWithIndex.push_back(make_pair(i,nodeRayDistances[i]));
167  }
168  std::sort(nodeHeightWithIndex.begin(),nodeHeightWithIndex.end(),comparator);
169 
170  vector<int> groundNodes;
171  groundNodes.push_back(nodeHeightWithIndex[0].first);
172  groundNodes.push_back(nodeHeightWithIndex[1].first);
173  groundNodes.push_back(nodeHeightWithIndex[2].first);
174 
175  int a=groundNodes[0];
176  int b=groundNodes[1];
177  int c=groundNodes[2];
178 
179  int numberOfMusclesOnBase=0;
180  if(subject.getMusclesPerNodes()[a][b]!=NULL)
181  numberOfMusclesOnBase++;
182  if(subject.getMusclesPerNodes()[a][c]!=NULL)
183  numberOfMusclesOnBase++;
184  if(subject.getMusclesPerNodes()[b][c]!=NULL)
185  numberOfMusclesOnBase++;
186 
187  //if all 3 are connected
188  if(numberOfMusclesOnBase==3)
189  {
190  //find the order or base points
191  //assuming we go to z direction
192  double maxDistance=-10000;
193  double minDistance=10000;
194  int furthestNode=-1;
195  int closestNode=-1;
196  for(int i=0;i<3;i++)
197  {
198 
199  double dist=goalPoint->getCenterOfMassPosition().distance(nodePositions[groundNodes[i]]);
200  if(dist>maxDistance)
201  {
202  furthestNode=i;
203  maxDistance=dist;
204  }
205  if(dist<minDistance)
206  {
207  closestNode=i;
208  minDistance=dist;
209  }
210  }
211  int groundNodesInOrder[3]={-1,-1,-1};
212  for(int i=0;i<3;i++)
213  {
214  if(i==furthestNode)
215  groundNodesInOrder[2]=groundNodes[furthestNode];
216  else if(subject.getMusclesPerNodes()[groundNodes[i]][subject.getOtherEndOfTheRod(groundNodes[furthestNode])] != NULL)
217  groundNodesInOrder[1]=groundNodes[i];
218  else
219  groundNodesInOrder[0]=groundNodes[i];
220  }
221 // cout<<"ground nodes in order: "<<groundNodesInOrder[0]<<" "<<groundNodesInOrder[1]<<" "<<groundNodesInOrder[2]<<" "<<endl;
222  //fill node mapping according to these new base points
223  subject.fillNodeMappingFromBasePoints(groundNodesInOrder[0],groundNodesInOrder[1],groundNodesInOrder[2]);
224 
225  int actionNo=0;
226  for(int i=0;i<13;i++)
227  {
228  for(int j=i+1;j<13;j++)
229  {
230  if(subject.muscleConnections[i][j]>=0)
231  {
232  int nodeStartOriginal=i;
233  int nodeEndOriginal=j;
234  //give the action that corresponds to these nodes originally to the ones with new mapping
235  int nodeStartNew=subject.nodeMappingReverse[nodeStartOriginal];
236  int nodeEndNew=subject.nodeMappingReverse[nodeEndOriginal];
237  tgBasicActuator * correspondingMuscle = subject.musclesPerNodes[nodeStartNew][nodeEndNew];
238  if(correspondingMuscle==NULL)
239  {
240  cout<<"NO MUSCLE EXISTS ACCORDING TO THE NEW MAPPING"<<endl;
241 // exit(1);
242  }
243  if(actionNo>=actions.size())
244  {
245  cout<<"Warning: actions < number of active muscles, no input given"<<endl;
246  continue;
247  }
248  correspondingMuscle->setControlInput(actions[actionNo][0]);
249  }
250  }
251  }
252  }
253  else if(numberOfMusclesOnBase==2)
254  {
255  //3 points, but there are 2 connections.
256  //We assume that it's the isoceles triangle.
257 // cout<<"3 points on ground, but not connected"<<endl;
258  double maxDistance=-50000;
259  double minDistance=50000;
260  int furthestNode=-1;
261  int closestNode=-1;
262  for(int i=0;i<3;i++)
263  {
264 
265  double dist=goalPoint->getCenterOfMassPosition().distance(nodePositions[groundNodes[i]]);
266  if(dist>maxDistance)
267  {
268  furthestNode=groundNodes[i];
269  maxDistance=dist;
270  }
271  if(dist<minDistance)
272  {
273  closestNode=groundNodes[i];
274  minDistance=dist;
275  }
276  }
277  int middleNode=-1;
278  for(int i=0;i<3;i++)
279  {
280  if(groundNodes[i]!=closestNode && groundNodes[i]!=furthestNode)
281  middleNode=groundNodes[i];
282  }
283  //We want to do a flop over the closest node and connected edge.
284  //Find closest and conected.
285  int landingNode;
286  int flippingNode;
287  if(subject.musclesPerNodes[closestNode][middleNode]!=NULL)
288  {
289  flippingNode=middleNode;
290  }
291  else
292  {
293  flippingNode=furthestNode;
294  }
295  //find the node that is connected to these two
296  for(int i=0;i<12;i++)
297  {
298  if(subject.musclesPerNodes[closestNode][i]!=NULL && subject.musclesPerNodes[flippingNode][i]!=NULL)
299  {
300  landingNode=i;
301  break;
302  }
303  }
304  //Select the base triangle based on that edge assuming we are coming from that base to this point.
305  int triangle[3]={closestNode,flippingNode,landingNode};
306  int triangleInOrder[3]={-1,-1,-1};
307  triangleInOrder[2]=landingNode;
308  if(subject.musclesPerNodes[closestNode][subject.getOtherEndOfTheRod(landingNode)] != NULL)
309  {
310  triangleInOrder[1]=closestNode;
311  triangleInOrder[0]=flippingNode;
312  }
313  else
314  {
315  triangleInOrder[1]=flippingNode;
316  triangleInOrder[0]=closestNode;
317  }
318  subject.fillNodeMappingFromBasePoints(triangleInOrder[0],triangleInOrder[1],triangleInOrder[2]);
319 
320  int actionNo=0;
321  for(int i=0;i<13;i++)
322  {
323  for(int j=i+1;j<13;j++)
324  {
325  if(subject.muscleConnections[i][j]>=0)
326  {
327  int nodeStartOriginal=i;
328  int nodeEndOriginal=j;
329  //give the action that corresponds to these nodes originally to the ones with new mapping
330  int nodeStartNew=subject.nodeMappingReverse[nodeStartOriginal];
331  int nodeEndNew=subject.nodeMappingReverse[nodeEndOriginal];
332  tgBasicActuator * correspondingMuscle = subject.musclesPerNodes[nodeStartNew][nodeEndNew];
333  if(correspondingMuscle==NULL)
334  {
335  cout<<"NO MUSCLE EXISTS ACCORDING TO THE NEW MAPPING"<<endl;
336  exit(1);
337  }
338  if(actionNo>=actions.size())
339  {
340  cout<<"Warning: actions < number of active muscles, no input given"<<endl;
341  continue;
342  }
343  correspondingMuscle->setControlInput(actions[actionNo][0]);
344  }
345  }
346  }
347  }
348 }
349 
350 void SuperBallPrefLengthController::onTeardown(SuperBallModel& subject)
351 {
352  std::cout<<"TEARDOWN CALLED"<<std::endl;
353  vector<double> scores;
354  scores.push_back(calculateDistanceMoved());
355  evolution->updateScores(scores);
356  std::cout<<"Distance moved: "<<scores[0]<<std::endl;
357 
358  initialPosition.clear();
359 }
360 
361 vector<vector<double> > SuperBallPrefLengthController::receiveActionsFromEvolution()
362 {
363  vector<vector <double> > actions;
364 
365  vector<AnnealEvoMember *> members = evolution->nextSetOfControllers();
366  for(int i=0;i<members.size();i++)
367  {
368  actions.push_back(members[i]->statelessParameters);
369  }
370  return actions;
371 }
372 
373 
374 
375 double SuperBallPrefLengthController::calculateDistanceMoved()
376 {
377  vector<double> scores;
378  double x= m_subject->getCenter()[0] - goalPoint->getCenterOfMassPosition().getX();
379  double z= m_subject->getCenter()[2] - goalPoint->getCenterOfMassPosition().getZ();
380  double distanceNew=sqrt(x*x+z*z);
381  double xx=initialPosition[0]-goalPoint->getCenterOfMassPosition().getX();
382  double zz=initialPosition[1]-goalPoint->getCenterOfMassPosition().getZ();
383  double distanceOld=sqrt(xx*xx+zz*zz);
384  double distanceMoved=distanceOld-distanceNew;
385 
386  //If you want to calculate only the distance moved independent of the target:
387 // distanceMoved=sqrt((x-xx)*(x-xx)+(z-zz)*(z-zz));
388 
389  return distanceMoved;
390 }
SuperBallPrefLengthController::onTeardown
virtual void onTeardown(SuperBallModel &subject)
Definition: SuperBallLearningController.cpp:350
SuperBallModel::getSensorInfo
std::vector< double > getSensorInfo()
Definition: SuperBallModel.cpp:203
tgBasicActuator::moveMotors
virtual void moveMotors(double dt)
Definition: tgBasicActuator.cpp:170
SuperBallModel::getSensorOrientations
std::vector< btVector3 > getSensorOrientations()
Definition: SuperBallModel.cpp:393
tgBasicActuator::setControlInput
virtual void setControlInput(double input)
Definition: tgBasicActuator.cpp:137
SuperBallModel.h
Contains the definition of class SuperBallModel. $Id$.
SuperBallModel::getAllMuscles
const std::vector< tgBasicActuator * > & getAllMuscles() const
Definition: SuperBallModel.cpp:349
SuperBallPrefLengthController::SuperBallPrefLengthController
SuperBallPrefLengthController(const double prefLength=5)
Definition: SuperBallLearningController.cpp:41
tgBasicActuator.h
Contains the definition of class tgBasicActuator.
SuperBallPrefLengthController::onSetup
virtual void onSetup(SuperBallModel &subject)
Definition: SuperBallLearningController.cpp:51
SuperBallModel::getSensorPositions
std::vector< btVector3 > getSensorPositions()
Definition: SuperBallModel.cpp:383
SuperBallPrefLengthController::onStep
virtual void onStep(SuperBallModel &subject, double dt)
Definition: SuperBallLearningController.cpp:86