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