diff --git a/scripts/learning/src/MG_FirstAttempt.json b/scripts/learning/src/MG_FirstAttempt.json new file mode 100644 index 000000000..4839a55c5 --- /dev/null +++ b/scripts/learning/src/MG_FirstAttempt.json @@ -0,0 +1,106 @@ +{ + "filePrefix" : "monteOut", + "fileSuffix" : ".json", + "resourcePath" : "../../../resources/src/", + "lowerPath" : "bgigous/AppQuadCoupling/", + "executable" : "./../../../build/dev/bgigous/MG_FirstAttempt/AppQuadCoupling", + "terrain" : [[[0, 0, 0, 0.0, 10000]]], + "learningParams": + { + "trialLength" : 10000, + "numTrials" : 100, + "numGenerations" : 1, + "deterministic": 0, + "nodeVals" : + { + "learning" : true, + "startingControllers" : 0, + "monteCarlo" : true, + "numberOfStates" : 0, + "numberOfOutputs" : 5, + "numberOfInstances" : 10, + "populationSize" : 100, + "useAverage" : true, + "numberToMutate" : 2, + "numberOfChildren" : 1, + "mutationChance" : 0.5, + "mutationDev" : 0.03, + "paramMax" : 1.0, + "paramMin": 0.0, + "childMutationChance" : 0.9 + }, + "edgeVals" : + { + "learning" : true, + "startingControllers" : 0, + "monteCarlo" : true, + "numberOfStates" : 0, + "numberOfOutputs" : 2, + "numberOfInstances" : 1632, + "populationSize" : 100, + "useAverage" : true, + "numberToMutate" : 2, + "numberOfChildren" : 1, + "mutationChance" : 0.5, + "mutationDev" : 0.03, + "paramMax" : 1.0, + "paramMin": 0.0, + "childMutationChance" : 0.9 + }, + "highVals" : + { + "learning" : true, + "startingControllers" : 0, + "monteCarlo" : true, + "numberOfStates" : 0, + "numberOfOutputs" : 2, + "numberOfInstances" : 10, + "populationSize" : 100, + "useAverage" : true, + "numberToMutate" : 2, + "numberOfChildren" : 1, + "mutationChance" : 0.5, + "mutationDev" : 0.03, + "paramMax" : 1.0, + "paramMin": 0.0, + "childMutationChance" : 0.9 + }, + "hLowVals" : + { + "learning" : true, + "startingControllers" : 0, + "monteCarlo" : true, + "numberOfStates" : 0, + "numberOfOutputs" : 2, + "numberOfInstances" : 5, + "populationSize" : 100, + "useAverage" : true, + "numberToMutate" : 2, + "numberOfChildren" : 1, + "mutationChance" : 0.5, + "mutationDev" : 0.03, + "paramMax" : 1.0, + "paramMin": 0.0, + "childMutationChance" : 0.9 + }, + "feedbackVals" : + { + "learning" : true, + "startingControllers" : 0, + "monteCarlo" : true, + "numberOfStates" : 2, + "numberOfOutputs" : 3, + "numberHidden" : 4, + "numberOfInstances" : 1, + "populationSize" : 100, + "useAverage" : true, + "numberToMutate" : 15, + "numberOfChildren" : 5, + "mutationChance" : 0.5, + "mutationDev" : 0.03, + "paramMax" : 1.0, + "paramMin": -1.0, + "childMutationChance" : 0.7 + } + } +} diff --git a/src/dev/CMakeLists.txt b/src/dev/CMakeLists.txt index 17ce9aaa2..d4219fc6b 100644 --- a/src/dev/CMakeLists.txt +++ b/src/dev/CMakeLists.txt @@ -18,5 +18,6 @@ subdirs( eajung ezhu mcdaly + bgigous ) diff --git a/src/dev/bgigous/CMakeLists.txt b/src/dev/bgigous/CMakeLists.txt new file mode 100644 index 000000000..0fdb72319 --- /dev/null +++ b/src/dev/bgigous/CMakeLists.txt @@ -0,0 +1,6 @@ +Project(bgigous) + +Subdirs( + MG_FirstAttempt + MG_HT +) diff --git a/src/dev/bgigous/MG_FirstAttempt/AppQuadCoupling.cpp b/src/dev/bgigous/MG_FirstAttempt/AppQuadCoupling.cpp new file mode 100644 index 000000000..139aaec8b --- /dev/null +++ b/src/dev/bgigous/MG_FirstAttempt/AppQuadCoupling.cpp @@ -0,0 +1,391 @@ +/* + * 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 AppQuadControlSpiral.cpp + * @brief Using Brian's existing spine controller for a quadruped, using more muscles to increase torsion. + * @author Dawn Hustig-Schultz, Brandon Gigous, Brian Mirletz + * @date July 2016 + * @version 1.0.0 + * $Id$ + */ + +#include "AppQuadCoupling.h" +#include "dev/btietz/JSONTests/tgCPGJSONLogger.h" + +AppQuadCoupling::AppQuadCoupling(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 = 40; //May need adjustment + startZ = 0; + startAngle = 0; + + suffix = "default"; + lowerPath = "default"; + + handleOptions(argc, argv); +} + +bool AppQuadCoupling::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; + + BigPuppySymmetricSpiral2* myModel = new BigPuppySymmetricSpiral2(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 = 16 + 10*8; // Number of muscles we're considering right now, which is 16 for spine and 10 for each hip/shoulder + 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 = 70.0; + const double minH = 15.0; + + const double hffMin = 0.0; + const double hffMax = 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, + hffMin, + hffMax); + /// @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; +} + +void AppQuadCoupling::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(&use_graphics), "Test using graphical view") + ("controller,c", po::value(&add_controller), "Attach the controller to the model.") + ("blocks,b", po::value(&add_blocks)->implicit_value(false), "Add a block field as obstacles.") + ("hills,H", po::value(&add_hills)->implicit_value(false), "Use hilly terrain.") + ("all_terrain,A", po::value(&all_terrain)->implicit_value(false), "Alternate through terrain types. Only works with graphics off") + ("phys_time,p", po::value(), "Physics timestep value (Hz). Default=1000") + ("graph_time,g", po::value(), "Graphics timestep value a.k.a. render rate (Hz). Default = 60") + ("episodes,e", po::value(&nEpisodes), "Number of episodes to run. Default=1") + ("steps,s", po::value(&nSteps), "Number of steps per episode to run. Default=60K (60 seconds)") + ("segments,S", po::value(&nSegments), "Number of segments in the tensegrity spine. Default=6") + ("start_x,x", po::value(&startX), "X Coordinate of starting position for robot. Default = 0") + ("start_y,y", po::value(&startY), "Y Coordinate of starting position for robot. Default = 20") + ("start_z,z", po::value(&startZ), "Z Coordinate of starting position for robot. Default = 0") + ("angle,a", po::value(&startAngle), "Angle of starting rotation for robot. Degrees. Default = 0") + ("goal_angle,B", po::value(&goalAngle), "Angle of starting rotation for goal box. Degrees. Default = 0") + ("learning_controller,l", po::value(&suffix), "Which learned controller to write to or use. Default = default") + ("lower_path,P", po::value(&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(); + std::cout << "Physics timestep set to: " << timestep_physics << " seconds.\n"; + } + + if (vm.count("graph_time")) + { + timestep_graphics = 1/vm["graph_time"].as(); + std::cout << "Graphics timestep set to: " << timestep_graphics << " seconds.\n"; + } +} + +const tgHillyGround::Config AppQuadCoupling::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 AppQuadCoupling::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* AppQuadCoupling::getBlocks() +{ + // Room to add a config + tgBlockField* myObstacle = new tgBlockField(); + return myObstacle; +} + +tgWorld* AppQuadCoupling::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 *AppQuadCoupling::createGraphicsView(tgWorld *world) +{ + return new tgSimViewGraphics(*world, timestep_physics, timestep_graphics); +} + +tgSimView *AppQuadCoupling::createView(tgWorld *world) +{ + return new tgSimView(*world, timestep_physics, timestep_graphics); +} + +bool AppQuadCoupling::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 AppQuadCoupling::simulate(tgSimulation *simulation) +{ + for (int i=0; irun(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 << "AppQuadCoupling" << std::endl; + AppQuadCoupling app (argc, argv); + + if (app.setup()) + app.run(); + + //Teardown is handled by delete, so that should be automatic + return 0; +} + + diff --git a/src/dev/bgigous/MG_FirstAttempt/AppQuadCoupling.h b/src/dev/bgigous/MG_FirstAttempt/AppQuadCoupling.h new file mode 100644 index 000000000..f8848758a --- /dev/null +++ b/src/dev/bgigous/MG_FirstAttempt/AppQuadCoupling.h @@ -0,0 +1,119 @@ +/* + * 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. +*/ + +#ifndef APP_QUAD_CONTROL_SPIRAL2_H +#define APP_QUAD_CONTROL_SPIRAL2_H + +/** + * @file AppQuadCoupling.cpp + * @brief Contains the definition function main() for the Multiple terrains app, used here for control of a quadruped. + * @author Brian Mirletz, Alexander Xydes, Dawn Hustig-Schultz, Brandon Gigous + * $Id$ + */ + +//robot +#include "dev/dhustigschultz/BigPuppy_SymmetricSpiral2/BigPuppySymmetricSpiral2.h" + +// controller +#include "dev/bgigous/MG_FirstAttempt/JSONQuadFeedbackControl.h" + +// obstacles +#include "models/obstacles/tgBlockField.h" + +// This library +#include "core/tgModel.h" +#include "core/tgSubject.h" +#include "core/tgSimViewGraphics.h" +#include "core/tgSimulation.h" +#include "core/tgWorld.h" +#include "core/terrain/tgBoxGround.h" +#include "core/terrain/tgHillyGround.h" + +// Boost +#include + +// The C++ Standard Library +#include +#include + +namespace po = boost::program_options; + +class AppQuadCoupling +{ +public: + AppQuadCoupling(int argc, char** argv); + + /** Setup the simulation */ + bool setup(); + /** Run the simulation */ + bool run(); + +private: + /** Parse command line options */ + void handleOptions(int argc, char** argv); + + const tgHillyGround::Config getHillyConfig(); + + const tgBoxGround::Config getBoxConfig(); + + tgModel* getBlocks(); + + /** Create the tgWorld object */ + tgWorld *createWorld(); + + /** Use for displaying tensegrities in simulation */ + tgSimViewGraphics *createGraphicsView(tgWorld *world); + + /** Use for trial episodes of many tensegrities in an experiment */ + tgSimView *createView(tgWorld *world); + + /** Run a series of episodes for nSteps each */ + void simulate(tgSimulation *simulation); + + + // Keep these around for cleanup + tgWorld* world; + tgSimView* view; + tgSimulation* simulation; + + bool use_graphics; + bool add_controller; + bool add_blocks; + bool add_hills; + bool all_terrain; + double timestep_physics; //Seconds + double timestep_graphics; // Seconds, AKA render rate. Leave at 1/60 for real-time viewing + int nEpisodes; // Number of episodes ("trial runs") + int nSteps; // Number of steps in each episode, 60k is 100 seconds (timestep_physics*nSteps) + int nSegments; // Number of segments in the tensegrity spine + int nTypes; // Number of types of terrain to be used. Currently 3 + + double startX; + double startY; + double startZ; + double startAngle; + double goalAngle; + + std::string lowerPath; + std::string suffix; + + bool bSetup; +}; + +#endif // APP_QUAD_CONTROL_SPIRAL2_H + diff --git a/src/dev/bgigous/MG_FirstAttempt/BigPuppySymmetricSpiral2.cpp b/src/dev/bgigous/MG_FirstAttempt/BigPuppySymmetricSpiral2.cpp new file mode 100644 index 000000000..d6d078887 --- /dev/null +++ b/src/dev/bgigous/MG_FirstAttempt/BigPuppySymmetricSpiral2.cpp @@ -0,0 +1,748 @@ +/* + * 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 BigPuppySymmetricSpiral2.cpp + * @brief Implementing a quadruped based off the Flemons BigPuppy model. + * @author Dawn Hustig-Schultz + * @date July 2015 + * @version 1.1.0 + * $Id$ + */ + + +//This application +#include "BigPuppySymmetricSpiral2.h" + +// This library +#include "core/tgModel.h" +#include "core/tgSimView.h" +#include "core/tgSimViewGraphics.h" +#include "core/tgSimulation.h" +#include "core/tgWorld.h" +#include "core/tgBasicActuator.h" +#include "tgcreator/tgKinematicActuatorInfo.h" +#include "tgcreator/tgKinematicContactCableInfo.h" +#include "core/tgRod.h" +#include "core/tgSpringCableActuator.h" +#include "core/tgString.h" +#include "tgcreator/tgBuildSpec.h" +#include "tgcreator/tgBasicActuatorInfo.h" +#include "tgcreator/tgRodInfo.h" +#include "tgcreator/tgStructure.h" +#include "tgcreator/tgStructureInfo.h" +// The Bullet Physics library +#include "LinearMath/btVector3.h" +// The C++ Standard Library +#include +#include + +//#define USE_KINEMATIC +//#define PASSIVE_STRUCTURE + +BigPuppySymmetricSpiral2::BigPuppySymmetricSpiral2(int segments, int hips, int legs, int feet) : +BaseSpineModelLearning(segments), +m_hips(hips), +m_legs(legs), +m_feet(feet) +{ +} + +BigPuppySymmetricSpiral2::~BigPuppySymmetricSpiral2() +{ +} + +void BigPuppySymmetricSpiral2::addNodesFoot(tgStructure& s, double r1, double r2){ + s.addNode(r2,0,r2);//0 + s.addNode(r2,0,-r2);//1 + s.addNode(-r2,0,-r2);//2 + s.addNode(-r2,0,r2);//3 + s.addNode(r2/2,r1/2,0);//4 + s.addNode(0,r1/2,-r2/2);//5 + s.addNode(-r2/2,r1/2,0);//6 + s.addNode(0,r1/2,r2/2);//7 +} + +void BigPuppySymmetricSpiral2::addRodsFoot(tgStructure& s){ + s.addPair(0,6,"rod"); + s.addPair(1,7,"rod"); + s.addPair(2,4,"rod"); + s.addPair(3,5,"rod"); +} + +void BigPuppySymmetricSpiral2::addNodesLeg(tgStructure& s, double r){ + s.addNode(0,0,0); //0: Bottom Center of lower leg segment + s.addNode(0,r,0); //1: Center of lower leg segment + s.addNode(r,r,0); //2: Right of lower leg segment + s.addNode(-r,r,0); //3: Left of lower leg segment + s.addNode(0,2*r,0); //4: Top of lower leg segment + s.addNode(0,-r/2,0); //5: Leg segment extension for connections to foot. + s.addNode(0,-r/2,-r/2); //6 + s.addNode(0,-r/2,r/2); //7 + s.addNode(r/2,-r/2,0); //8 + s.addNode(-r/2,-r/2,0); //9 +} + +void BigPuppySymmetricSpiral2::addRodsLeg(tgStructure& s){ + s.addPair(0,1,"rod"); + s.addPair(1,2,"rod"); + s.addPair(1,3,"rod"); + s.addPair(1,4,"rod"); + s.addPair(0,5,"rod"); + s.addPair(5,6,"rod"); + s.addPair(5,7,"rod"); + s.addPair(5,8,"rod"); + s.addPair(5,9,"rod"); +} + +void BigPuppySymmetricSpiral2::addNodesHip(tgStructure& s, double r){ + s.addNode(0,0,0); //Node 0 + s.addNode(0,r,r); //Node 1 + s.addNode(0,-r,-r); //Node 2 + s.addNode(0,-r,r); //Node 3 +} + +void BigPuppySymmetricSpiral2::addRodsHip(tgStructure& s){ + s.addPair(0,1,"rod"); + s.addPair(0,2,"rod"); + s.addPair(0,3,"rod"); +} + +void BigPuppySymmetricSpiral2::addNodesVertebra(tgStructure& s, double r){ + s.addNode(0,0,0); //Node 0 + s.addNode(r,0,r); //Node 1 + s.addNode(r,0,-r); //Node 2 + s.addNode(-r,0,-r); //Node 3 + s.addNode(-r,0,r); //Node 4 +} + +void BigPuppySymmetricSpiral2::addRodsVertebra(tgStructure& s){ + s.addPair(0,1,"rod"); + s.addPair(0,2,"rod"); + s.addPair(0,3,"rod"); + s.addPair(0,4,"rod"); +} + +void BigPuppySymmetricSpiral2::addSegments(tgStructure& puppy, tgStructure& vertebra, tgStructure& hip, tgStructure& leg, double r){ + const double offsetDist = r+1; + const double offsetDist2 = offsetDist*6; + const double offsetDist3 = offsetDist2+2; + const double yOffset_leg = -(2*r+1); + const double yOffset_foot = -(2*r+6); + + //Vertebrae + btVector3 offset(offsetDist,0.0,0); + //Hips + btVector3 offset1(offsetDist*2,0.0,offsetDist); + btVector3 offset2(offsetDist2,0.0,offsetDist); + btVector3 offset3(offsetDist*2,0.0,-offsetDist); + btVector3 offset4(offsetDist2,0.0,-offsetDist); + //Lower legs + btVector3 offset5(offsetDist3,yOffset_leg,offsetDist); + btVector3 offset6(offsetDist3,yOffset_leg,-offsetDist); + btVector3 offset7(r*2,yOffset_leg,offsetDist); + btVector3 offset8(r*2,yOffset_leg,-offsetDist); + //Feet + btVector3 offset9(offsetDist3+1,yOffset_foot,offsetDist); + btVector3 offset10(offsetDist3+1,yOffset_foot,-offsetDist); + btVector3 offset11(r*2+1,yOffset_foot,offsetDist); + btVector3 offset12(r*2+1,yOffset_foot,-offsetDist); + + for(std::size_t i = 0; i < m_segments; i++) { //Connect segments for spine of puppy + tgStructure* t = new tgStructure (vertebra); + t->addTags(tgString("spine segment num", i + 1)); + t->move((i + 1)*offset); + + if (i % 2 == 1){ + + t->addRotation(btVector3((i + 1) * offsetDist, 0.0, 0.0), btVector3(1, 0, 0), 0.0); + + } + else{ + + t->addRotation(btVector3((i + 1) * offsetDist, 0.0, 0.0), btVector3(1, 0, 0), M_PI/2.0); + + } + + puppy.addChild(t); //Add a segment to the puppy + } + + for(std::size_t i = m_segments; i < (m_segments + 2); i++) {//deal with right hip and shoulder first + tgStructure* t = new tgStructure (hip); + t->addTags(tgString("segment num", i + 1)); + + if(i % 2 == 0){ + t->move(offset2); + t->addRotation(btVector3(offsetDist2, 0.0, offsetDist), btVector3(0, 1, 0), M_PI); + + } + else{ + t->move(offset1); + t->addRotation(btVector3(offsetDist*2, 0.0, offsetDist), btVector3(0, 1, 0), M_PI); + } + + puppy.addChild(t); //Add a segment to the puppy + } + + for(std::size_t i = (m_segments + 2); i < (m_segments + m_hips); i++) {//deal with left hip and shoulder now + tgStructure* t = new tgStructure (hip); + t->addTags(tgString("segment num", i + 1)); + + if(i % 2 == 0){ + t->move(offset4); + } + else{ + t->move(offset3); + } + + puppy.addChild(t); //Add a segment to the puppy + + } + + for(std::size_t i = (m_segments + m_hips); i < (m_segments + m_hips + 2); i++) {//right front and back legs + tgStructure* t = new tgStructure (leg); + t->addTags(tgString("segment num", i + 1)); + + if(i % 2 == 0){ + t->move(offset5); + t->addRotation(btVector3(offsetDist3, yOffset_leg, offsetDist), btVector3(0, 1, 0), M_PI); + + } + else{ + t->move(offset7); + t->addRotation(btVector3(r*2, yOffset_leg, offsetDist), btVector3(0, 1, 0), M_PI); + //the rotations for the legs are a remnant of the earlier design. Removing them now + //would mean changing all my muscle attachments. I will do this someday. + + } + + puppy.addChild(t); //Add a segment to the puppy + } + + for(std::size_t i = (m_segments + m_hips + 2); i < (m_segments + m_hips + m_legs); i++) {//left front and back legs + tgStructure* t = new tgStructure (leg); + t->addTags(tgString("segment num", i + 1)); + + if(i % 2 == 0){ + t->move(offset6); + t->addRotation(btVector3(offsetDist3, yOffset_leg, -offsetDist), btVector3(0, 1, 0), M_PI); + + } + else{ + t->move(offset8); + t->addRotation(btVector3(r*2, yOffset_leg, -offsetDist), btVector3(0, 1, 0), M_PI); + } + + puppy.addChild(t); //Add a segment to the puppy + } + + /*for(std::size_t i = (m_segments + m_hips + m_legs); i < (m_segments + m_hips + m_legs + 2); i++) {//right front and back feet + tgStructure* t = new tgStructure (foot); + t->addTags(tgString("segment num", i + 1)); + + if(i % 2 == 0){ + t->move(offset9); + t->addRotation(btVector3(offsetDist3+1, yOffset_foot, offsetDist), btVector3(0, 1, 0), 0.0); + + } + else{ + t->move(offset11); + t->addRotation(btVector3(r*2+1, yOffset_foot, offsetDist), btVector3(0, 1, 0), 0.0); + } + + puppy.addChild(t); //Add a segment to the puppy + } + + for(std::size_t i = (m_segments + m_hips + m_legs + 2); i < (m_segments + m_hips + m_legs + m_feet); i++) {//left front and back feet + tgStructure* t = new tgStructure (foot); + t->addTags(tgString("segment num", i + 1)); + + if(i % 2 == 0){ + t->move(offset10); + t->addRotation(btVector3(offsetDist3+1, yOffset_foot, -offsetDist), btVector3(0, 1, 0), 0.0); + + } + else{ + t->move(offset12); + t->addRotation(btVector3(r*2+1, yOffset_foot, -offsetDist), btVector3(0, 1, 0), 0.0); + } + + puppy.addChild(t); //Add a segment to the puppy + } */ +} + +void BigPuppySymmetricSpiral2::addMuscles(tgStructure& puppy){ + //Time to add the muscles to the structure. Todo: try to clean this up some more. + std::vector children = puppy.getChildren(); + for(std::size_t i = 2; i < (children.size() - (m_hips + m_legs)); i++) { + + tgNodes n0 = children[i-2]->getNodes(); + tgNodes n1 = children[i-1]->getNodes(); + tgNodes n2 = children[i]->getNodes(); + + + if(i==2){ + //Extra muscles, to keep front vertebra from swinging. + puppy.addPair(n0[3], n1[3], tgString("spine front upper right muscleAct seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[3], n1[4], tgString("spine front upper left muscleAct seg", i-2) + tgString(" seg", i-1)); + + puppy.addPair(n0[4], n1[3], tgString("spine front lower right muscleAct seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[4], n1[4], tgString("spine front lower left muscleAct seg", i-2) + tgString(" seg", i-1)); + + + } + + //Add muscles to the puppy + if(i < 3){ + if(i % 2 == 0){ //front + puppy.addPair(n0[1], n1[3], tgString("spine front lower right muscleAct seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[1], n1[4], tgString("spine front lower left muscleAct seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[2], n1[3], tgString("spine front upper right muscleAct seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[2], n1[4], tgString("spine front upper left muscleAct seg", i-2) + tgString(" seg", i-1)); + } + else{ //rear + puppy.addPair(n0[1], n1[3], tgString("spine rear upper left muscleAct seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[1], n1[4], tgString("spine rear lower left muscleAct seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[2], n1[3], tgString("spine rear upper right muscleAct seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[2], n1[4], tgString("spine rear lower right muscleAct seg", i-2) + tgString(" seg", i-1)); + } + } + if(i < 7){//Was 6 + if(i % 2 == 0){ + puppy.addPair(n0[1], n2[4], tgString("spine2 bottom muscleAct seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[2], n2[3], tgString("spine2 top muscleAct seg", i-2) + tgString(" seg", i-1)); + } + else{ + puppy.addPair(n0[1], n2[4], tgString("spine2 lateral left muscleAct seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[2], n2[3], tgString("spine2 lateral right muscleAct seg", i-2) + tgString(" seg", i-1)); + + } + } + if(i > 0 && i < 7){ + if(i % 2 == 0){//rear + puppy.addPair(n1[1], n2[3], tgString("spine rear upper left muscleAct seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[1], n2[4], tgString("spine rear lower left muscleAct seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[2], n2[3], tgString("spine rear upper right muscleAct seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[2], n2[4], tgString("spine rear lower right muscleAct seg", i-1) + tgString(" seg", i)); + } + else{//front + + puppy.addPair(n1[1], n2[3], tgString("spine front lower right muscleAct seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[1], n2[4], tgString("spine front lower left muscleAct seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[2], n2[3], tgString("spine front upper right muscleAct seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[2], n2[4], tgString("spine front upper left muscleAct seg", i-1) + tgString(" seg", i)); + } + } + if (i >= 2 && i < 7){ + puppy.addPair(n1[3], n2[3], tgString("spine spiral muscleAct seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[4], n2[3], tgString("spine spiral muscleAct seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[3], n2[4], tgString("spine spiral muscleAct seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[4], n2[4], tgString("spine spiral muscleAct seg", i-1) + tgString(" seg", i)); + } + /*if(i==2 || i==6){ + puppy.addPair(n1[3], n2[3], tgString("spine spiral muscleAct seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[4], n2[3], tgString("spine spiral muscleAct seg", i-1) + tgString(" seg", i)); + } + if(i==3){ + puppy.addPair(n1[3], n2[3], tgString("spine spiral muscleAct seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[3], n2[4], tgString("spine spiral muscleAct seg", i-1) + tgString(" seg", i)); + } + if(i==4){ + puppy.addPair(n1[3], n2[4], tgString("spine spiral muscleAct seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[4], n2[4], tgString("spine spiral muscleAct seg", i-1) + tgString(" seg", i)); + } + if(i==5){ + puppy.addPair(n1[4], n2[3], tgString("spine spiral muscleAct seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[4], n2[4], tgString("spine spiral muscleAct seg", i-1) + tgString(" seg", i)); + }*/ + if(i == 6){ + //rear + puppy.addPair(n1[1], n2[2], tgString("spine rear lower left muscleAct seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[2], n2[2], tgString("spine rear lower right muscleAct seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[1], n2[1], tgString("spine rear upper left muscleAct seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[2], n2[1], tgString("spine rear upper right muscleAct seg", i-1) + tgString(" seg", i)); + } + + } + + + //Now add muscles to hips.... + tgNodes n0 = children[0]->getNodes(); + tgNodes n1 = children[1]->getNodes(); + tgNodes n2 = children[2]->getNodes(); + tgNodes n3 = children[3]->getNodes(); + tgNodes n4 = children[4]->getNodes(); + tgNodes n5 = children[5]->getNodes(); + tgNodes n6 = children[6]->getNodes(); + tgNodes n7 = children[7]->getNodes(); + tgNodes n8 = children[8]->getNodes(); + tgNodes n9 = children[9]->getNodes(); + tgNodes n10 = children[10]->getNodes(); + tgNodes n11 = children[11]->getNodes(); + tgNodes n12 = children[12]->getNodes(); + tgNodes n13 = children[13]->getNodes(); + tgNodes n14 = children[14]->getNodes(); + + //Left shoulder muscles + puppy.addPair(n7[1], n1[1], tgString("left shoulder rear upper muscleAct seg", 6) + tgString(" seg", 1)); + puppy.addPair(n7[1], n1[4], tgString("left shoulder front upper muscleAct seg", 6) + tgString(" seg", 1)); + puppy.addPair(n7[1], n0[2], tgString("left shoulder front top muscleAct seg", 6) + tgString(" seg", 0)); + puppy.addPair(n7[1], n2[3], tgString("left shoulder rear top muscleAct seg", 6) + tgString(" seg", 2)); + + puppy.addPair(n7[3], n1[1], tgString("left shoulder rear lower muscleAct seg", 6) + tgString(" seg", 1)); + puppy.addPair(n7[3], n1[4], tgString("left shoulder front lower muscleAct seg", 6) + tgString(" seg", 1)); + puppy.addPair(n7[3], n0[1], tgString("left shoulder front bottom muscleAct seg", 6) + tgString(" seg", 0)); + puppy.addPair(n7[3], n2[4], tgString("left shoulder rear bottom muscleAct seg", 6) + tgString(" seg", 2)); + + //Extra muscles, to move left shoulder forward and back: + puppy.addPair(n7[0], n1[1], tgString("left shoulder rear mid muscleAct seg", 6) + tgString(" seg", 1)); + puppy.addPair(n7[0], n1[4], tgString("left shoulder front mid muscleAct seg", 6) + tgString(" seg", 1)); + + //Left hip muscles + puppy.addPair(n8[1], n5[1], tgString("left hip rear upper muscleAct seg", 7) + tgString(" seg", 5)); + puppy.addPair(n8[1], n5[4], tgString("left hip front upper muscleAct seg", 7) + tgString(" seg", 5)); + puppy.addPair(n8[1], n4[2], tgString("left hip front top muscleAct seg", 7) + tgString(" seg", 4)); + puppy.addPair(n8[1], n6[3], tgString("left hip rear top muscleAct seg", 7) + tgString(" seg", 4)); + + puppy.addPair(n8[3], n5[1], tgString("left hip rear lower muscleAct seg", 7) + tgString(" seg", 5)); + puppy.addPair(n8[3], n5[4], tgString("left hip front lower muscleAct seg", 7) + tgString(" seg", 5)); + puppy.addPair(n8[3], n4[1], tgString("left hip front bottom muscleAct seg", 7) + tgString(" seg", 4)); + puppy.addPair(n8[3], n6[4], tgString("left hip front bottom muscleAct seg", 7) + tgString(" seg", 6)); + + //Extra muscles, to move left hip forward and back: + puppy.addPair(n8[0], n5[1], tgString("left hip rear mid muscleAct seg", 7) + tgString(" seg", 3)); //could also be n3[3] + puppy.addPair(n8[0], n5[4], tgString("left hip front mid muscleAct seg", 7) + tgString(" seg", 5)); + + //Right shoulder muscles + puppy.addPair(n9[1], n1[2], tgString("right shoulder rear upper muscleAct seg", 8) + tgString(" seg", 1)); + puppy.addPair(n9[1], n1[3], tgString("right shoulder front upper muscleAct seg", 8) + tgString(" seg", 1)); + puppy.addPair(n9[1], n0[2], tgString("right shoulder front top muscleAct seg", 8) + tgString(" seg", 0)); + puppy.addPair(n9[1], n2[3], tgString("right shoulder rear top muscleAct seg", 8) + tgString(" seg", 2)); + + puppy.addPair(n9[3], n1[2], tgString("right shoulder rear lower muscleAct seg", 8) + tgString(" seg", 1)); + puppy.addPair(n9[3], n1[3], tgString("right shoulder front lower muscleAct seg", 8) + tgString(" seg", 1)); + puppy.addPair(n9[3], n0[1], tgString("right shoulder front bottom muscleAct seg", 8) + tgString(" seg", 0)); + puppy.addPair(n9[3], n2[4], tgString("right shoulder rear bottom muscleAct seg", 8) + tgString(" seg", 2)); + + //Extra muscles, to move right shoulder forward and back: + puppy.addPair(n9[0], n1[2], tgString("right shoulder rear mid muscleAct seg", 8) + tgString(" seg", 1)); + puppy.addPair(n9[0], n1[3], tgString("right shoulder front mid muscleAct seg", 8) + tgString(" seg", 1)); + + //Right hip muscles + puppy.addPair(n10[1], n5[2], tgString("right hip rear upper muscleAct seg", 9) + tgString(" seg", 5)); + puppy.addPair(n10[1], n5[3], tgString("right hip front upper muscleAct seg", 9) + tgString(" seg", 5)); + puppy.addPair(n10[1], n4[2], tgString("right hip front top muscleAct seg", 9) + tgString(" seg", 4)); + puppy.addPair(n10[1], n6[3], tgString("right hip rear top muscleAct seg", 9) + tgString(" seg", 4)); + + puppy.addPair(n10[3], n5[2], tgString("right hip rear lower muscleAct seg", 9) + tgString(" seg", 5)); + puppy.addPair(n10[3], n5[3], tgString("right hip front lower muscleAct seg", 9) + tgString(" seg", 5)); + puppy.addPair(n10[3], n4[1], tgString("right hip bottom muscleAct seg", 9) + tgString(" seg", 4)); + puppy.addPair(n10[3], n6[4], tgString("right hip bottom muscleAct seg", 9) + tgString(" seg", 6)); + + //Extra muscles, to move right hip forward and back: + puppy.addPair(n10[0], n5[2], tgString("right hip rear mid muscleAct seg", 9) + tgString(" seg", 3)); //could also be n3[3] + puppy.addPair(n10[0], n5[3], tgString("right hip front mid muscleAct seg", 9) + tgString(" seg", 5)); + + //Leg/hip connections: + + //Right front leg/shoulder + puppy.addPair(n11[4], n7[3], tgString("right front leg outer bicep muscle seg", 10) + tgString(" seg", 6)); + puppy.addPair(n11[4], n7[2], tgString("right front leg inner bicep muscle seg", 10) + tgString(" seg", 6)); + puppy.addPair(n11[4], n1[4], tgString("right front leg front abdomen connection muscle seg", 10) + tgString(" seg", 1)); + puppy.addPair(n11[3], n1[1],tgString("right front leg front abdomen connection muscle seg", 11) + tgString(" seg", 5)); + puppy.addPair(n11[2], n1[4],tgString("right front leg abdomen connection muscle seg", 11) + tgString(" seg", 5)); + + puppy.addPair(n11[3], n7[3], tgString("right front leg outer tricep muscle seg", 10) + tgString(" seg", 6)); + puppy.addPair(n11[3], n7[2], tgString("right front leg inner tricep muscle seg", 10) + tgString(" seg", 6)); + + puppy.addPair(n11[2], n7[3], tgString("right front leg outer front tricep muscle seg", 10) + tgString(" seg", 6)); + puppy.addPair(n11[2], n7[2], tgString("right front leg inner front tricep muscle seg", 10) + tgString(" seg", 6)); + + //Adding muscle to pull up on right front leg: + puppy.addPair(n11[4], n7[1], tgString("right front leg mid bicep muscle3 seg", 10) + tgString(" seg", 6)); + + //Left front leg/shoulder + puppy.addPair(n13[4], n9[2], tgString("left front leg inner bicep muscle seg", 12) + tgString(" seg", 8)); + puppy.addPair(n13[4], n9[3], tgString("left front leg outer bicep muscle seg", 12) + tgString(" seg", 8)); + puppy.addPair(n13[4], n1[3], tgString("left front leg front abdomen connection muscle seg", 12) + tgString(" seg", 1)); //Was n1[2] + puppy.addPair(n13[3], n1[2], tgString("left front leg front abdomen connection muscle seg", 13) + tgString(" seg", 5)); + puppy.addPair(n13[2], n1[3], tgString("left front leg front abdomen connection muscle seg", 13) + tgString(" seg", 5)); + + puppy.addPair(n13[3], n9[2], tgString("left front leg inner tricep muscle seg", 12) + tgString(" seg", 8)); + puppy.addPair(n13[3], n9[3], tgString("left front leg outer tricep muscle seg", 12) + tgString(" seg", 8)); + + puppy.addPair(n13[2], n9[2], tgString("left front leg inner front tricep muscle seg", 12) + tgString(" seg", 8)); + puppy.addPair(n13[2], n9[3], tgString("left front leg outer front tricep muscle seg", 12) + tgString(" seg", 8)); + + //Adding muscle to pull up on left front leg: + puppy.addPair(n13[4], n9[1], tgString("leg left mid bicep muscle3 seg", 12) + tgString(" seg", 8)); + + //Right rear leg/hip + puppy.addPair(n12[4], n8[3], tgString("right hind leg outer thigh muscle seg", 11) + tgString(" seg", 7)); + puppy.addPair(n12[4], n8[2], tgString("right hind leg inner thigh muscle seg", 11) + tgString(" seg", 7)); + + puppy.addPair(n12[4], n3[1],tgString("right hind leg rear abdomen connection muscle seg", 11) + tgString(" seg", 3)); + puppy.addPair(n12[3], n5[1],tgString("right hind leg rear abdomen connection muscle seg", 11) + tgString(" seg", 5)); + puppy.addPair(n12[2], n5[4],tgString("right hind leg rear abdomen connection muscle seg", 11) + tgString(" seg", 5)); + + puppy.addPair(n12[3], n8[3], tgString("right hind leg outer calf muscle seg", 11) + tgString(" seg", 7)); + puppy.addPair(n12[3], n8[2], tgString("right hind leg inner calf muscle seg", 11) + tgString(" seg", 7)); + + puppy.addPair(n12[2], n8[3], tgString("right hind leg outer front calf muscle seg", 11) + tgString(" seg", 7)); + puppy.addPair(n12[2], n8[2], tgString("right hind leg inner front calf muscle seg", 11) + tgString(" seg", 7)); + + //Adding muscle to pull rear right leg up: + puppy.addPair(n12[4], n8[1], tgString("right hind leg central thigh muscle3 seg", 11) + tgString(" seg", 7)); + + //Left rear leg/hip + puppy.addPair(n14[4], n10[2], tgString("left hind leg inner thigh muscle seg", 13) + tgString(" seg", 9)); + puppy.addPair(n14[4], n10[3], tgString("left hind leg outer thigh muscle seg", 13) + tgString(" seg", 9)); + + puppy.addPair(n14[4], n3[2], tgString("left hind leg rear abdomen connection muscle seg", 13) + tgString(" seg", 3)); + puppy.addPair(n14[3], n5[2], tgString("left hind leg rear abdomen connection muscle seg", 13) + tgString(" seg", 5)); + puppy.addPair(n14[2], n5[3], tgString("left hind leg rear abdomen connection muscle seg", 13) + tgString(" seg", 5)); + + puppy.addPair(n14[3], n10[2], tgString("left hind leg inner calf muscle seg", 13) + tgString(" seg", 9)); + puppy.addPair(n14[3], n10[3], tgString("left hind leg outer calf muscle seg", 13) + tgString(" seg", 9)); + + puppy.addPair(n14[2], n10[2], tgString("left hind leg inner front calf muscle seg", 13) + tgString(" seg", 9)); + puppy.addPair(n14[2], n10[3], tgString("left hind leg outer front calf muscle seg", 13) + tgString(" seg", 9)); + + //Adding muscle to pull rear left leg up: + puppy.addPair(n14[4], n10[1], tgString("left hind leg central thigh muscle3 seg", 13) + tgString(" seg", 9)); + + //Populate feet with muscles. Todo: think up names to differentiate each! + /*for(std::size_t i = (m_segments + m_hips + m_legs); i < children.size(); i++) { + tgNodes ni = children[i]->getNodes(); + tgNodes ni4 = children[i-4]->getNodes(); + + puppy.addPair(ni[0],ni[1],tgString("foot muscle seg", i)); + puppy.addPair(ni[0],ni[3],tgString("foot muscle seg", i)); + puppy.addPair(ni[1],ni[2],tgString("foot muscle seg", i)); + puppy.addPair(ni[2],ni[3],tgString("foot muscle seg", i)); + puppy.addPair(ni[0],ni[7],tgString("foot muscle2 seg", i)); + puppy.addPair(ni[1],ni[4],tgString("foot muscle2 seg", i)); + puppy.addPair(ni[2],ni[5],tgString("foot muscle2 seg", i)); + puppy.addPair(ni[3],ni[6],tgString("foot muscle2 seg", i)); + puppy.addPair(ni[4],ni[5],tgString("foot muscle2 seg", i)); + puppy.addPair(ni[4],ni[7],tgString("foot muscle2 seg", i)); + puppy.addPair(ni[5],ni[6],tgString("foot muscle2 seg", i)); + puppy.addPair(ni[6],ni[7],tgString("foot muscle2 seg", i)); + + //Connect feet to legs: + puppy.addPair(ni4[5],ni[0],tgString("foot muscle2 seg", i) + tgString(" seg", i-4)); + puppy.addPair(ni4[5],ni[1],tgString("foot muscle2 seg", i) + tgString(" seg", i-4)); + puppy.addPair(ni4[5],ni[2],tgString("foot muscle2 seg", i) + tgString(" seg", i-4)); + puppy.addPair(ni4[5],ni[3],tgString("foot muscle2 seg", i) + tgString(" seg", i-4)); + + puppy.addPair(ni4[0],ni[4],tgString("foot muscle2 seg", i) + tgString(" seg", i-4)); + puppy.addPair(ni4[0],ni[5],tgString("foot muscle2 seg", i) + tgString(" seg", i-4)); + puppy.addPair(ni4[0],ni[6],tgString("foot muscle2 seg", i) + tgString(" seg", i-4)); + puppy.addPair(ni4[0],ni[7],tgString("foot muscle2 seg", i) + tgString(" seg", i-4)); + + }*/ + +} + +void BigPuppySymmetricSpiral2::setup(tgWorld& world) +{ + //Rod and Muscle configuration. + const double density = 4.2/300.0; //Note: this needs to be high enough or things fly apart... + const double radius = 0.5; + const double rod_space = 10.0; + const double rod_space2 = 8.0; + const double friction = 0.5; + const double rollFriction = 0.0; + const double restitution = 0.0; + + const tgRod::Config rodConfig(radius, density, friction, rollFriction, restitution); + + const double stiffness = 1000.0; + const double stiffnessPassive = 4000.0; + const double damping = .01*stiffness; + const double pretension = 0.0; + const bool history = true; + const double maxTens = 7000.0; + const double maxSpeed = 12.0; + + const double passivePretension = 1000; + const double passivePretension2 = 3500; + const double passivePretension3 = 3500; + +#ifdef USE_KINEMATIC + + const double mRad = 1.0; + const double motorFriction = 10.0; + const double motorInertia = 1.0; + const bool backDrivable = false; + #ifdef PASSIVE_STRUCTURE + tgKinematicActuator::Config motorConfig(2000, 20, passivePretension, + mRad, motorFriction, motorInertia, backDrivable, + history, maxTens, maxSpeed); + + tgKinematicActuator::Config motorConfigOther(stiffnessPassive, damping, passivePretension2, + mRad, motorFriction, motorInertia, backDrivable, + history, maxTens, maxSpeed); + + tgKinematicActuator::Config motorConfigFeet(stiffnessPassive, damping, passivePretension, + mRad, motorFriction, motorInertia, backDrivable, + history, maxTens, maxSpeed); + tgKinematicActuator::Config motorConfigLegs(stiffnessPassive, 20, passivePretension3, + mRad, motorFriction, motorInertia, backDrivable, + history, maxTens, maxSpeed); + #else + tgKinematicActuator::Config motorConfigSpine(stiffness, damping, pretension, + mRad, motorFriction, motorInertia, backDrivable, + history, maxTens, maxSpeed); + + tgKinematicActuator::Config motorConfigOther(stiffnessPassive, damping, passivePretension2, + mRad, motorFriction, motorInertia, backDrivable, + history, maxTens, maxSpeed); + + tgKinematicActuator::Config motorConfigFeet(stiffnessPassive, damping, passivePretension, + mRad, motorFriction, motorInertia, backDrivable, + history, maxTens, maxSpeed); + tgKinematicActuator::Config motorConfigLegs(stiffnessPassive, damping, passivePretension3, + mRad, motorFriction, motorInertia, backDrivable, + history, maxTens, maxSpeed); + #endif + +#else + + #ifdef PASSIVE_STRUCTURE + tgSpringCableActuator::Config muscleConfig(2000, 20, passivePretension, history); + tgSpringCableActuator::Config muscleConfigOther(stiffnessPassive, damping, passivePretension2); + tgSpringCableActuator::Config muscleConfigFeet(stiffnessPassive, damping, passivePretension); + tgSpringCableActuator::Config muscleConfigLegs(stiffnessPassive, damping, passivePretension3); + + + #else + tgSpringCableActuator::Config muscleConfigSpine(stiffness, damping, pretension, history, maxTens, 2*maxSpeed); + tgSpringCableActuator::Config muscleConfigOther(stiffnessPassive, damping, passivePretension2, history); + tgSpringCableActuator::Config muscleConfigFeet(stiffnessPassive, damping, passivePretension, history); + tgSpringCableActuator::Config muscleConfigLegs(stiffnessPassive, damping, passivePretension3, history); + #endif + +#endif + + //Foot: + tgStructure foot; + addNodesFoot(foot,rod_space,rod_space2); + addRodsFoot(foot); + + //Leg: + tgStructure leg; + addNodesLeg(leg,rod_space); + addRodsLeg(leg); + + //Create the basic unit of the puppy + tgStructure vertebra; + addNodesVertebra(vertebra,rod_space); + addRodsVertebra(vertebra); + + //Create the basic unit for the hips/shoulders. + tgStructure hip; + addNodesHip(hip,rod_space); + addRodsHip(hip); + + //Build the puppy + tgStructure puppy; + + const double yOffset_foot = -(2*rod_space+6); + + addSegments(puppy,vertebra,hip,leg,rod_space); + + puppy.move(btVector3(0.0,-yOffset_foot,0.0)); + + addMuscles(puppy); //,m_segments,m_hips,m_legs,m_feet + + //Time to add the muscles to the structure. Todo: make this a function; also try to clean this up. + std::vector children = puppy.getChildren(); + + // Create the build spec that uses tags to turn the structure into a real model + tgBuildSpec spec; + spec.addBuilder("rod", new tgRodInfo(rodConfig)); + +#ifdef USE_KINEMATIC + + #ifdef PASSIVE_STRUCTURE + spec.addBuilder("muscleAct", new tgKinematicContactCableInfo(motorConfig)); + spec.addBuilder("muscle ", new tgKinematicContactCableInfo(motorConfigOther)); + //spec.addBuilder("muscle2 ", new tgKinematicContactCableInfo(motorConfigFeet)); + spec.addBuilder("muscle3 ", new tgKinematicContactCableInfo(motorConfigLegs)); + #else + spec.addBuilder("muscleAct", new tgKinematicContactCableInfo(motorConfigSpine)); + spec.addBuilder("muscle ", new tgKinematicContactCableInfo(motorConfigOther)); + //spec.addBuilder("muscle2 ", new tgKinematicContactCableInfo(motorConfigFeet)); + spec.addBuilder("muscle3 ", new tgKinematicContactCableInfo(motorConfigLegs)); + + #endif + +#else + #ifdef PASSIVE_STRUCTURE + spec.addBuilder("muscleAct", new tgBasicActuatorInfo(muscleConfig)); + spec.addBuilder("muscle " , new tgBasicActuatorInfo(muscleConfigOther)); + //spec.addBuilder("muscle2 " , new tgBasicActuatorInfo(muscleConfigFeet)); + spec.addBuilder("muscle3 " , new tgBasicActuatorInfo(muscleConfigLegs)); + #else + spec.addBuilder("muscleAct" , new tgBasicActuatorInfo(muscleConfigSpine)); + spec.addBuilder("muscle " , new tgBasicActuatorInfo(muscleConfigOther)); + //spec.addBuilder("muscle2 " , new tgBasicActuatorInfo(muscleConfigFeet)); + spec.addBuilder("muscle3 " , new tgBasicActuatorInfo(muscleConfigLegs)); + #endif + +#endif + + + + // Create your structureInfo + tgStructureInfo structureInfo(puppy, spec); + + // Use the structureInfo to build ourselves + structureInfo.buildInto(*this, world); + + // We could now use tgCast::filter or similar to pull out the + // models (e.g. muscles) that we want to control. + m_allMuscles = tgCast::filter (getDescendants()); + + m_allSegments = this->find ("spine segment"); + + // Actually setup the children, notify controller that the setup has finished + BaseSpineModelLearning::setup(world); + + children.clear(); +} + +void BigPuppySymmetricSpiral2::step(double dt) +{ + // Precondition + if (dt <= 0.0) + { + throw std::invalid_argument("dt is not positive"); + } + else + { + // Notify observers (controllers) of the step so that they can take action + BaseSpineModelLearning::step(dt); + } +} + +void BigPuppySymmetricSpiral2::teardown() +{ + BaseSpineModelLearning::teardown(); +} diff --git a/src/dev/bgigous/MG_FirstAttempt/BigPuppySymmetricSpiral2.h b/src/dev/bgigous/MG_FirstAttempt/BigPuppySymmetricSpiral2.h new file mode 100644 index 000000000..d16bba810 --- /dev/null +++ b/src/dev/bgigous/MG_FirstAttempt/BigPuppySymmetricSpiral2.h @@ -0,0 +1,174 @@ +/* + * 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. +*/ + +#ifndef BIGPUPPY_SYMMETRIC_SPIRAL2_H +#define BIGPUPPY_SYMMETRIC_SPIRAL2_H + +/** + * @file BigPuppySymmetricSpiral2.h + * @brief Implementing the Flemons quadruped model (roughly), but as a subclass of Brian's BaseSpineModelLearning module. + * @author Dawn Hustig-Schultz + * @date Jan 2016 + * @version 1.0.0 + * $Id$ + */ + +#include "examples/learningSpines/BaseSpineModelLearning.h" + +// This library +#include "core/tgModel.h" +#include "core/tgSubject.h" +// The C++ Standard Library +#include +#include +#include +#include + +class tgSpringCableActuator; +class tgWorld; +class tgStructure; //Do I need this, or tgStructureInfo? +class tgStructureInfo; //Do I need this, or just tgStructure, or both? +class tgBasicActuator; + +class BigPuppySymmetricSpiral2: public BaseSpineModelLearning //public tgSubject, public tgModel +{ +public: + + BigPuppySymmetricSpiral2(int segments, int hips, int legs, int feet); + + virtual ~BigPuppySymmetricSpiral2(); + + /** + * Create the model. Place the rods and strings into the world + * that is passed into the simulation. This is triggered + * automatically when the model is added to the simulation, when + * tgModel::setup(world) is called (if this model is a child), + * and when reset is called. Also notifies controllers of setup. + * @param[in] world - the world we're building into + */ + virtual void setup(tgWorld& world); + + /** + * Undoes setup. Deletes child models. Called automatically on + * reset and end of simulation. Notifies controllers of teardown + */ + virtual void teardown(); + + /** + * Step the model, its children. Notifies controllers of step. + * @param[in] dt, the timestep. Must be positive. + */ + virtual void step(double dt); + +protected: + + const std::size_t m_hips; + const std::size_t m_legs; + const std::size_t m_feet; + +private: + + /** + * A function called during setup that determines the positions of + * the nodes of the quadruped's foot based on construction parameters. Rewrite this function + * for your own models + * @param[in] s: A tgStructure that we're building into + * r1: one half the height of a non-foot tgStructure + * r2: one half the length of a bottom edge of the foot tgStructure + */ + void addNodesFoot(tgStructure& s, double r1, double r2); + + /** + * A function called during setup that creates the rods for the quadruped's foot from + * the relevant nodes. Rewrite this function for your own models. + * @param[in] s: A tgStructure that we're building into + */ + static void addRodsFoot(tgStructure& s); + + /** + * A function called during setup that determines the positions of + * the nodes of the quadruped's leg based on construction parameters. Rewrite this function + * for your own models + * @param[in] s: A tgStructure that we're building into + * r: one half the height of the tgStructure + */ + void addNodesLeg(tgStructure& s, double r); + + /** + * A function called during setup that creates the rods for the quadruped's leg from + * the relevant nodes. Rewrite this function for your own models. + * @param[in] s: A tgStructure that we're building into + */ + static void addRodsLeg(tgStructure& s); + + /** + * A function called during setup that determines the positions of + * the nodes of the quadruped's hip based on construction parameters. Rewrite this function + * for your own models + * @param[in] s: A tgStructure that we're building into + * r: one half the height of the tgStructure + */ + void addNodesHip(tgStructure& s, double r); + + /** + * A function called during setup that creates the rods for the quadruped's hip from + * the relevant nodes. Rewrite this function for your own models. + * @param[in] s: A tgStructure that we're building into + */ + static void addRodsHip(tgStructure& s); + + /** + * A function called during setup that determines the positions of + * the nodes of a single vertebra (which is an X-tensegrity module) + * based on construction parameters. Rewrite this function + * for your own models + * @param[in] s: A tgStructure that we're building into + * r: one half the height of the tgStructure + */ + void addNodesVertebra(tgStructure& s, double r); + + /** + * A function called during setup that creates the rods of a single + * vertebra (which is an X-tensegrity module) from + * the relevant nodes. Rewrite this function for your own models. + * @param[in] s: A tgStructure that we're building into + */ + static void addRodsVertebra(tgStructure& s); + + /** + * A function called during setup that adds all the segments to the + * BigPuppy structure. Rewrite this function for your own models. + * @param[in] puppy: A tgStructure that we're building into + * r: one half the height of a substructure tgStructure + * segments: the number of segments in the spine of the quadruped + * hips: the number of hip segments + * legs: the number of leg segments + * feet: the number of feet segments + */ + void addSegments(tgStructure& puppy, tgStructure& vertebra, tgStructure& hip, tgStructure& leg, double r); + + /** + * A function called during setup that creates muscles (Strings) for the quadruped from + * the relevant nodes. Rewrite this function for your own models. + * @param[in] puppy: A tgStructure that we're building into + */ + void addMuscles(tgStructure& puppy); //, std::size_t segments, std::size_t hips, std::size_t legs, std::size_t feet + +}; + +#endif diff --git a/src/dev/bgigous/MG_FirstAttempt/CMakeLists.txt b/src/dev/bgigous/MG_FirstAttempt/CMakeLists.txt new file mode 100644 index 000000000..53f74cc71 --- /dev/null +++ b/src/dev/bgigous/MG_FirstAttempt/CMakeLists.txt @@ -0,0 +1,23 @@ +link_libraries(learningSpines + boost_program_options + tgcreator + core + util + terrain + Adapters + Configuration + AnnealEvolution + tgOpenGLSupport + obstacles + KinematicString + CPG_feedback + sensors + controllers) + +add_executable(AppQuadCoupling + AppQuadCoupling.cpp + JSONQuadFeedbackControl.cpp + BigPuppySymmetricSpiral2.cpp) + + +target_link_libraries(AppQuadCoupling ${ENV_LIB_DIR}/libjsoncpp.a FileHelpers boost_program_options obstacles JSONControl) diff --git a/src/dev/bgigous/MG_FirstAttempt/JSONQuadFeedbackControl.cpp b/src/dev/bgigous/MG_FirstAttempt/JSONQuadFeedbackControl.cpp new file mode 100644 index 000000000..bce6f0425 --- /dev/null +++ b/src/dev/bgigous/MG_FirstAttempt/JSONQuadFeedbackControl.cpp @@ -0,0 +1,842 @@ +/* + * 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 JSONFeedbackControl.cpp + * @brief A controller for the template class BaseSpineModelLearning + * @author Brian Mirletz + * @version 1.1.0 + * $Id$ + */ + +#include "JSONQuadFeedbackControl.h" + + +// Should include tgString, but compiler complains since its been +// included from BaseSpineModelLearning. Perhaps we should move things +// to a cpp over there +#include "core/tgSpringCableActuator.h" +#include "core/tgBasicActuator.h" +#include "controllers/tgImpedanceController.h" +#include "examples/learningSpines/tgCPGActuatorControl.h" +#include "dev/CPG_feedback/tgCPGCableControl.h" + +#include "examples/learningSpines/BaseSpineModelLearning.h" +#include "helpers/FileHelpers.h" + +#include "learning/AnnealEvolution/AnnealEvolution.h" +#include "learning/Configuration/configuration.h" + +#include "dev/CPG_feedback/CPGEquationsFB.h" +#include "dev/CPG_feedback/CPGNodeFB.h" + +#include "neuralNet/Neural Network v2/neuralNetwork.h" + +#include + +#include +#include +#include + +//#define LOGGING +#define USE_KINEMATIC + +using namespace std; + +JSONQuadFeedbackControl::Config::Config(int ss, + int tm, + int om, + int param, + int segnum, + double ct, + double la, + double ha, + double lp, + double hp, + double kt, + double kp, + double kv, + bool def, + double cl, + double lf, + double hf, + double ffMin, + double ffMax, + double afMin, + double afMax, + double pfMin, + double pfMax, + double maxH, + double minH, + double hffMin, + double hffMax) : +JSONCPGControl::Config::Config(ss, tm, om, param, segnum, ct, la, ha, + lp, hp, kt, kp, kv, def, cl, lf, hf), +freqFeedbackMin(ffMin), +freqFeedbackMax(ffMax), +highFreqFeedbackMin(hffMin), +highFreqFeedbackMax(hffMax), +ampFeedbackMin(afMin), +ampFeedbackMax(afMax), +phaseFeedbackMin(pfMin), +phaseFeedbackMax(pfMax), +maxHeight(maxH), +minHeight(minH) +{ + +} +/** + * Defining the adapters here assumes the controller is around and + * attached for the lifecycle of the learning runs. I.E. that the setup + * and teardown functions are used for tgModel + */ +JSONQuadFeedbackControl::JSONQuadFeedbackControl(JSONQuadFeedbackControl::Config config, + std::string args, + std::string resourcePath) : +JSONCPGControl(config, args, resourcePath), +m_config(config) +{ + // Path and filename handled by base class + +} + +JSONQuadFeedbackControl::~JSONQuadFeedbackControl() +{ + delete nn; +} + +void JSONQuadFeedbackControl::onSetup(BaseSpineModelLearning& subject) +{ + m_pCPGSys = new CPGEquationsFB(100); + + Json::Value root; // will contains the root value after parsing. + Json::Reader reader; + + 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 nodeVals = root.get("nodeVals", "UTF-8"); + Json::Value edgeVals = root.get("edgeVals", "UTF-8"); + + std::cout << nodeVals << std::endl; + + nodeVals = nodeVals.get("params", "UTF-8"); + edgeVals = edgeVals.get("params", "UTF-8"); + + // parameters for high CPG to high CPG coupling + Json::Value highEdgeVals = root.get("highVals", "UTF-8"); + highEdgeVals = highEdgeVals.get("params", "UTF-8"); + + // parameters for high CPG to low CPG coupling + Json::Value highLowEdgeVals = root.get("hLowVals", "UTF-8"); + highLowEdgeVals = highLowEdgeVals.get("params", "UTF-8"); + + array_4D edgeParams = scaleEdgeActions(edgeVals); + array_2D nodeParams = scaleNodeActions(nodeVals); + array_4D highEdgeParams = scaleHighEdgeActions(highEdgeVals); + + setupCPGs(subject, nodeParams, edgeParams); + setupHighCouplings(highEdgeParams); + setupHighLowCouplings(highLowEdgeVals); + + Json::Value feedbackParams = root.get("feedbackVals", "UTF-8"); + feedbackParams = feedbackParams.get("params", "UTF-8"); + + // Setup neural network + m_config.numStates = feedbackParams.get("numStates", "UTF-8").asInt(); + m_config.numActions = feedbackParams.get("numActions", "UTF-8").asInt(); + //m_config.numHidden = feedbackParams.get("numHidden", "UTF-8").asInt(); + + std::string nnFile = controlFilePath + feedbackParams.get("neuralFilename", "UTF-8").asString(); + + nn = new neuralNetwork(m_config.numStates, m_config.numStates*2, m_config.numActions); + + nn->loadWeights(nnFile.c_str()); + + initConditions = subject.getSegmentCOM(m_config.segmentNumber); + for (int i = 0; i < initConditions.size(); i++) + { + std::cout << initConditions[i] << " "; + } + std::cout << std::endl; +#ifdef LOGGING // Conditional compile for data logging + m_dataObserver.onSetup(subject); +#endif + +#if (0) // Conditional Compile for debug info + std::cout << *m_pCPGSys << std::endl; +#endif + m_updateTime = 0.0; + bogus = false; +} + +void JSONQuadFeedbackControl::onStep(BaseSpineModelLearning& subject, double dt) +{ + m_updateTime += dt; + if (m_updateTime >= m_config.controlTime) + { +#if (1) + std::vector desComs = getFeedback(subject); + +#else + std::size_t numControllers = subject.getNumberofMuslces() * 3; + + double descendingCommand = 0.0; + std::vector desComs (numControllers, descendingCommand); +#endif + try + { + m_pCPGSys->update(desComs, m_updateTime); + } + catch (std::runtime_error& e) + { + // Stops the trial immediately, lets teardown know it broke + bogus = true; + throw (e); + } + +#ifdef LOGGING // Conditional compile for data logging + m_dataObserver.onStep(subject, m_updateTime); +#endif + notifyStep(m_updateTime); + m_updateTime = 0; + } + + double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1]; + + /// Max and min heights added to config + if (currentHeight > m_config.maxHeight || currentHeight < m_config.minHeight) + { + /// @todo if bogus, stop trial (reset simulation) + bogus = true; + throw std::runtime_error("Height out of range"); + } +} + +void JSONQuadFeedbackControl::onTeardown(BaseSpineModelLearning& subject) +{ + scores.clear(); + // @todo - check to make sure we ran for the right amount of time + + std::vector finalConditions = subject.getSegmentCOM(m_config.segmentNumber); + + const double newX = finalConditions[0]; + const double newZ = finalConditions[2]; + const double oldX = initConditions[0]; + const double oldZ = initConditions[2]; + + const double distanceMoved = sqrt((newX-oldX) * (newX-oldX) + + (newZ-oldZ) * (newZ-oldZ)); + //double diff_of_sqrs = (newX-oldX)*(newX-oldX) - (newZ-oldZ)*(newZ-oldZ); + //const double distanceMoved = diff_of_sqrs >= 0 ? sqrt(diff_of_sqrs) : -sqrt(-diff_of_sqrs); + + if (bogus) + { + scores.push_back(-1.0); + } + else + { + scores.push_back(distanceMoved); + } + + /// @todo - consolidate with other controller classes. + /// @todo - return length scale as a parameter + double totalEnergySpent=0; + + std::vector tmpStrings = subject.find ("spine "); + + for(std::size_t i=0; igetHistory(); + + for(std::size_t j=1; j 0) // Vestigial code + motorSpeed = 0; + const double workDone = previousTension * motorSpeed; + totalEnergySpent += workDone; + } + } + + scores.push_back(totalEnergySpent); + + std::cout << "Dist travelled " << scores[0] << std::endl; + + Json::Value root; // will contains the root value after parsing. + Json::Reader reader; + + 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"); + } + + Json::Value prevScores = root.get("scores", Json::nullValue); + + Json::Value subScores; + subScores["distance"] = scores[0]; + subScores["energy"] = totalEnergySpent; + + prevScores.append(subScores); + root["scores"] = prevScores; + + ofstream payloadLog; + payloadLog.open(controlFilename.c_str(),ofstream::out); + + payloadLog << root << std::endl; + + delete m_pCPGSys; + m_pCPGSys = NULL; + + for (size_t b = 0; b < 5; b++) + { + for(size_t i = 0; i < m_controllers[b].size(); i++) + { + delete m_controllers[b][i]; + } + m_controllers[b].clear(); + } + + for (size_t i = 0; i < m_highControllers.size(); i++) + { + delete m_highControllers[i]; + } + m_highControllers.clear(); +} + +void JSONQuadFeedbackControl::setupCPGs(BaseSpineModelLearning& subject, array_2D nodeActions, array_4D edgeActions) +{ + CPGEquationsFB& m_CPGFBSys = *(tgCast::cast(m_pCPGSys)); + + // Make higher level CPGs, give them node params + for (std::size_t b = 0; b < 5; b++) + { + tgPIDController::Config config(20000.0, 0.0, 5.0, true); // Non backdrivable + tgCPGCableControl* pStringControl = new tgCPGCableControl(config); + + // need to make a 1 row 2D array so we can pass it into assignNodeNumber. + // using nodeActions will not work. (notice subtle name difference) ~B + array_2D nodeAction(boost::extents[1][nodeActions.shape()[1]]); + for (std::size_t a = 0; a < nodeActions.shape()[1]; a++) + nodeAction[0][a] = nodeActions[b][a]; + + // must give node number, to make things easier and stuff + pStringControl->assignNodeNumberFB(m_CPGFBSys, nodeAction); + + m_highControllers.push_back(pStringControl); + } + // So the higher level CPGs are indices 0-4 + + std::vector spineMuscles = subject.find ("spine "); + std::vector leftShoulderMuscles = subject.find ("left shoulder "); + std::vector leftHipMuscles = subject.find ("left hip "); + std::vector leftFrontLegMuscles = subject.find ("left front leg "); + std::vector leftHindLegMuscles = subject.find ("left hind leg "); + std::vector rightShoulderMuscles = subject.find ("right shoulder "); + std::vector rightHipMuscles = subject.find ("right hip "); + std::vector rightFrontLegMuscles = subject.find ("right front leg "); + std::vector rightHindLegMuscles = subject.find ("right hind leg "); + + std::vector allMuscles[9] = { spineMuscles, leftShoulderMuscles, leftFrontLegMuscles, rightShoulderMuscles, rightFrontLegMuscles, leftHipMuscles, leftHindLegMuscles, rightHipMuscles, rightHindLegMuscles }; + + // which muscle groups get which node params + const size_t node_assignment[9] = { 0, 1, 1, 2, 2, 3, 3, 4, 4 }; + for (std::size_t b = 0; b < 9; b++) + { + for (std::size_t i = 0; i < allMuscles[b].size(); i++) + { + + tgPIDController::Config config(20000.0, 0.0, 5.0, true); // Non backdrivable + tgCPGCableControl* pStringControl = new tgCPGCableControl(config); + + allMuscles[b][i]->attach(pStringControl); + + // need to make a 1 row 2D array so we can pass it into assignNodeNumber. + // using nodeActions will not work. (notice subtle name difference) ~B + array_2D nodeAction(boost::extents[1][nodeActions.shape()[1]]); + for (std::size_t a = 0; a < nodeActions.shape()[1]; a++) + nodeAction[0][a] = nodeActions[node_assignment[b]+5][a]; + // Why b+5? Because node params 0-4 have already been assigned to high level CPGs (above) + + // assign node number + pStringControl->assignNodeNumberFB(m_CPGFBSys, nodeAction); + + m_controllers[b].push_back(pStringControl); + } + } + + // Then determine connectivity and setup string + for (size_t b = 0; b < 9; b++) + { + for (std::size_t i = 0; i < m_controllers[b].size(); i++) + { + // The starting index (x and y) for the edgeActions matrix + size_t actionsStart = b == 0 ? 0 : 16 + 10*(b-1); + // There are 16 for spine, 10 for hip/shoulder/leg + size_t numMuscles = b == 0 ? 16 : 10; + + array_4D edgeActionsForBodyPart(boost::extents[3][numMuscles][numMuscles][edgeActions.shape()[3]]); + // I'm doing a nasty copy. boost's documentation for copying a subarray is too confusing. + for (size_t x = 0; x < 3; x++) for (size_t y = 0; y < numMuscles; y++) for (size_t z = 0; z < numMuscles; z++) for (size_t w = 0; w < edgeActions.shape()[3]; w++) + edgeActionsForBodyPart[x][y][z][w] = edgeActions[x][y+actionsStart][z+actionsStart][w]; + + tgCPGActuatorControl * const pStringInfo = m_controllers[b][i]; + assert(pStringInfo != NULL); + pStringInfo->setConnectivity(m_controllers[b], edgeActionsForBodyPart); + + //String will own this pointer + tgImpedanceController* p_ipc = new tgImpedanceController( m_config.tension, + m_config.kPosition, + m_config.kVelocity); + if (m_config.useDefault) + { + pStringInfo->setupControl(*p_ipc); + } + else + { + pStringInfo->setupControl(*p_ipc, m_config.controlLength); + } + } + } +} + +void JSONQuadFeedbackControl::setupHighCouplings(array_4D highEdgeActions) +{ + // Determine connectivity between higher level CPGs + for (std::size_t i = 0; i < m_highControllers.size(); i++) + { + tgCPGActuatorControl * const pStringInfo = m_highControllers[i]; + assert(pStringInfo != NULL); + pStringInfo->setConnectivity(m_highControllers, highEdgeActions); + + /* + //String will own this pointer + tgImpedanceController* p_ipc = new tgImpedanceController( m_config.tension, + m_config.kPosition, + m_config.kVelocity); + if (m_config.useDefault) + { + pStringInfo->setupControl(*p_ipc); + } + else + { + pStringInfo->setupControl(*p_ipc, m_config.controlLength); + } + */ + } +} + +void JSONQuadFeedbackControl::setupHighLowCouplings(Json::Value highLowEdgeActions) +{ + // Now couple high level with low level + // list of other nodenums, eq size list of weights/phases + //m_pCPGSystem->defineConnections(m_nodeNumber, connectivityList, weights, phases); + + double lowerLimit = m_config.lowPhase; + double upperLimit = m_config.highPhase; + double range = upperLimit - lowerLimit; + + CPGEquationsFB& m_CPGFBSys = *(tgCast::cast(m_pCPGSys)); + + Json::Value::iterator edgeIt = highLowEdgeActions.end(); + + // Number of node to start at (Note that nodes 0-4 are high CPGs) + int nodeStart = 5; + for (size_t b = 0; b < 5; b++) + { + edgeIt--; + + std::vector connectivityList; + std::vector weights; + std::vector phases; + + Json::Value param = *edgeIt; + assert(param.size() == 2); + + // Nasty brute forcing. Don't try this at home. Shield your eyes if necessary. + int n = b == 0 ? 16 : 20; + // 20 because each hip/shoulder + leg pair has 20 total strings + // ! + for (size_t i = 0; i < n; i++, nodeStart++) + { + connectivityList.push_back(nodeStart); + weights.push_back(param[0].asDouble()); + phases.push_back(param[1].asDouble()); + } + + m_CPGFBSys.defineConnections(b, connectivityList, weights, phases); + } + + assert(nodeStart == 16+10*8+5); + + // TODO? + assert(highLowEdgeActions.begin() == edgeIt); +} + +array_4D JSONQuadFeedbackControl::scaleEdgeActions + (Json::Value edgeParam) +{ + assert(edgeParam[0].size() == 2); + + double lowerLimit = m_config.lowPhase; + double upperLimit = m_config.highPhase; + double range = upperLimit - lowerLimit; + + array_4D actionList(boost::extents[m_config.segmentSpan] + [m_config.theirMuscles] + [m_config.ourMuscles] + [m_config.params]); + + /* Horrid while loop to populate upper diagonal of matrix, since + * its symmetric and we want to minimze parameters used in learing + * note that i==1, j==k will refer to the same muscle + * @todo use boost to set up array so storage is only allocated for + * elements that are used + */ + int i = 0; + int j = 0; + int k = 0; + + // Quirk of the old learning code. Future examples can move forward + Json::Value::iterator edgeIt = edgeParam.end(); + + int count = 0; + + int musc = 16; + for (size_t b = 0; b < 9; b++) + { + while (i < m_config.segmentSpan) + { + while(j < musc) + { + while(k < musc) + { + //cout << b << " " << i << " " << j << " " << k << endl; + if (edgeIt == edgeParam.begin()) + { + std::cout << "ran out before table populated!" + << std::endl; + /// @todo consider adding exception here + break; + } + else + { + if (i == 1 && j == k) + { + // std::cout << "Skipped identical muscle" << std::endl; + //Skip since its the same muscle + } + else + { + edgeIt--; + Json::Value edgeParam1 = *edgeIt; + assert(edgeParam1.size() == 2); + // Weight from 0 to 1 + actionList[i][j][k][0] = edgeParam1[0].asDouble(); + //std::cout << actionList[i][j][k][0] << " "; + // Phase offset from -pi to pi + actionList[i][j][k][1] = edgeParam1[1].asDouble() * + (range) + lowerLimit; + //actionList[i][j][k][0] = 0.0; + //actionList[i][j][k][1] = 0.0; + //std::cout << actionList[i][j][k][1] << std::endl; + count++; + } + } + k++; + } + j++; + k = j; + + } + j = musc - (b > 0 ? 10 : 16); + k = j; + i++; + } + j = musc; + k = musc; + i = 0; + musc += 10; + } + + std::cout<< "Params used: " << count << std::endl; + + assert(edgeParam.begin() == edgeIt); + + return actionList; +} + +array_4D JSONQuadFeedbackControl::scaleHighEdgeActions (Json::Value highEdgeParam) +{ + assert(highEdgeParam[0].size() == 2); + + double lowerLimit = m_config.lowPhase; + double upperLimit = m_config.highPhase; + double range = upperLimit - lowerLimit; + + // This is a 4d array, but we don't really worry about 1st dimension + // since these higher CPGs aren't attached to any rigid bodies + array_4D actionList(boost::extents[m_config.segmentSpan] + [5] + [5] + [m_config.params]); + + /* Horrid while loop to populate upper diagonal of matrix, since + * its symmetric and we want to minimze parameters used in learing + * note that i==1, j==k will refer to the same muscle + * @todo use boost to set up array so storage is only allocated for + * elements that are used + */ + int i = 0; + int j = 0; + int k = 0; + + // Quirk of the old learning code. Future examples can move forward + Json::Value::iterator highEdgeIt = highEdgeParam.end(); + + int count = 0; + + while(j < 5) + { + while(k < 5) + { + if (j == k) + { + // std::cout << "Skipped identical muscle" << std::endl; + //Skip since its the same muscle + } + else + { + if (highEdgeIt == highEdgeParam.begin()) + { + std::cout << "ran out before table populated!" + << std::endl; + /// @todo consider adding exception here + break; + } + else + { + //cout << j << " " << k << endl; + highEdgeIt--; + Json::Value edgeParam1 = *highEdgeIt; + assert(edgeParam1.size() == 2); + // Weight from 0 to 1 + while (i < m_config.segmentSpan) + { + actionList[i][j][k][0] = edgeParam1[0].asDouble(); + //std::cout << actionList[i][j][k][0] << " "; + // Phase offset from -pi to pi + actionList[i][j][k][1] = edgeParam1[1].asDouble() * + (range) + lowerLimit; + //std::cout << actionList[i][j][k][1] << std::endl; + i++; + } + i = 0; + count++; + } + } + k++; + } + j++; + k = j; + } + + std::cout<< "Params used: " << count << std::endl; + + assert(highEdgeParam.begin() == highEdgeIt); + + return actionList; +} + +array_2D JSONQuadFeedbackControl::scaleNodeActions (Json::Value actions) +{ + std::size_t numControllers = actions.size(); + std::size_t numActions = actions[0].size(); + + array_2D nodeActions(boost::extents[numControllers][numActions]); + + array_2D limits(boost::extents[2][numActions]); + + // Check if we need to update limits + assert(numActions == 5); + + limits[0][0] = m_config.lowFreq; + limits[1][0] = m_config.highFreq; + limits[0][1] = m_config.lowAmp; + limits[1][1] = m_config.highAmp; + limits[0][2] = m_config.freqFeedbackMin; + limits[1][2] = m_config.freqFeedbackMax; + limits[0][3] = m_config.ampFeedbackMin; + limits[1][3] = m_config.ampFeedbackMax; + limits[0][4] = m_config.phaseFeedbackMin; + limits[1][4] = m_config.phaseFeedbackMax; + + Json::Value::iterator nodeIt = actions.begin(); + + // This one is square + for( std::size_t i = 0; i < numControllers; i++) + { + if (i < 5) + { + limits[0][2] = m_config.highFreqFeedbackMin; + limits[1][2] = m_config.highFreqFeedbackMax; + } + Json::Value nodeParam = *nodeIt; + for( std::size_t j = 0; j < numActions; j++) + { + nodeActions[i][j] = ( (nodeParam.get(j, 0.0)).asDouble() * + (limits[1][j] - limits[0][j])) + limits[0][j]; + } + nodeIt++; + } + + return nodeActions; +} + +std::vector JSONQuadFeedbackControl::getFeedback(BaseSpineModelLearning& subject) +{ + // Placeholder + std::vector feedback; + + const std::vector& spineCables = subject.find ("spine "); + const std::vector& leftShoulderCables = subject.find ("left shoulder "); + const std::vector& leftHipCables = subject.find ("left hip "); + const std::vector& leftFrontLegCables = subject.find ("left front leg "); + const std::vector& leftHindLegCables = subject.find ("left hind leg "); + const std::vector& rightShoulderCables = subject.find ("right shoulder "); + const std::vector& rightHipCables = subject.find ("right hip "); + const std::vector& rightFrontLegCables = subject.find ("right front leg "); + const std::vector& rightHindLegCables = subject.find ("right hind leg "); + + const std::vector allCables[9] = { spineCables, leftShoulderCables, leftFrontLegCables, rightShoulderCables, rightFrontLegCables, leftHipCables, leftHindLegCables, rightHipCables, rightHindLegCables }; + + double *inputs = new double[m_config.numStates]; + + for (size_t b = 0; b < 9; b++) + { + std::size_t n = allCables[b].size(); + for(std::size_t i = 0; i != n; i++) + { + std::vector< std::vector > actions; + + const tgSpringCableActuator& cable = *(allCables[b][i]); + std::vector state = getCableState(cable); + + // Rescale to 0 to 1 (consider doing this inside getState + for (std::size_t i = 0; i < state.size(); i++) + { + inputs[i]=state[i] / 2.0 + 0.5; + } + + double *output = nn->feedForwardPattern(inputs); + vector tmpAct; + for(int j=0;j cableFeedback = transformFeedbackActions(actions); + + feedback.insert(feedback.end(), cableFeedback.begin(), cableFeedback.end()); + } + } + + // inputting 0 for now + std::size_t n = m_highControllers.size(); + for (size_t i = 0; i != n; i++) + { + std::vector< std::vector > actions; + + // Rescale to 0 to 1 (consider doing this inside getState + for (std::size_t i = 0; i < m_config.numStates; i++) + { + inputs[i] = 0; + } + + double *output = nn->feedForwardPattern(inputs); + vector tmpAct; + for(int j=0;j cableFeedback = transformFeedbackActions(actions); + + feedback.insert(feedback.end(), cableFeedback.begin(), cableFeedback.end()); + } + + return feedback; +} + +std::vector JSONQuadFeedbackControl::getCableState(const tgSpringCableActuator& cable) +{ + // For each string, scale value from -1 to 1 based on initial length or max tension of motor + + std::vector state; + + // Scale length by starting length + const double startLength = cable.getStartLength(); + state.push_back((cable.getCurrentLength() - startLength) / startLength); + + const double maxTension = cable.getConfig().maxTens; + state.push_back((cable.getTension() - maxTension / 2.0) / maxTension); + + return state; +} + +std::vector JSONQuadFeedbackControl::transformFeedbackActions(std::vector< std::vector >& actions) +{ + // Placeholder + std::vector feedback; + + // Leave in place for generalization later + const std::size_t numControllers = 1; + const std::size_t numActions = m_config.numActions; + + assert( actions.size() == numControllers); + assert( actions[0].size() == numActions); + + // Scale values back to -1 to +1 + for( std::size_t i = 0; i < numControllers; i++) + { + for( std::size_t j = 0; j < numActions; j++) + { + feedback.push_back(actions[i][j] * 2.0 - 1.0); + } + } + + return feedback; +} + diff --git a/src/dev/bgigous/MG_FirstAttempt/JSONQuadFeedbackControl.h b/src/dev/bgigous/MG_FirstAttempt/JSONQuadFeedbackControl.h new file mode 100644 index 000000000..93698c4ac --- /dev/null +++ b/src/dev/bgigous/MG_FirstAttempt/JSONQuadFeedbackControl.h @@ -0,0 +1,147 @@ +/* + * 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. +*/ + +#ifndef JSON_QUAD_FEEDBACK_CONTROL_H +#define JSON_QUAD_FEEDBACK_CONTROL_H + +/** + * @file JSONFeedbackControl.h + * @brief A controller for the template class BaseSpineModelLearning + * @author Brian Mirletz + * @version 1.1.0 + * $Id$ + */ + +#include "dev/btietz/JSONTests/JSONCPGControl.h" + +#include + +// Forward Declarations +class neuralNetwork; +class tgSpringCableActuator; + +/** + * JSONFeedbackControl learns the parameters for a CPG system on a + * spine like tensegrity structure specified as a BaseSpineModelLearning. Parameters are generated by + * AnnealEvolution and used in the CPGEquations family of classes. + * tgImpedanceController controllers are used for the detailed muscle control. + * Due to the number of parameters, the learned parameters are split + * into one config file for the nodes and another for the CPG's "edges" + */ +class JSONQuadFeedbackControl : public JSONCPGControl +{ +public: + +struct Config : public JSONCPGControl::Config + { + public: + /** + * The only constructor. + */ + Config( int ss, + int tm, + int om, + int param, + int segnum = 6, + double ct = 0.1, + double la = 0, + double ha = 30, + double lp = -1 * M_PI, + double hp = M_PI, + double kt = 0.0, + double kp = 1000.0, + double kv = 100.0, + bool def = true, + double cl = 10.0, + double lf = 0.0, + double hf = 30.0, + double ffMin = 0.0, + double ffMax = 0.0, + double afMin = 0.0, + double afMax = 0.0, + double pfMin = 0.0, + double pfMax = 0.0, + double maxH = 60.0, //May need to tune this value more + double minH = 1.0, //Perhaps same + double hffMin = 0.0, + double hffMax = 0.0 + ); + + const double freqFeedbackMin; + const double freqFeedbackMax; + const double ampFeedbackMin; + const double ampFeedbackMax; + const double phaseFeedbackMin; + const double phaseFeedbackMax; + + const double maxHeight; + const double minHeight; + + const double highFreqFeedbackMin; + const double highFreqFeedbackMax; + + // Values to be filled in by JSON file during onSetup + int numStates; + int numActions; + + }; + + JSONQuadFeedbackControl(JSONQuadFeedbackControl::Config config, + std::string args, + std::string resourcePath = ""); + + virtual ~JSONQuadFeedbackControl(); + + virtual void onSetup(BaseSpineModelLearning& subject); + + virtual void onStep(BaseSpineModelLearning& subject, double dt); + + virtual void onTeardown(BaseSpineModelLearning& subject); + +protected: + + virtual void setupCPGs(BaseSpineModelLearning& subject, array_2D nodeActions, array_4D edgeActions); + virtual void setupHighCouplings(array_4D highEdgeActions); + // I'm cheating and just using the Json thing directly. ~B + virtual void setupHighLowCouplings(Json::Value highLowEdgeActions); + + virtual array_4D scaleEdgeActions (Json::Value edgeParam); + virtual array_2D scaleNodeActions (Json::Value actions); + virtual array_4D scaleHighEdgeActions (Json::Value highEdgeParam); + + std::vector getFeedback(BaseSpineModelLearning& subject); + + std::vector getCableState(const tgSpringCableActuator& cable); + + std::vector transformFeedbackActions(std::vector< std::vector >& actions); + + JSONQuadFeedbackControl::Config m_config; + + // Hacky and probably temporary. + // Make array of 5 basic body parts. Each body part has vector of CPG controllers. + std::vector m_controllers[9]; + + // Higher level CPGs + std::vector m_highControllers; + + /// @todo generalize this if we need more than one + neuralNetwork* nn; + +}; + +#endif // JSON_QUAD_FEEDBACK_CONTROL_H diff --git a/src/dev/bgigous/MG_FirstAttempt/README.txt b/src/dev/bgigous/MG_FirstAttempt/README.txt new file mode 100644 index 000000000..ee5856c19 --- /dev/null +++ b/src/dev/bgigous/MG_FirstAttempt/README.txt @@ -0,0 +1 @@ +This project is a first attempt at being able to couple the CPGs in the legs, hips, and spine in the MountainGoat without making the state space gigantic. Based on code from src/dev/dhusticschultz/BP_SC_SymmetricSpiral2.