27 #include "AppGoalTensionNNW.h"
32 #include "dev/btietz/JSONTests/tgCPGJSONLogger.h"
34 #include <json/json.h>
36 AppGoalTensionNNW::AppGoalTensionNNW(
int argc,
char** argv)
40 add_controller =
true;
44 timestep_physics = 1.0f/1000.0f;
45 timestep_graphics = 1.0f/60.0f;
59 handleOptions(argc, argv);
67 world = createWorld();
71 view = createGraphicsView(world);
73 view = createView(world);
81 startAngle = ((rand() / (double)RAND_MAX) - 0.5) * 3.1415;
92 std::string resourcePath =
"bmirletz/TC_nn_Tension/";
94 std::string controlFilename = controlFilePath + suffix;
96 bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
97 if ( !parsingSuccessful )
100 std::cout <<
"Failed to parse configuration\n"
101 << reader.getFormattedErrorMessages();
102 throw std::invalid_argument(
"Bad filename for JSON, check that resource path exists");
106 Json::Value impedenceVals = root.get(
"impedenceVals",
"UTF-8");
107 impedenceVals = impedenceVals.get(
"params",
"UTF-8");
110 if (impedenceVals[0].isArray())
112 impedenceVals = impedenceVals[0];
115 const double impedanceMax = 2000.0;
117 const int segmentSpan = 3;
118 const int numMuscles = 8;
119 const int numParams = 2;
120 const int segNumber = 5;
121 const double controlTime = .01;
122 const double lowPhase = -1 * M_PI;
123 const double highPhase = M_PI;
124 const double lowAmplitude = 0.0;
125 const double highAmplitude = 300.0;
128 const double kt = impedanceMax * (impedenceVals.get(j, 0.0)).asDouble();
129 const double kp = impedanceMax * (impedenceVals.get(1, 0.0)).asDouble();
130 const double kv = impedanceMax * (impedenceVals.get(2, 0.0)).asDouble();
131 const bool def =
true;
134 const double cl = 10.0;
135 const double lf = 0.0;
136 const double hf = 30.0;
139 const double ffMin = -0.5;
140 const double ffMax = 10.0;
141 const double afMin = 0.0;
142 const double afMax = 200.0;
143 const double pfMin = -0.5;
144 const double pfMax = 6.28;
145 const double tensionFeedback = impedanceMax *(impedenceVals.get(3, 0.0)).asDouble();
181 myControl->
attach(myLogger);
184 myModel->
attach(myControl);
192 tgModel* blockField = getBlocks();
200 void AppGoalTensionNNW::handleOptions(
int argc,
char **argv)
203 po::options_description desc(
"Allowed options");
205 (
"help,h",
"produce help message")
206 (
"graphics,G", po::value<bool>(&use_graphics),
"Test using graphical view")
207 (
"controller,c", po::value<bool>(&add_controller),
"Attach the controller to the model.")
208 (
"blocks,b", po::value<bool>(&add_blocks)->implicit_value(
false),
"Add a block field as obstacles.")
209 (
"hills,H", po::value<bool>(&add_hills)->implicit_value(
false),
"Use hilly terrain.")
210 (
"all_terrain,A", po::value<bool>(&all_terrain)->implicit_value(
false),
"Alternate through terrain types. Only works with graphics off")
211 (
"phys_time,p", po::value<double>(),
"Physics timestep value (Hz). Default=1000")
212 (
"graph_time,g", po::value<double>(),
"Graphics timestep value a.k.a. render rate (Hz). Default = 60")
213 (
"episodes,e", po::value<int>(&nEpisodes),
"Number of episodes to run. Default=1")
214 (
"steps,s", po::value<int>(&nSteps),
"Number of steps per episode to run. Default=60K (60 seconds)")
215 (
"segments,S", po::value<int>(&nSegments),
"Number of segments in the tensegrity spine. Default=6")
216 (
"start_x,x", po::value<double>(&startX),
"X Coordinate of starting position for robot. Default = 0")
217 (
"start_y,y", po::value<double>(&startY),
"Y Coordinate of starting position for robot. Default = 20")
218 (
"start_z,z", po::value<double>(&startZ),
"Z Coordinate of starting position for robot. Default = 0")
219 (
"angle,a", po::value<double>(&startAngle),
"Angle of starting rotation for robot. Degrees. Default = 0")
220 (
"goal_angle,B", po::value<double>(&goalAngle),
"Angle of starting rotation for goal box. Degrees. Default = 0")
221 (
"learning_controller,l", po::value<std::string>(&suffix),
"Which learned controller to write to or use. Default = default")
224 po::variables_map vm;
225 po::store(po::parse_command_line(argc, argv, desc), vm);
227 if (vm.count(
"help"))
229 std::cout << desc <<
"\n";
235 if (vm.count(
"phys_time"))
237 timestep_physics = 1/vm[
"phys_time"].as<
double>();
238 std::cout <<
"Physics timestep set to: " << timestep_physics <<
" seconds.\n";
241 if (vm.count(
"graph_time"))
243 timestep_graphics = 1/vm[
"graph_time"].as<
double>();
244 std::cout <<
"Graphics timestep set to: " << timestep_graphics <<
" seconds.\n";
250 btVector3 eulerAngles = btVector3(0.0, 0.0, 0.0);
251 btScalar friction = 0.5;
252 btScalar restitution = 0.0;
254 btVector3 size = btVector3(0.0, 0.1, 0.0);
255 btVector3 origin = btVector3(0.0, 0.0, 0.0);
259 double triangleSize = 4.0;
260 double waveHeight = 2.0;
263 size, origin, nx, ny, margin, triangleSize,
265 return hillGroundConfig;
270 const double yaw = 0.0;
271 const double pitch = 0.0;
272 const double roll = 0.0;
273 const double friction = 0.5;
274 const double restitution = 0.0;
275 const btVector3 size(1000.0, 1.5, 1000.0);
285 tgModel* AppGoalTensionNNW::getBlocks()
292 tgWorld* AppGoalTensionNNW::createWorld()
311 return new tgWorld(config, ground);
321 return new tgSimView(*world, timestep_physics, timestep_graphics);
339 simulate(simulation);
350 void AppGoalTensionNNW::simulate(
tgSimulation *simulation)
352 for (
int i=0; i<nEpisodes; i++) {
353 fprintf(stderr,
"Episode %d\n", i);
356 simulation->
run(nSteps);
358 catch (std::runtime_error e)
364 if (i != nEpisodes - 1)
375 simulation->
reset(ground);
378 else if (i % nTypes == 1)
382 simulation->
reset(ground);
385 else if (i % nTypes == 2)
388 tgModel* obstacle = getBlocks();
395 tgModel* obstacle = getBlocks();
413 int main(
int argc,
char** argv)
415 std::cout <<
"AppGoalTensionNNW" << std::endl;
void addObstacle(tgModel *pObstacle)
int main(int argc, char **argv)
void addModel(tgModel *pModel)
A series of functions to assist with file input/output.
static std::string getResourcePath(std::string relPath)
Rand seeding simular to the evolution and terrain classes.
void attach(tgObserver< T > *pObserver)