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