NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
AppGoalTension.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 "AppGoalTension.h"
28 
29 #include "helpers/FileHelpers.h"
30 #include "tgcreator/tgUtil.h"
31 
32 #include <json/json.h>
33 
34 AppGoalTension::AppGoalTension(int argc, char** argv)
35 {
36  bSetup = false;
37  use_graphics = false;
38  add_controller = true;
39  add_blocks = false;
40  add_hills = false;
41  all_terrain = false;
42  timestep_physics = 1.0f/1000.0f;
43  timestep_graphics = 1.0f/60.0f;
44  nEpisodes = 1;
45  nSteps = 60000;
46  nSegments = 6;
47  nTypes = 3;
48 
49  startX = 0;
50  startY = 20;
51  startZ = 0;
52  startAngle = 0;
53  goalAngle = 0;
54 
55  suffix = "default";
56 
57  handleOptions(argc, argv);
58 
59  tgUtil::seedRandom();
60 }
61 
63 {
64  // First create the world
65  world = createWorld();
66 
67  // Second create the view
68  if (use_graphics)
69  view = createGraphicsView(world); // For visual experimenting on one tensegrity
70  else
71  view = createView(world); // For running multiple episodes
72 
73  // Third create the simulation
74  simulation = new tgSimulation(*view);
75 
76  // Fourth create the models with their controllers and add the models to the
77  // simulation
78 #if (0)
79  startAngle = ((rand() / (double)RAND_MAX) - 0.5) * 3.1415;
80 #endif
81  FlemonsSpineModelGoal* myModel =
82  new FlemonsSpineModelGoal(nSegments, goalAngle, startAngle);
83 
84  // Fifth create the controllers, attach to model
85  if (add_controller)
86  {
87  Json::Value root; // will contains the root value after parsing.
88  Json::Reader reader;
89 
90  std::string resourcePath = "bmirletz/TC_Tension/";
91  std::string controlFilePath = FileHelpers::getResourcePath(resourcePath);
92  std::string controlFilename = controlFilePath + suffix;
93 
94  bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
95  if ( !parsingSuccessful )
96  {
97  // report to the user the failure and their locations in the document.
98  std::cout << "Failed to parse configuration\n"
99  << reader.getFormattedErrorMessages();
100  throw std::invalid_argument("Bad filename for JSON");
101  }
102  // Get the value of the member of root named 'encoding', return 'UTF-8' if there is no
103  // such member.
104  Json::Value impedenceVals = root.get("impedenceVals", "UTF-8");
105  impedenceVals = impedenceVals.get("params", "UTF-8");
106 
107  // Keep drilling if necessary
108  if (impedenceVals[0].isArray())
109  {
110  impedenceVals = impedenceVals[0];
111  }
112 
113  const double impedanceMax = 2000.0;
114 
115  const int segmentSpan = 3;
116  const int numMuscles = 8;
117  const int numParams = 2;
118  const int segNumber = 5; // For learning results
119  const double controlTime = .01;
120  const double lowPhase = -1 * M_PI;
121  const double highPhase = M_PI;
122  const double lowAmplitude = 0.0;
123  const double highAmplitude = 300.0;
124  // JSONCPP's .get really wants this to be typed...
125  int j = 0;
126  const double kt = impedanceMax * (impedenceVals.get(j, 0.0)).asDouble();
127  const double kp = impedanceMax * (impedenceVals.get(1, 0.0)).asDouble();
128  const double kv = impedanceMax * (impedenceVals.get(2, 0.0)).asDouble();
129  const bool def = true;
130 
131  // Overridden by def being true
132  const double cl = 10.0;
133  const double lf = 0.0;
134  const double hf = 30.0;
135 
136  // Feedback parameters
137  const double ffMin = -10.0;
138  const double ffMax = 10.0;
139  const double afMin = 0.0;
140  const double afMax = 200.0;
141  const double pfMin = 0.0;
142  const double pfMax = 0.0;
143  const double tensionFeedback = impedanceMax *(impedenceVals.get(3, 0.0)).asDouble();
144 
145  JSONGoalControl::Config control_config(segmentSpan,
146  numMuscles,
147  numMuscles,
148  numParams,
149  segNumber,
150  controlTime,
151  lowAmplitude,
152  highAmplitude,
153  lowPhase,
154  highPhase,
155  kt,
156  kp,
157  kv,
158  def,
159  cl,
160  lf,
161  hf,
162  ffMin,
163  ffMax,
164  afMin,
165  afMax,
166  pfMin,
167  pfMax,
168  tensionFeedback
169  );
170 
172  JSONGoalTension* const myControl =
173  new JSONGoalTension(control_config, suffix, resourcePath);
174 
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 AppGoalTension::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  ("goal_angle,B", po::value<double>(&goalAngle), "Angle of starting rotation for goal box. Degrees. Default = 0")
212  ("learning_controller,l", po::value<std::string>(&suffix), "Which learned controller to write to or use. Default = default")
213  ;
214 
215  po::variables_map vm;
216  po::store(po::parse_command_line(argc, argv, desc), vm);
217 
218  if (vm.count("help"))
219  {
220  std::cout << desc << "\n";
221  exit(0);
222  }
223 
224  po::notify(vm);
225 
226  if (vm.count("phys_time"))
227  {
228  timestep_physics = 1/vm["phys_time"].as<double>();
229  std::cout << "Physics timestep set to: " << timestep_physics << " seconds.\n";
230  }
231 
232  if (vm.count("graph_time"))
233  {
234  timestep_graphics = 1/vm["graph_time"].as<double>();
235  std::cout << "Graphics timestep set to: " << timestep_graphics << " seconds.\n";
236  }
237 }
238 
239 const tgHillyGround::Config AppGoalTension::getHillyConfig()
240 {
241  btVector3 eulerAngles = btVector3(0.0, 0.0, 0.0);
242  btScalar friction = 0.5;
243  btScalar restitution = 0.0;
244  // Size doesn't affect hilly terrain
245  btVector3 size = btVector3(0.0, 0.1, 0.0);
246  btVector3 origin = btVector3(0.0, 0.0, 0.0);
247  size_t nx = 240;
248  size_t ny = 240;
249  double margin = 0.5;
250  double triangleSize = 4.0;
251  double waveHeight = 2.0;
252  double offset = 0.0;
253  const tgHillyGround::Config hillGroundConfig(eulerAngles, friction, restitution,
254  size, origin, nx, ny, margin, triangleSize,
255  waveHeight, offset);
256  return hillGroundConfig;
257 }
258 
259 const tgBoxGround::Config AppGoalTension::getBoxConfig()
260 {
261  const double yaw = 0.0;
262  const double pitch = 0.0;
263  const double roll = 0.0;
264  const double friction = 0.5;
265  const double restitution = 0.0;
266  const btVector3 size(1000.0, 1.5, 1000.0);
267 
268  const tgBoxGround::Config groundConfig(btVector3(yaw, pitch, roll),
269  friction,
270  restitution,
271  size );
272 
273  return groundConfig;
274 }
275 
276 tgModel* AppGoalTension::getBlocks()
277 {
278  // Room to add a config
279  tgBlockField* myObstacle = new tgBlockField();
280  return myObstacle;
281 }
282 
283 tgWorld* AppGoalTension::createWorld()
284 {
285  const tgWorld::Config config(
286  981 // gravity, cm/sec^2
287  );
288 
289  tgBulletGround* ground;
290 
291  if (add_hills)
292  {
293  const tgHillyGround::Config hillGroundConfig = getHillyConfig();
294  ground = new tgHillyGround(hillGroundConfig);
295  }
296  else
297  {
298  const tgBoxGround::Config groundConfig = getBoxConfig();
299  ground = new tgBoxGround(groundConfig);
300  }
301 
302  return new tgWorld(config, ground);
303 }
304 
305 tgSimViewGraphics *AppGoalTension::createGraphicsView(tgWorld *world)
306 {
307  return new tgSimViewGraphics(*world, timestep_physics, timestep_graphics);
308 }
309 
310 tgSimView *AppGoalTension::createView(tgWorld *world)
311 {
312  return new tgSimView(*world, timestep_physics, timestep_graphics);
313 }
314 
316 {
317  if (!bSetup)
318  {
319  setup();
320  }
321 
322  if (use_graphics)
323  {
324  // Run until the user stops
325  simulation->run();
326  }
327  else
328  {
329  // or run for a specific number of steps
330  simulate(simulation);
331  }
332 
334  delete simulation;
335  delete view;
336  delete world;
337 
338  return true;
339 }
340 
341 void AppGoalTension::simulate(tgSimulation *simulation)
342 {
343  for (int i=0; i<nEpisodes; i++) {
344  fprintf(stderr,"Episode %d\n", i);
345  try
346  {
347  simulation->run(nSteps);
348  }
349  catch (std::runtime_error e)
350  {
351  // Nothing to do here, score will be set to -1
352  }
353 
354  // Don't change the terrain before the last episode to avoid leaks
355  if (i != nEpisodes - 1)
356  {
357 
358  if (all_terrain)
359  {
360  // Next run has Hills
361  if (i % nTypes == 0)
362  {
363 
364  const tgHillyGround::Config hillGroundConfig = getHillyConfig();
365  tgBulletGround* ground = new tgHillyGround(hillGroundConfig);
366  simulation->reset(ground);
367  }
368  // Flat
369  else if (i % nTypes == 1)
370  {
371  const tgBoxGround::Config groundConfig = getBoxConfig();
372  tgBulletGround* ground = new tgBoxGround(groundConfig);
373  simulation->reset(ground);
374  }
375  // Flat with blocks
376  else if (i % nTypes == 2)
377  {
378  simulation->reset();
379  tgModel* obstacle = getBlocks();
380  simulation->addObstacle(obstacle);
381  }
382  }
383  else if(add_blocks)
384  {
385  simulation->reset();
386  tgModel* obstacle = getBlocks();
387  simulation->addObstacle(obstacle);
388  }
389  // Avoid resetting twice on the last run
390  else
391  {
392  simulation->reset();
393  }
394  }
395  }
396 }
397 
404 int main(int argc, char** argv)
405 {
406  std::cout << "AppGoalTension" << std::endl;
407  AppGoalTension app (argc, argv);
408 
409  if (app.setup())
410  app.run();
411 
412  //Teardown is handled by delete, so that should be automatic
413  return 0;
414 }
415 
void addObstacle(tgModel *pObstacle)
void addModel(tgModel *pModel)
A series of functions to assist with file input/output.
static std::string getResourcePath(std::string relPath)
Definition: FileHelpers.cpp:40
int main(int argc, char **argv)
void run() const
Rand seeding simular to the evolution and terrain classes.
void attach(tgObserver< T > *pObserver)
Definition: tgSubject.h:91