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