NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
AppQuadCoupling.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 "AppQuadCoupling.h"
29 #include "dev/btietz/JSONTests/tgCPGJSONLogger.h"
30 
31 AppQuadCoupling::AppQuadCoupling(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  BigPuppySymmetricSpiral2* myModel = new BigPuppySymmetricSpiral2(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 = 16 + 10*8; // Number of muscles we're considering right now, which is 16 for spine and 10 for each hip/shoulder
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 = 70.0;
116  const double minH = 15.0;
117 
118  const double hffMin = 0.0;
119  const double hffMax = 1.0;
120 
121  JSONQuadFeedbackControl::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  maxH,
145  minH,
146  hffMin,
147  hffMax);
149  JSONQuadFeedbackControl* const myControl =
150  new JSONQuadFeedbackControl(control_config, suffix, lowerPath);
151 
152 #if (0)
153  tgCPGJSONLogger* const myLogger =
154  new tgCPGJSONLogger("logs/CPGValues.txt");
155 
156  myControl->attach(myLogger);
157 #endif
158  myModel->attach(myControl);
159  }
160 
161  // Sixth add model & controller to simulation
162  simulation->addModel(myModel);
163 
164  if (add_blocks)
165  {
166  tgModel* blockField = getBlocks();
167  simulation->addObstacle(blockField);
168  }
169 
170  bSetup = true;
171  return bSetup;
172 }
173 
174 void AppQuadCoupling::handleOptions(int argc, char **argv)
175 {
176  // Declare the supported options.
177  po::options_description desc("Allowed options");
178  desc.add_options()
179  ("help,h", "produce help message")
180  ("graphics,G", po::value<bool>(&use_graphics), "Test using graphical view")
181  ("controller,c", po::value<bool>(&add_controller), "Attach the controller to the model.")
182  ("blocks,b", po::value<bool>(&add_blocks)->implicit_value(false), "Add a block field as obstacles.")
183  ("hills,H", po::value<bool>(&add_hills)->implicit_value(false), "Use hilly terrain.")
184  ("all_terrain,A", po::value<bool>(&all_terrain)->implicit_value(false), "Alternate through terrain types. Only works with graphics off")
185  ("phys_time,p", po::value<double>(), "Physics timestep value (Hz). Default=1000")
186  ("graph_time,g", po::value<double>(), "Graphics timestep value a.k.a. render rate (Hz). Default = 60")
187  ("episodes,e", po::value<int>(&nEpisodes), "Number of episodes to run. Default=1")
188  ("steps,s", po::value<int>(&nSteps), "Number of steps per episode to run. Default=60K (60 seconds)")
189  ("segments,S", po::value<int>(&nSegments), "Number of segments in the tensegrity spine. Default=6")
190  ("start_x,x", po::value<double>(&startX), "X Coordinate of starting position for robot. Default = 0")
191  ("start_y,y", po::value<double>(&startY), "Y Coordinate of starting position for robot. Default = 20")
192  ("start_z,z", po::value<double>(&startZ), "Z Coordinate of starting position for robot. Default = 0")
193  ("angle,a", po::value<double>(&startAngle), "Angle of starting rotation for robot. Degrees. Default = 0")
194  ("goal_angle,B", po::value<double>(&goalAngle), "Angle of starting rotation for goal box. Degrees. Default = 0")
195  ("learning_controller,l", po::value<std::string>(&suffix), "Which learned controller to write to or use. Default = default")
196  ("lower_path,P", po::value<std::string>(&lowerPath), "Which resources folder in which you want to store controllers. Default = default")
197  ;
198 
199  po::variables_map vm;
200  po::store(po::parse_command_line(argc, argv, desc), vm);
201 
202  if (vm.count("help"))
203  {
204  std::cout << desc << "\n";
205  exit(0);
206  }
207 
208  po::notify(vm);
209 
210  if (vm.count("phys_time"))
211  {
212  timestep_physics = 1/vm["phys_time"].as<double>();
213  std::cout << "Physics timestep set to: " << timestep_physics << " seconds.\n";
214  }
215 
216  if (vm.count("graph_time"))
217  {
218  timestep_graphics = 1/vm["graph_time"].as<double>();
219  std::cout << "Graphics timestep set to: " << timestep_graphics << " seconds.\n";
220  }
221 }
222 
223 const tgHillyGround::Config AppQuadCoupling::getHillyConfig()
224 {
225  btVector3 eulerAngles = btVector3(0.0, 0.0, 0.0);
226  btScalar friction = 0.5;
227  btScalar restitution = 0.0;
228  // Size doesn't affect hilly terrain
229  btVector3 size = btVector3(0.0, 0.1, 0.0);
230  btVector3 origin = btVector3(0.0, 0.0, 0.0);
231  size_t nx = 100;
232  size_t ny = 100;
233  double margin = 0.5;
234  double triangleSize = 4.0;
235  double waveHeight = 2.0;
236  double offset = 0.0;
237  const tgHillyGround::Config hillGroundConfig(eulerAngles, friction, restitution,
238  size, origin, nx, ny, margin, triangleSize,
239  waveHeight, offset);
240  return hillGroundConfig;
241 }
242 
243 const tgBoxGround::Config AppQuadCoupling::getBoxConfig()
244 {
245  const double yaw = 0.0;
246  const double pitch = 0.0;
247  const double roll = 0.0;
248  const double friction = 0.5;
249  const double restitution = 0.0;
250  const btVector3 size(1000.0, 1.5, 1000.0);
251 
252  const tgBoxGround::Config groundConfig(btVector3(yaw, pitch, roll),
253  friction,
254  restitution,
255  size );
256 
257  return groundConfig;
258 }
259 
260 tgModel* AppQuadCoupling::getBlocks()
261 {
262  // Room to add a config
263  tgBlockField* myObstacle = new tgBlockField();
264  return myObstacle;
265 }
266 
267 tgWorld* AppQuadCoupling::createWorld()
268 {
269  const tgWorld::Config config(
270  981 // gravity, cm/sec^2
271  );
272 
273  tgBulletGround* ground;
274 
275  if (add_hills)
276  {
277  const tgHillyGround::Config hillGroundConfig = getHillyConfig();
278  ground = new tgHillyGround(hillGroundConfig);
279  }
280  else
281  {
282  const tgBoxGround::Config groundConfig = getBoxConfig();
283  ground = new tgBoxGround(groundConfig);
284  }
285 
286  return new tgWorld(config, ground);
287 }
288 
289 tgSimViewGraphics *AppQuadCoupling::createGraphicsView(tgWorld *world)
290 {
291  return new tgSimViewGraphics(*world, timestep_physics, timestep_graphics);
292 }
293 
294 tgSimView *AppQuadCoupling::createView(tgWorld *world)
295 {
296  return new tgSimView(*world, timestep_physics, timestep_graphics);
297 }
298 
300 {
301  if (!bSetup)
302  {
303  setup();
304  }
305 
306  if (use_graphics)
307  {
308  // Run until the user stops
309  simulation->run();
310  }
311  else
312  {
313  // or run for a specific number of steps
314  simulate(simulation);
315  }
316 
318  delete simulation;
319  delete view;
320  delete world;
321 
322  return true;
323 }
324 
325 void AppQuadCoupling::simulate(tgSimulation *simulation)
326 {
327  for (int i=0; i<nEpisodes; i++) {
328  fprintf(stderr,"Episode %d\n", i);
329  try
330  {
331  simulation->run(nSteps);
332  }
333  catch (std::runtime_error e)
334  {
335  // Nothing to do here, score will be set to -1
336  }
337 
338  // Don't change the terrain before the last episode to avoid leaks
339  if (all_terrain && i != nEpisodes - 1)
340  {
341  // Next run has Hills
342  if (i % nTypes == 0)
343  {
344 
345  const tgHillyGround::Config hillGroundConfig = getHillyConfig();
346  tgBulletGround* ground = new tgHillyGround(hillGroundConfig);
347  simulation->reset(ground);
348  }
349  // Flat
350  else if (i % nTypes == 1)
351  {
352  const tgBoxGround::Config groundConfig = getBoxConfig();
353  tgBulletGround* ground = new tgBoxGround(groundConfig);
354  simulation->reset(ground);
355  }
356  // Flat with blocks
357  else if (i % nTypes == 2)
358  {
359  simulation->reset();
360  tgModel* obstacle = getBlocks();
361  simulation->addObstacle(obstacle);
362  }
363  }
364  // Avoid resetting twice on the last run
365  else if (i != nEpisodes - 1)
366  {
367  simulation->reset();
368  }
369 
370  }
371 }
372 
379 int main(int argc, char** argv)
380 {
381  std::cout << "AppQuadCoupling" << std::endl;
382  AppQuadCoupling app (argc, argv);
383 
384  if (app.setup())
385  app.run();
386 
387  //Teardown is handled by delete, so that should be automatic
388  return 0;
389 }
390 
391 
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