NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
AppTerrainJSON.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 "AppTerrainJSON.h"
28 #include "tgCPGJSONLogger.h"
29 
30 AppTerrainJSON::AppTerrainJSON(int argc, char** argv)
31 {
32  bSetup = false;
33  use_graphics = false;
34  add_controller = true;
35  add_blocks = false;
36  add_hills = false;
37  all_terrain = false;
38  timestep_physics = 1.0f/1000.0f;
39  timestep_graphics = 1.0f/60.0f;
40  nEpisodes = 1;
41  nSteps = 60000;
42  nSegments = 6;
43  nTypes = 3;
44 
45  startX = 0;
46  startY = 20;
47  startZ = 0;
48  startAngle = 0;
49 
50  suffix = "default";
51 
52  handleOptions(argc, argv);
53 }
54 
56 {
57  // First create the world
58  world = createWorld();
59 
60  // Second create the view
61  if (use_graphics)
62  view = createGraphicsView(world); // For visual experimenting on one tensegrity
63  else
64  view = createView(world); // For running multiple episodes
65 
66  // Third create the simulation
67  simulation = new tgSimulation(*view);
68 
69  // Fourth create the models with their controllers and add the models to the
70  // simulation
72  FlemonsSpineModelContact* myModel =
73  new FlemonsSpineModelContact(nSegments);
74 
75  // Fifth create the controllers, attach to model
76  if (add_controller)
77  {
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  JSONFeedbackControl::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);
129  JSONFeedbackControl* const myControl =
130  new JSONFeedbackControl(control_config, suffix, "bmirletz/TetrahedralComplex_Contact/");
131 
132 #if (0)
133  tgCPGJSONLogger* const myLogger =
134  new tgCPGJSONLogger("logs/CPGValues.txt");
135 
136  myControl->attach(myLogger);
137 #endif
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 AppTerrainJSON::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 AppTerrainJSON::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 = 100;
210  size_t ny = 100;
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 AppTerrainJSON::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* AppTerrainJSON::getBlocks()
239 {
240  // Room to add a config
241  tgBlockField* myObstacle = new tgBlockField();
242  return myObstacle;
243 }
244 
245 tgWorld* AppTerrainJSON::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 *AppTerrainJSON::createGraphicsView(tgWorld *world)
268 {
269  return new tgSimViewGraphics(*world, timestep_physics, timestep_graphics);
270 }
271 
272 tgSimView *AppTerrainJSON::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 AppTerrainJSON::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  // Avoid resetting twice on the last run
343  else if (i != nEpisodes - 1)
344  {
345  simulation->reset();
346  }
347 
348  }
349 }
350 
357 int main(int argc, char** argv)
358 {
359  std::cout << "AppTerrainJSON" << std::endl;
360  AppTerrainJSON app (argc, argv);
361 
362  if (app.setup())
363  app.run();
364 
365  //Teardown is handled by delete, so that should be automatic
366  return 0;
367 }
368 
void addObstacle(tgModel *pObstacle)
int main(int argc, char **argv)
void addModel(tgModel *pModel)
void run() const
void attach(tgObserver< T > *pObserver)
Definition: tgSubject.h:91