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