-
Notifications
You must be signed in to change notification settings - Fork 81
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Another controller idea, with hips/legs learning different parameter …
…set from spine segments. Just saving a baseline for now, will make changes Wed/Thurs.
- Loading branch information
1 parent
6fb45a7
commit 0b36db4
Showing
6 changed files
with
1,219 additions
and
0 deletions.
There are no files selected for viewing
385 changes: 385 additions & 0 deletions
385
src/dev/dhustigschultz/BP_SC_SymmetricSpiral_Segments/AppQuadControlSegments.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,385 @@ | ||
/* | ||
* Copyright © 2012, United States Government, as represented by the | ||
* Administrator of the National Aeronautics and Space Administration. | ||
* All rights reserved. | ||
* | ||
* The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed | ||
* under the Apache License, Version 2.0 (the "License"); | ||
* you may not use this file except in compliance with the License. | ||
* You may obtain a copy of the License at | ||
* http://www.apache.org/licenses/LICENSE-2.0. | ||
* | ||
* Unless required by applicable law or agreed to in writing, | ||
* software distributed under the License is distributed on an | ||
* "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, | ||
* either express or implied. See the License for the specific language | ||
* governing permissions and limitations under the License. | ||
*/ | ||
|
||
/** | ||
* @file AppQuadControlSegments.cpp | ||
* @brief Using Brian's existing spine controller for a quadruped. | ||
* @author Dawn Hustig-Schultz, Brian Mirletz | ||
* @date Feb. 2016 | ||
* @version 1.0.0 | ||
* $Id$ | ||
*/ | ||
|
||
#include "AppQuadControlSegments.h" | ||
#include "dev/btietz/JSONTests/tgCPGJSONLogger.h" | ||
|
||
AppQuadControlSegments::AppQuadControlSegments(int argc, char** argv) | ||
{ | ||
bSetup = false; | ||
use_graphics = false; | ||
add_controller = true; | ||
add_blocks = false; | ||
add_hills = false; | ||
all_terrain = false; | ||
timestep_physics = 1.0f/1000.0f; | ||
timestep_graphics = 1.0f/60.0f; | ||
nEpisodes = 1; | ||
nSteps = 60000; | ||
nSegments = 7; | ||
nTypes = 3; | ||
|
||
startX = 0; | ||
startY = 20; //May need adjustment | ||
startZ = 0; | ||
startAngle = 0; | ||
|
||
suffix = "default"; | ||
lowerPath = "default"; | ||
|
||
handleOptions(argc, argv); | ||
} | ||
|
||
bool AppQuadControlSegments::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; | ||
|
||
BigPuppySymmetricSpiralSegments* myModel = new BigPuppySymmetricSpiralSegments(segments, hips, legs); | ||
|
||
// 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 = 16; //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 = 30.0; | ||
const double minH = 1.0; | ||
|
||
JSONSegmentsFeedbackControl::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 | ||
JSONSegmentsFeedbackControl* const myControl = | ||
new JSONSegmentsFeedbackControl(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; | ||
} | ||
|
||
void AppQuadControlSegments::handleOptions(int argc, char **argv) | ||
{ | ||
// Declare the supported options. | ||
po::options_description desc("Allowed options"); | ||
desc.add_options() | ||
("help,h", "produce help message") | ||
("graphics,G", po::value<bool>(&use_graphics), "Test using graphical view") | ||
("controller,c", po::value<bool>(&add_controller), "Attach the controller to the model.") | ||
("blocks,b", po::value<bool>(&add_blocks)->implicit_value(false), "Add a block field as obstacles.") | ||
("hills,H", po::value<bool>(&add_hills)->implicit_value(false), "Use hilly terrain.") | ||
("all_terrain,A", po::value<bool>(&all_terrain)->implicit_value(false), "Alternate through terrain types. Only works with graphics off") | ||
("phys_time,p", po::value<double>(), "Physics timestep value (Hz). Default=1000") | ||
("graph_time,g", po::value<double>(), "Graphics timestep value a.k.a. render rate (Hz). Default = 60") | ||
("episodes,e", po::value<int>(&nEpisodes), "Number of episodes to run. Default=1") | ||
("steps,s", po::value<int>(&nSteps), "Number of steps per episode to run. Default=60K (60 seconds)") | ||
("segments,S", po::value<int>(&nSegments), "Number of segments in the tensegrity spine. Default=6") | ||
("start_x,x", po::value<double>(&startX), "X Coordinate of starting position for robot. Default = 0") | ||
("start_y,y", po::value<double>(&startY), "Y Coordinate of starting position for robot. Default = 20") | ||
("start_z,z", po::value<double>(&startZ), "Z Coordinate of starting position for robot. Default = 0") | ||
("angle,a", po::value<double>(&startAngle), "Angle of starting rotation for robot. Degrees. Default = 0") | ||
("goal_angle,B", po::value<double>(&goalAngle), "Angle of starting rotation for goal box. Degrees. Default = 0") | ||
("learning_controller,l", po::value<std::string>(&suffix), "Which learned controller to write to or use. Default = default") | ||
("lower_path,P", po::value<std::string>(&lowerPath), "Which resources folder in which you want to store controllers. Default = default") | ||
; | ||
|
||
po::variables_map vm; | ||
po::store(po::parse_command_line(argc, argv, desc), vm); | ||
|
||
if (vm.count("help")) | ||
{ | ||
std::cout << desc << "\n"; | ||
exit(0); | ||
} | ||
|
||
po::notify(vm); | ||
|
||
if (vm.count("phys_time")) | ||
{ | ||
timestep_physics = 1/vm["phys_time"].as<double>(); | ||
std::cout << "Physics timestep set to: " << timestep_physics << " seconds.\n"; | ||
} | ||
|
||
if (vm.count("graph_time")) | ||
{ | ||
timestep_graphics = 1/vm["graph_time"].as<double>(); | ||
std::cout << "Graphics timestep set to: " << timestep_graphics << " seconds.\n"; | ||
} | ||
} | ||
|
||
const tgHillyGround::Config AppQuadControlSegments::getHillyConfig() | ||
{ | ||
btVector3 eulerAngles = btVector3(0.0, 0.0, 0.0); | ||
btScalar friction = 0.5; | ||
btScalar restitution = 0.0; | ||
// Size doesn't affect hilly terrain | ||
btVector3 size = btVector3(0.0, 0.1, 0.0); | ||
btVector3 origin = btVector3(0.0, 0.0, 0.0); | ||
size_t nx = 100; | ||
size_t ny = 100; | ||
double margin = 0.5; | ||
double triangleSize = 4.0; | ||
double waveHeight = 2.0; | ||
double offset = 0.0; | ||
const tgHillyGround::Config hillGroundConfig(eulerAngles, friction, restitution, | ||
size, origin, nx, ny, margin, triangleSize, | ||
waveHeight, offset); | ||
return hillGroundConfig; | ||
} | ||
|
||
const tgBoxGround::Config AppQuadControlSegments::getBoxConfig() | ||
{ | ||
const double yaw = 0.0; | ||
const double pitch = 0.0; | ||
const double roll = 0.0; | ||
const double friction = 0.5; | ||
const double restitution = 0.0; | ||
const btVector3 size(1000.0, 1.5, 1000.0); | ||
|
||
const tgBoxGround::Config groundConfig(btVector3(yaw, pitch, roll), | ||
friction, | ||
restitution, | ||
size ); | ||
|
||
return groundConfig; | ||
} | ||
|
||
tgModel* AppQuadControlSegments::getBlocks() | ||
{ | ||
// Room to add a config | ||
tgBlockField* myObstacle = new tgBlockField(); | ||
return myObstacle; | ||
} | ||
|
||
tgWorld* AppQuadControlSegments::createWorld() | ||
{ | ||
const tgWorld::Config config( | ||
981 // gravity, cm/sec^2 | ||
); | ||
|
||
tgBulletGround* ground; | ||
|
||
if (add_hills) | ||
{ | ||
const tgHillyGround::Config hillGroundConfig = getHillyConfig(); | ||
ground = new tgHillyGround(hillGroundConfig); | ||
} | ||
else | ||
{ | ||
const tgBoxGround::Config groundConfig = getBoxConfig(); | ||
ground = new tgBoxGround(groundConfig); | ||
} | ||
|
||
return new tgWorld(config, ground); | ||
} | ||
|
||
tgSimViewGraphics *AppQuadControlSegments::createGraphicsView(tgWorld *world) | ||
{ | ||
return new tgSimViewGraphics(*world, timestep_physics, timestep_graphics); | ||
} | ||
|
||
tgSimView *AppQuadControlSegments::createView(tgWorld *world) | ||
{ | ||
return new tgSimView(*world, timestep_physics, timestep_graphics); | ||
} | ||
|
||
bool AppQuadControlSegments::run() | ||
{ | ||
if (!bSetup) | ||
{ | ||
setup(); | ||
} | ||
|
||
if (use_graphics) | ||
{ | ||
// Run until the user stops | ||
simulation->run(); | ||
} | ||
else | ||
{ | ||
// or run for a specific number of steps | ||
simulate(simulation); | ||
} | ||
|
||
///@todo consider app.cleanup() | ||
delete simulation; | ||
delete view; | ||
delete world; | ||
|
||
return true; | ||
} | ||
|
||
void AppQuadControlSegments::simulate(tgSimulation *simulation) | ||
{ | ||
for (int i=0; i<nEpisodes; i++) { | ||
fprintf(stderr,"Episode %d\n", i); | ||
try | ||
{ | ||
simulation->run(nSteps); | ||
} | ||
catch (std::runtime_error e) | ||
{ | ||
// Nothing to do here, score will be set to -1 | ||
} | ||
|
||
// Don't change the terrain before the last episode to avoid leaks | ||
if (all_terrain && i != nEpisodes - 1) | ||
{ | ||
// Next run has Hills | ||
if (i % nTypes == 0) | ||
{ | ||
|
||
const tgHillyGround::Config hillGroundConfig = getHillyConfig(); | ||
tgBulletGround* ground = new tgHillyGround(hillGroundConfig); | ||
simulation->reset(ground); | ||
} | ||
// Flat | ||
else if (i % nTypes == 1) | ||
{ | ||
const tgBoxGround::Config groundConfig = getBoxConfig(); | ||
tgBulletGround* ground = new tgBoxGround(groundConfig); | ||
simulation->reset(ground); | ||
} | ||
// Flat with blocks | ||
else if (i % nTypes == 2) | ||
{ | ||
simulation->reset(); | ||
tgModel* obstacle = getBlocks(); | ||
simulation->addObstacle(obstacle); | ||
} | ||
} | ||
// Avoid resetting twice on the last run | ||
else if (i != nEpisodes - 1) | ||
{ | ||
simulation->reset(); | ||
} | ||
|
||
} | ||
} | ||
|
||
/** | ||
* The entry point. | ||
* @param[in] argc the number of command-line arguments | ||
* @param[in] argv argv[0] is the executable name | ||
* @return 0 | ||
*/ | ||
int main(int argc, char** argv) | ||
{ | ||
std::cout << "AppQuadControlSegments" << std::endl; | ||
AppQuadControlSegments app (argc, argv); | ||
|
||
if (app.setup()) | ||
app.run(); | ||
|
||
//Teardown is handled by delete, so that should be automatic | ||
return 0; | ||
} | ||
|
||
|
Oops, something went wrong.