NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
AppGoalOnline.cpp
Go to the documentation of this file.
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 
26 #include "AppGoalOnline.h"
27 
28 AppGoalOnline::AppGoalOnline(int argc, char** argv)
29 {
30  bSetup = false;
31  use_graphics = false;
32  add_controller = true;
33  add_blocks = false;
34  add_hills = false;
35  all_terrain = false;
36  timestep_physics = 1.0f/1000.0f;
37  timestep_graphics = 1.0f/60.0f;
38  nEpisodes = 1;
39  nSteps = 60000;
40  nSegments = 6;
41  nTypes = 3;
42 
43  startX = 0;
44  startY = 20;
45  startZ = 0;
46  startAngle = 0;
47 
48  suffix = "default";
49 
50  handleOptions(argc, argv);
51 }
52 
54 {
55  // First create the world
56  world = createWorld();
57 
58  // Second create the view
59  if (use_graphics)
60  view = createGraphicsView(world); // For visual experimenting on one tensegrity
61  else
62  view = createView(world); // For running multiple episodes
63 
64  // Third create the simulation
65  simulation = new tgSimulation(*view);
66 
67  // Fourth create the models with their controllers and add the models to the
68  // simulation
71  FlemonsSpineModelGoal* myModel =
72  new FlemonsSpineModelGoal(nSegments, 0.0, 0.0);
73 
74  // Fifth create the controllers, attach to model
75  if (add_controller)
76  {
77  const int segmentSpan = 3;
78  const int numMuscles = 8;
79  const int numParams = 2;
80  const int segNumber = 0; // For learning results
81  const double controlTime = .01;
82  const double lowPhase = -1 * M_PI;
83  const double highPhase = M_PI;
84  const double lowAmplitude = 0.0;
85  const double highAmplitude = 300.0;
86  const double kt = 0.0;
87  const double kp = 1000.0;
88  const double kv = 200.0;
89  const bool def = true;
90 
91  // Overridden by def being true
92  const double cl = 10.0;
93  const double lf = 0.0;
94  const double hf = 30.0;
95 
96  // Feedback parameters
97  const double ffMin = -0.5;
98  const double ffMax = 10.0;
99  const double afMin = 0.0;
100  const double afMax = 200.0;
101  const double pfMin = -0.5;
102  const double pfMax = 6.28;
103  const double tensionFeedback = 1000.0;
104 
105  // How often to check/change the controller during online learning
106  const double feedbackTime = 3.0;
107 
108  SpineOnlineControl::Config control_config(segmentSpan,
109  numMuscles,
110  numMuscles,
111  numParams,
112  segNumber,
113  controlTime,
114  lowAmplitude,
115  highAmplitude,
116  lowPhase,
117  highPhase,
118  kt,
119  kp,
120  kv,
121  def,
122  cl,
123  lf,
124  hf,
125  ffMin,
126  ffMax,
127  afMin,
128  afMax,
129  pfMin,
130  pfMax,
131  tensionFeedback,
132  feedbackTime
133  );
135  SpineOnlineControl* const myControl =
136  new SpineOnlineControl(control_config, suffix, "bmirletz/TetrahedralComplex_Online/");
137 
138  myModel->attach(myControl);
139  }
140 
141  // Sixth add model & controller to simulation
142  simulation->addModel(myModel);
143 
144  if (add_blocks)
145  {
146  tgModel* blockField = getBlocks();
147  simulation->addObstacle(blockField);
148  }
149 
150  bSetup = true;
151  return bSetup;
152 }
153 
154 void AppGoalOnline::handleOptions(int argc, char **argv)
155 {
156  // Declare the supported options.
157  po::options_description desc("Allowed options");
158  desc.add_options()
159  ("help,h", "produce help message")
160  ("graphics,G", po::value<bool>(&use_graphics), "Test using graphical view")
161  ("controller,c", po::value<bool>(&add_controller), "Attach the controller to the model.")
162  ("blocks,b", po::value<bool>(&add_blocks)->implicit_value(false), "Add a block field as obstacles.")
163  ("hills,H", po::value<bool>(&add_hills)->implicit_value(false), "Use hilly terrain.")
164  ("all_terrain,A", po::value<bool>(&all_terrain)->implicit_value(false), "Alternate through terrain types. Only works with graphics off")
165  ("phys_time,p", po::value<double>(), "Physics timestep value (Hz). Default=1000")
166  ("graph_time,g", po::value<double>(), "Graphics timestep value a.k.a. render rate (Hz). Default = 60")
167  ("episodes,e", po::value<int>(&nEpisodes), "Number of episodes to run. Default=1")
168  ("steps,s", po::value<int>(&nSteps), "Number of steps per episode to run. Default=60K (60 seconds)")
169  ("segments,S", po::value<int>(&nSegments), "Number of segments in the tensegrity spine. Default=6")
170  ("start_x,x", po::value<double>(&startX), "X Coordinate of starting position for robot. Default = 0")
171  ("start_y,y", po::value<double>(&startY), "Y Coordinate of starting position for robot. Default = 20")
172  ("start_z,z", po::value<double>(&startZ), "Z Coordinate of starting position for robot. Default = 0")
173  ("angle,a", po::value<double>(&startAngle), "Angle of starting rotation for robot. Degrees. Default = 0")
174  ("learning_controller,l", po::value<std::string>(&suffix), "Which learned controller to write to or use. Default = default")
175  ;
176 
177  po::variables_map vm;
178  po::store(po::parse_command_line(argc, argv, desc), vm);
179 
180  if (vm.count("help"))
181  {
182  std::cout << desc << "\n";
183  exit(0);
184  }
185 
186  po::notify(vm);
187 
188  if (vm.count("phys_time"))
189  {
190  timestep_physics = 1/vm["phys_time"].as<double>();
191  std::cout << "Physics timestep set to: " << timestep_physics << " seconds.\n";
192  }
193 
194  if (vm.count("graph_time"))
195  {
196  timestep_graphics = 1/vm["graph_time"].as<double>();
197  std::cout << "Graphics timestep set to: " << timestep_graphics << " seconds.\n";
198  }
199 }
200 
201 const tgHillyGround::Config AppGoalOnline::getHillyConfig()
202 {
203  btVector3 eulerAngles = btVector3(0.0, 0.0, 0.0);
204  btScalar friction = 0.5;
205  btScalar restitution = 0.0;
206  // Size doesn't affect hilly terrain
207  btVector3 size = btVector3(0.0, 0.1, 0.0);
208  btVector3 origin = btVector3(0.0, 0.0, 0.0);
209  size_t nx = 180;
210  size_t ny = 180;
211  double margin = 0.5;
212  double triangleSize = 4.0;
213  double waveHeight = 2.0;
214  double offset = 0.0;
215  const tgHillyGround::Config hillGroundConfig(eulerAngles, friction, restitution,
216  size, origin, nx, ny, margin, triangleSize,
217  waveHeight, offset);
218  return hillGroundConfig;
219 }
220 
221 const tgBoxGround::Config AppGoalOnline::getBoxConfig()
222 {
223  const double yaw = 0.0;
224  const double pitch = 0.0;
225  const double roll = 0.0;
226  const double friction = 0.5;
227  const double restitution = 0.0;
228  const btVector3 size(1000.0, 1.5, 1000.0);
229 
230  const tgBoxGround::Config groundConfig(btVector3(yaw, pitch, roll),
231  friction,
232  restitution,
233  size );
234 
235  return groundConfig;
236 }
237 
238 tgModel* AppGoalOnline::getBlocks()
239 {
240  // Room to add a config
241  tgBlockField* myObstacle = new tgBlockField();
242  return myObstacle;
243 }
244 
245 tgWorld* AppGoalOnline::createWorld()
246 {
247  const tgWorld::Config config(
248  981 // gravity, cm/sec^2
249  );
250 
251  tgBulletGround* ground;
252 
253  if (add_hills)
254  {
255  const tgHillyGround::Config hillGroundConfig = getHillyConfig();
256  ground = new tgHillyGround(hillGroundConfig);
257  }
258  else
259  {
260  const tgBoxGround::Config groundConfig = getBoxConfig();
261  ground = new tgBoxGround(groundConfig);
262  }
263 
264  return new tgWorld(config, ground);
265 }
266 
267 tgSimViewGraphics *AppGoalOnline::createGraphicsView(tgWorld *world)
268 {
269  return new tgSimViewGraphics(*world, timestep_physics, timestep_graphics);
270 }
271 
272 tgSimView *AppGoalOnline::createView(tgWorld *world)
273 {
274  return new tgSimView(*world, timestep_physics, timestep_graphics);
275 }
276 
278 {
279  if (!bSetup)
280  {
281  setup();
282  }
283 
284  if (use_graphics)
285  {
286  // Run until the user stops
287  simulation->run();
288  }
289  else
290  {
291  // or run for a specific number of steps
292  simulate(simulation);
293  }
294 
296  delete simulation;
297  delete view;
298  delete world;
299 
300  return true;
301 }
302 
303 void AppGoalOnline::simulate(tgSimulation *simulation)
304 {
305  for (int i=0; i<nEpisodes; i++) {
306  fprintf(stderr,"Episode %d\n", i);
307  try
308  {
309  simulation->run(nSteps);
310  }
311  catch (std::runtime_error e)
312  {
313  // Nothing to do here, score will be set to -1
314  }
315 
316  // Don't change the terrain before the last episode to avoid leaks
317  if (all_terrain && i != nEpisodes - 1)
318  {
319  // Next run has Hills
320  if (i % nTypes == 0)
321  {
322 
323  const tgHillyGround::Config hillGroundConfig = getHillyConfig();
324  tgBulletGround* ground = new tgHillyGround(hillGroundConfig);
325  simulation->reset(ground);
326  }
327  // Flat
328  else if (i % nTypes == 1)
329  {
330  const tgBoxGround::Config groundConfig = getBoxConfig();
331  tgBulletGround* ground = new tgBoxGround(groundConfig);
332  simulation->reset(ground);
333  }
334  // Flat with blocks
335  else if (i % nTypes == 2)
336  {
337  simulation->reset();
338  tgModel* obstacle = getBlocks();
339  simulation->addObstacle(obstacle);
340  }
341  }
342  else
343  {
344  simulation->reset();
345  }
346  }
347 }
348 
355 int main(int argc, char** argv)
356 {
357  std::cout << "AppGoalOnline" << std::endl;
358  AppGoalOnline app (argc, argv);
359 
360  if (app.setup())
361  app.run();
362 
363  //Teardown is handled by delete, so that should be automatic
364  return 0;
365 }
366 
void addObstacle(tgModel *pObstacle)
void addModel(tgModel *pModel)
void run() const
void attach(tgObserver< T > *pObserver)
Definition: tgSubject.h:91
int main(int argc, char **argv)