void control_state(state *my_states, all_times *all_my_times)
{
switch (my_states->main)
{
case CONFIG_STATE:
control_config(my_states, all_my_times);
break;
case DELAY_STATE:
break;
case SHOOTING_STATE:
break;
case SHOOTING_DONE:
break;
default:
my_states->main = 0;
break;
}
}
bool AppTSGoal::setup()
{
// First create the world
const double scale = 100;
world = createWorld();
// Second create the view
if (use_graphics)
view = createGraphicsView(world); // For visual experimenting on one tensegrity
else
view = createView(world); // For running multiple episodes
// Third create the simulation
simulation = new tgSimulation(*view);
// Fourth create the models with their controllers and add the models to the
// simulation
/// @todo add position and angle to configuration
TetraSpineGoal* myModel =
new TetraSpineGoal(nSegments, 0.0, scale);
// Fifth create the controllers, attach to model
if (add_controller)
{
const int segmentSpan = 3;
const int numMuscles = 6;
const int numParams = 2;
const int segNumber = 5; // For learning results
const double controlTime = .01;
const double lowPhase = -1 * M_PI;
const double highPhase = M_PI;
const double lowAmplitude = 0.0;
const double highAmplitude = 300.0;
const double kt = 0.0;
const double kp = 1000.0;
const double kv = 200.0;
const bool def = true;
// Overridden by def being true
const double cl = 10.0;
const double lf = 0.0;
const double hf = 30.0;
// Feedback parameters
const double ffMin = -0.5;
const double ffMax = 10.0;
const double afMin = 0.0;
const double afMax = 200.0;
const double pfMin = -0.5;
const double pfMax = 6.28;
const double tensionFeedback = 1000.0;
#if (0) // Switch for .nnw or JSON based methods
JSONGoalControl::Config control_config(segmentSpan,
numMuscles,
numMuscles,
numParams,
segNumber,
controlTime,
lowAmplitude,
highAmplitude,
lowPhase,
highPhase,
kt,
kp,
kv,
def,
cl,
lf,
hf,
ffMin,
ffMax,
afMin,
afMax,
pfMin,
pfMax,
tensionFeedback
);
/// @todo fix memory leak that occurs here
JSONGoalControl* const myControl =
new JSONGoalControl(control_config, suffix, "bmirletz/Tetra_Goal/");
#else
JSONFeedbackControl::Config control_config(segmentSpan,
numMuscles,
numMuscles,
numParams,
segNumber,
controlTime,
lowAmplitude,
highAmplitude,
lowPhase,
highPhase,
kt,
kp,
kv,
def,
cl,
lf,
hf,
ffMin,
ffMax,
afMin,
afMax,
pfMin,
pfMax);
/// @todo fix memory leak that occurs here
JSONFeedbackControl * const myControl =
new JSONFeedbackControl(control_config, suffix, "bmirletz/Tetra_Goal/");
#endif
myModel->attach(myControl);
}
// Sixth add model & controller to simulation
simulation->addModel(myModel);
if (add_blocks)
{
tgModel* blockField = getBlocks();
simulation->addObstacle(blockField);
}
bSetup = true;
return bSetup;
}
bool AppGoalTension::setup()
{
// First create the world
world = createWorld();
// Second create the view
if (use_graphics)
view = createGraphicsView(world); // For visual experimenting on one tensegrity
else
view = createView(world); // For running multiple episodes
// Third create the simulation
simulation = new tgSimulation(*view);
// Fourth create the models with their controllers and add the models to the
// simulation
#if (0)
startAngle = ((rand() / (double)RAND_MAX) - 0.5) * 3.1415;
#endif
FlemonsSpineModelGoal* myModel =
new FlemonsSpineModelGoal(nSegments, goalAngle, startAngle);
// Fifth create the controllers, attach to model
if (add_controller)
{
Json::Value root; // will contains the root value after parsing.
Json::Reader reader;
std::string resourcePath = "bmirletz/TC_Tension/";
std::string controlFilePath = FileHelpers::getResourcePath(resourcePath);
std::string controlFilename = controlFilePath + suffix;
bool parsingSuccessful = reader.parse( FileHelpers::getFileString(controlFilename.c_str()), root );
if ( !parsingSuccessful )
{
// report to the user the failure and their locations in the document.
std::cout << "Failed to parse configuration\n"
<< reader.getFormattedErrorMessages();
throw std::invalid_argument("Bad filename for JSON");
}
// Get the value of the member of root named 'encoding', return 'UTF-8' if there is no
// such member.
Json::Value impedenceVals = root.get("impedenceVals", "UTF-8");
impedenceVals = impedenceVals.get("params", "UTF-8");
// Keep drilling if necessary
if (impedenceVals[0].isArray())
{
impedenceVals = impedenceVals[0];
}
const double impedanceMax = 2000.0;
const int segmentSpan = 3;
const int numMuscles = 8;
const int numParams = 2;
const int segNumber = 5; // For learning results
const double controlTime = .01;
const double lowPhase = -1 * M_PI;
const double highPhase = M_PI;
const double lowAmplitude = 0.0;
const double highAmplitude = 300.0;
// JSONCPP's .get really wants this to be typed...
int j = 0;
const double kt = impedanceMax * (impedenceVals.get(j, 0.0)).asDouble();
const double kp = impedanceMax * (impedenceVals.get(1, 0.0)).asDouble();
const double kv = impedanceMax * (impedenceVals.get(2, 0.0)).asDouble();
const bool def = true;
// Overridden by def being true
const double cl = 10.0;
const double lf = 0.0;
const double hf = 30.0;
// Feedback parameters
const double ffMin = -10.0;
const double ffMax = 10.0;
const double afMin = 0.0;
const double afMax = 200.0;
const double pfMin = 0.0;
const double pfMax = 0.0;
const double tensionFeedback = impedanceMax *(impedenceVals.get(3, 0.0)).asDouble();
JSONGoalControl::Config control_config(segmentSpan,
numMuscles,
numMuscles,
numParams,
segNumber,
controlTime,
lowAmplitude,
highAmplitude,
lowPhase,
highPhase,
kt,
kp,
kv,
def,
cl,
lf,
hf,
ffMin,
ffMax,
afMin,
afMax,
pfMin,
pfMax,
tensionFeedback
);
/// @todo fix memory leak that occurs here
JSONGoalTension* const myControl =
new JSONGoalTension(control_config, suffix, resourcePath);
myModel->attach(myControl);
}
// Sixth add model & controller to simulation
simulation->addModel(myModel);
if (add_blocks)
{
tgModel* blockField = getBlocks();
simulation->addObstacle(blockField);
}
bSetup = true;
return bSetup;
}
/**
* The entry point.
* @param[in] argc the number of command-line arguments
* @param[in] argv argv[0] is the executable name; argv[1], if supplied, is the
* suffix for the controller
* @return 0
*/
int main(int argc, char** argv)
{
std::cout << "AppTetraSpineHardwareLearning" << std::endl;
// First create the world
const tgWorld::Config config(981); // gravity, cm/sec^2
tgWorld world(config);
// Second create the view
const double stepSize = 1.0/1000.0; // Seconds
const double renderRate = 1.0/60.0; // Seconds
tgSimView view(world, stepSize, renderRate);
// Third create the simulation
tgSimulation simulation(view);
// Fourth create the models with their controllers and add the models to the
// simulation
const int segments = 3;
TetraSpineStaticModel_hf* myModel =
new TetraSpineStaticModel_hf(segments);
/* Required for setting up learning file input/output. */
const std::string suffix((argc > 1) ? argv[1] : "default");
const int segmentSpan = 3;
const int numMuscles = 6;
const int numParams = 2;
const int segment = 1;
const double controlTime = .001;
BaseSpineCPGControl::Config control_config(segmentSpan, numMuscles, numMuscles, numParams, segment, controlTime);
LearningSpineJSON* const myControl =
new LearningSpineJSON(control_config, suffix);
myModel->attach(myControl);
tgCPGLogger* const myLogger =
new tgCPGLogger("logs/CPGValues.txt");
myControl->attach(myLogger);
simulation.addModel(myModel);
int i = 0;
while (i < 1)
{
simulation.run(60000);
simulation.reset();
i++;
}
/// @todo Does the model assume ownership of the controller?
/** No - a single controller could be attached to multiple subjects
* However, having this here causes a segfault, since there is a call
* to onTeardown() when the simulation is deleted
*/
#if (0)
delete myControl;
#endif
//Teardown is handled by delete, so that should be automatic
return 0;
}
/**
* The entry point.
* @param[in] argc the number of command-line arguments
* @param[in] argv argv[0] is the executable name; argv[1], if supplied, is the
* suffix for the controller
* @return 0
*/
int main(int argc, char** argv)
{
std::cout << "AppFlemonsSpineContact" << std::endl;
// First create the world
const tgWorld::Config config(981); // gravity, cm/sec^2
#if (1)
btVector3 eulerAngles = btVector3(0.0, 0.0, 0.0);
btScalar friction = 0.5;
btScalar restitution = 0.0;
btVector3 size = btVector3(500.0, 0.5, 500.0);
btVector3 origin = btVector3(0.0, 0.0, 0.0);
size_t nx = 100;
size_t ny = 100;
double margin = 0.5;
double triangleSize = 5.0;
double waveHeight = 3.0;
double offset = 0.0;
tgHillyGround::Config groundConfig(eulerAngles, friction, restitution,
size, origin, nx, ny, margin, triangleSize,
waveHeight, offset);
tgHillyGround* ground = new tgHillyGround(groundConfig);
tgWorld world(config, ground);
#else
tgWorld world(config);
#endif
// Second create the view
const double stepSize = 1.0/1000.0; // Seconds
const double renderRate = 1.0/60.0; // Seconds
tgSimView view(world, stepSize, renderRate);
// Third create the simulation
tgSimulation simulation(view);
// Fourth create the models with their controllers and add the models to the
// simulation
const int segments = 6;
FlemonsSpineModelContact* myModel =
new FlemonsSpineModelContact(segments);
/* Required for setting up learning file input/output. */
const std::string suffix((argc > 1) ? argv[1] : "default");
const int segmentSpan = 3;
const int numMuscles = 8;
const int numParams = 2;
const int segNumber = 0; // For learning results
const double controlTime = .01;
const double lowPhase = -1 * M_PI;
const double highPhase = M_PI;
const double lowAmplitude = 0.0;
const double highAmplitude = 300.0;
const double kt = 0.0;
const double kp = 1000.0;
const double kv = 200.0;
const bool def = true;
// Overridden by def being true
const double cl = 10.0;
const double lf = 0.0;
const double hf = 30.0;
// Feedback parameters
const double ffMin = -0.5;
const double ffMax = 10.0;
const double afMin = 0.0;
const double afMax = 200.0;
const double pfMin = -0.5;
const double pfMax = 6.28;
SpineFeedbackControl::Config control_config(segmentSpan,
numMuscles,
numMuscles,
numParams,
segNumber,
controlTime,
lowAmplitude,
highAmplitude,
lowPhase,
highPhase,
kt,
kp,
kv,
def,
cl,
lf,
hf,
ffMin,
ffMax,
afMin,
afMax,
pfMin,
pfMax
);
SpineFeedbackControl* const myControl =
new SpineFeedbackControl(control_config, suffix, "bmirletz/TetrahedralComplex_Contact/");
myModel->attach(myControl);
simulation.addModel(myModel);
int i = 0;
while (i < 30000)
{
try
{
simulation.run(30000);
simulation.reset();
i++;
}
catch (std::runtime_error e)
{
simulation.reset();
}
}
/// @todo Does the model assume ownership of the controller?
/** No - a single controller could be attached to multiple subjects
* However, having this here causes a segfault, since there is a call
* to onTeardown() when the simulation is deleted
*/
#if (0)
delete myControl;
#endif
//Teardown is handled by delete, so that should be automatic
return 0;
}
bool AppQuadControl::setup()
{
// First create the world
world = createWorld();
// Second create the view
if (use_graphics)
view = createGraphicsView(world); // For visual experimenting on one tensegrity
else
view = createView(world); // For running multiple episodes
// Third create the simulation
simulation = new tgSimulation(*view);
// Fourth create the models with their controllers and add the models to the
// simulation
/// @todo add position and angle to configuration
//FlemonsSpineModelContact* myModel =
//new FlemonsSpineModelContact(nSegments);
//Parameters for the structure:
const int segments = 7;
const int hips = 4;
const int legs = 4;
const int feet = 4;
BigPuppySymmetric* myModel = new BigPuppySymmetric(segments, hips, legs, feet);
// Fifth create the controllers, attach to model
if (add_controller)
{
const int segmentSpan = 3; //Not sure what this will be for mine!
const int numMuscles = 8; //This may be ok, but confirm.
const int numParams = 2;
const int segNumber = 0; // For learning results
const double controlTime = .01;
const double lowPhase = -1 * M_PI;
const double highPhase = M_PI;
const double lowAmplitude = 0.0;
const double highAmplitude = 300.0;
const double kt = 0.0; //May need to retune kt, kp, and kv
const double kp = 1000.0;
const double kv = 200.0;
const bool def = true;
// Overridden by def being true
const double cl = 10.0;
const double lf = 0.0;
const double hf = 30.0;
// Feedback parameters... may need to retune
const double ffMin = -0.5;
const double ffMax = 10.0;
const double afMin = 0.0;
const double afMax = 200.0;
const double pfMin = -0.5;
const double pfMax = 6.28;
const double maxH = 60.0;
const double minH = 1.0;
JSONQuadFeedbackControl::Config control_config(segmentSpan,
numMuscles,
numMuscles,
numParams,
segNumber,
controlTime,
lowAmplitude,
highAmplitude,
lowPhase,
highPhase,
kt,
kp,
kv,
def,
cl,
lf,
hf,
ffMin,
ffMax,
afMin,
afMax,
pfMin,
pfMax,
maxH,
minH);
/// @todo fix memory leak that occurs here
JSONQuadFeedbackControl* const myControl =
new JSONQuadFeedbackControl(control_config, suffix, lowerPath);
#if (0)
tgCPGJSONLogger* const myLogger =
new tgCPGJSONLogger("logs/CPGValues.txt");
myControl->attach(myLogger);
#endif
myModel->attach(myControl);
}
// Sixth add model & controller to simulation
simulation->addModel(myModel);
if (add_blocks)
{
tgModel* blockField = getBlocks();
simulation->addObstacle(blockField);
}
bSetup = true;
return bSetup;
}
/**
* The entry point.
* @param[in] argc the number of command-line arguments
* @param[in] argv argv[0] is the executable name; argv[1], if supplied, is the
* suffix for the controller
* @return 0
*/
int main(int argc, char** argv)
{
std::cout << "AppNestedStructureTest" << std::endl;
// First create the world
const tgWorld::Config config(981); // gravity, cm/sec^2
tgWorld world(config);
// Second create the view
const double stepSize = 1.0/1000.0; // Seconds
const double renderRate = 1.0/60.0; // Seconds
tgSimViewGraphics view(world, stepSize, renderRate);
// Third create the simulation
tgSimulation simulation(view);
// Fourth create the models with their controllers and add the models to the
// simulation
const int segments = 12;
FlemonsSpineModelLearning* myModel =
new FlemonsSpineModelLearning(segments);
/* Required for setting up learning file input/output. */
const std::string suffix((argc > 1) ? argv[1] : "default");
const int segmentSpan = 3;
const int numMuscles = 8;
const int numParams = 2;
const int segNumber = 6; // For learning results
const double controlTime = .001;
const double lowPhase = -1 * M_PI;
const double highPhase = M_PI;
const double lowAmplitude = -30.0;
const double highAmplitude = 30.0;
const double kt = 0.0;
const double kp = 1000.0;
const double kv = 210.0;
const bool def = true;
// Overridden by def being true
const double cl = 10.0;
const double lf = -30.0;
const double hf = 30.0;
BaseSpineCPGControl::Config control_config(segmentSpan,
numMuscles,
numMuscles,
numParams,
segNumber,
controlTime,
lowAmplitude,
highAmplitude,
lowPhase,
highPhase,
kt,
kp,
kv,
def,
cl,
lf,
hf
);
KinematicSpineCPGControl* const myControl =
new KinematicSpineCPGControl(control_config, suffix, "learningSpines/TetrahedralComplex/");
myModel->attach(myControl);
#if (0)
tgCPGLogger* const myLogger =
new tgCPGLogger("logs/CPGValues.txt");
myControl->attach(myLogger);
#endif
simulation.addModel(myModel);
int i = 0;
while (i < 1)
{
simulation.run(30000);
#ifdef BT_USE_DOUBLE_PRECISION
std::cout << "Double precision" << std::endl;
#else
std::cout << "Single Precision" << std::endl;
#endif
simulation.reset();
i++;
}
/// @todo Does the model assume ownership of the controller?
/** No - a single controller could be attached to multiple subjects
* However, having this here causes a segfault, since there is a call
* to onTeardown() when the simulation is deleted
*/
#if (0)
delete myControl;
#endif
//Teardown is handled by delete, so that should be automatic
return 0;
}
bool AppMultiTerrain_OC::setup()
{
// First create the world
world = createWorld();
// Second create the view
if (use_graphics)
view = createGraphicsView(world); // For visual experimenting on one tensegrity
else
view = createView(world); // For running multiple episodes
// Third create the simulation
simulation = new tgSimulation(*view);
// Fourth create the models with their controllers and add the models to the
// simulation
// TODO properly add this to the header info and learning apparatus
double goalAngle = -M_PI / 2.0;
/// @todo add position and angle to configuration
OctahedralComplex* myModel =
new OctahedralComplex(nSegments, goalAngle);
// Fifth create the controllers, attach to model
if (add_controller)
{
const int segmentSpan = 3;
const int numMuscles = 4;
const int numParams = 2;
const int segNumber = 0; // For learning results
const double controlTime = .01;
const double lowPhase = -1 * M_PI;
const double highPhase = M_PI;
const double lowAmplitude = 0.0;
const double highAmplitude = 300.0;
const double kt = 0.0;
const double kp = 1000.0;
const double kv = 210.0;
const bool def = true;
// Overridden by def being true
const double cl = 10.0;
const double lf = 0.0;
const double hf = 30.0;
// Feedback parameters
const double ffMin = -0.5;
const double ffMax = 10.0;
const double afMin = 0.0;
const double afMax = 200.0;
const double pfMin = -0.5;
const double pfMax = 6.28;
const double tensionFeedback = 1000.0;
#if (1)
JSONGoalControl::Config control_config(segmentSpan,
numMuscles,
numMuscles,
numParams,
segNumber,
controlTime,
lowAmplitude,
highAmplitude,
lowPhase,
highPhase,
kt,
kp,
kv,
def,
cl,
lf,
hf,
ffMin,
ffMax,
afMin,
afMax,
pfMin,
pfMax,
tensionFeedback
);
/// @todo fix memory leak that occurs here
OctahedralGoalControl* const myControl =
new OctahedralGoalControl(control_config, suffix, "bmirletz/OctaCL_6/");
#else
SpineFeedbackControl::Config control_config(segmentSpan,
numMuscles,
numMuscles,
numParams,
segNumber,
controlTime,
lowAmplitude,
highAmplitude,
lowPhase,
highPhase,
kt,
kp,
kv,
def,
cl,
lf,
hf,
ffMin,
ffMax,
afMin,
afMax,
pfMin,
pfMax);
SpineFeedbackControl* const myControl =
new SpineFeedbackControl(control_config, suffix, "bmirletz/OctaCL_6/");
#endif
myModel->attach(myControl);
}
// Sixth add model & controller to simulation
simulation->addModel(myModel);
if (add_blocks)
{
tgModel* blockField = getBlocks();
simulation->addObstacle(blockField);
}
bSetup = true;
return bSetup;
}
bool AppGoalOnline::setup()
{
// First create the world
world = createWorld();
// Second create the view
if (use_graphics)
view = createGraphicsView(world); // For visual experimenting on one tensegrity
else
view = createView(world); // For running multiple episodes
// Third create the simulation
simulation = new tgSimulation(*view);
// Fourth create the models with their controllers and add the models to the
// simulation
/// @todo add position and angle to configuration
/// @todo Generalize angle code
FlemonsSpineModelGoal* myModel =
new FlemonsSpineModelGoal(nSegments, 0.0);
// Fifth create the controllers, attach to model
if (add_controller)
{
const int segmentSpan = 3;
const int numMuscles = 8;
const int numParams = 2;
const int segNumber = 0; // For learning results
const double controlTime = .01;
const double lowPhase = -1 * M_PI;
const double highPhase = M_PI;
const double lowAmplitude = 0.0;
const double highAmplitude = 300.0;
const double kt = 0.0;
const double kp = 1000.0;
const double kv = 200.0;
const bool def = true;
// Overridden by def being true
const double cl = 10.0;
const double lf = 0.0;
const double hf = 30.0;
// Feedback parameters
const double ffMin = -0.5;
const double ffMax = 10.0;
const double afMin = 0.0;
const double afMax = 200.0;
const double pfMin = -0.5;
const double pfMax = 6.28;
const double tensionFeedback = 1000.0;
// How often to check/change the controller during online learning
const double feedbackTime = 3.0;
SpineOnlineControl::Config control_config(segmentSpan,
numMuscles,
numMuscles,
numParams,
segNumber,
controlTime,
lowAmplitude,
highAmplitude,
lowPhase,
highPhase,
kt,
kp,
kv,
def,
cl,
lf,
hf,
ffMin,
ffMax,
afMin,
afMax,
pfMin,
pfMax,
tensionFeedback,
feedbackTime
);
/// @todo fix memory leak that occurs here
SpineOnlineControl* const myControl =
new SpineOnlineControl(control_config, suffix, "bmirletz/TetrahedralComplex_Online/");
myModel->attach(myControl);
}
// Sixth add model & controller to simulation
simulation->addModel(myModel);
if (add_blocks)
{
tgModel* blockField = getBlocks();
simulation->addObstacle(blockField);
}
bSetup = true;
return bSetup;
}
