diff --git a/scripts/learning/src/MG_Spec.json b/scripts/learning/src/MG_Spec.json new file mode 100644 index 000000000..9f5df190b --- /dev/null +++ b/scripts/learning/src/MG_Spec.json @@ -0,0 +1,70 @@ +{ + "filePrefix" : "monteOut", + "fileSuffix" : ".json", + "resourcePath" : "../../../resources/src/", + "lowerPath" : "dhustigschultz/AppMGControl/", + "executable" : "./../../../build/dev/dhustigschultz/MG_Controller/AppMGControl", + "terrain" : [[[0, 0, 0, 0.0, 60000]]], + "learningParams": + { + "trialLength" : 60000, + "numTrials" : 12, + "numGenerations" : 1, + "deterministic": 0, + "nodeVals" : + { + "learning" : true, + "startingControllers" : 0, + "monteCarlo" : true, + "numberOfStates" : 0, + "numberOfOutputs" : 5, + "numberOfInstances" : 1, + "populationSize" : 12, + "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" : 11476, + "populationSize" : 12, + "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" : 12, + "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/dhustigschultz/CMakeLists.txt b/src/dev/dhustigschultz/CMakeLists.txt index 82da8c920..677da3a56 100644 --- a/src/dev/dhustigschultz/CMakeLists.txt +++ b/src/dev/dhustigschultz/CMakeLists.txt @@ -31,4 +31,6 @@ subdirs( BP_SC_SymmetricSpiral3 BigPuppy_SymmetricSpiral_Metrics + MountainGoat + MG_Controller ) diff --git a/src/dev/dhustigschultz/MG_Controller/AppMGControl.cpp b/src/dev/dhustigschultz/MG_Controller/AppMGControl.cpp new file mode 100644 index 000000000..a86f1647c --- /dev/null +++ b/src/dev/dhustigschultz/MG_Controller/AppMGControl.cpp @@ -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 AppMGControl.cpp + * @brief Using Brian's existing spine controller for a quadruped, using more muscles to increase torsion. + * @author Dawn Hustig-Schultz, Brian Mirletz + * @date April. 2016 + * @version 1.0.0 + * $Id$ + */ + +#include "AppMGControl.h" +#include "dev/btietz/JSONTests/tgCPGJSONLogger.h" + +AppMGControl::AppMGControl(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 AppMGControl::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; + + MountainGoat* myModel = new MountainGoat(segments, hips, legs); + + // Fifth create the controllers, attach to model + if (add_controller) + { + const int segmentSpan = 3;//3; //Not sure what this will be for mine! + const int numMuscles = 112 + 40; //+ 40; //Changed for experiment. Learning different params for all strings in the spine! + 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 = 1.0; + + JSONMGFeedbackControl::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 + JSONMGFeedbackControl* const myControl = + new JSONMGFeedbackControl(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 AppMGControl::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 AppMGControl::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 AppMGControl::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* AppMGControl::getBlocks() +{ + // Room to add a config + tgBlockField* myObstacle = new tgBlockField(); + return myObstacle; +} + +tgWorld* AppMGControl::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 *AppMGControl::createGraphicsView(tgWorld *world) +{ + return new tgSimViewGraphics(*world, timestep_physics, timestep_graphics); +} + +tgSimView *AppMGControl::createView(tgWorld *world) +{ + return new tgSimView(*world, timestep_physics, timestep_graphics); +} + +bool AppMGControl::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 AppMGControl::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 << "AppMGControl" << std::endl; + AppMGControl 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/dhustigschultz/MG_Controller/AppMGControl.h b/src/dev/dhustigschultz/MG_Controller/AppMGControl.h new file mode 100644 index 000000000..f136e9e42 --- /dev/null +++ b/src/dev/dhustigschultz/MG_Controller/AppMGControl.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_MG_CONTROL_H +#define APP_MG_CONTROL_H + +/** + * @file AppMGControl.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 + * $Id$ + */ + +//robot +#include "dev/dhustigschultz/MountainGoat/MountainGoat.h" + +// controller +#include "JSONMGFeedbackControl.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 AppMGControl +{ +public: + AppMGControl(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_MG_CONTROL_H + diff --git a/src/dev/dhustigschultz/MG_Controller/CMakeLists.txt b/src/dev/dhustigschultz/MG_Controller/CMakeLists.txt new file mode 100644 index 000000000..17b226aba --- /dev/null +++ b/src/dev/dhustigschultz/MG_Controller/CMakeLists.txt @@ -0,0 +1,37 @@ +link_libraries(boost_program_options + tgcreator + core + util + terrain + Adapters + Configuration + AnnealEvolution + tgOpenGLSupport + obstacles + KinematicString + CPG_feedback + sensors + controllers + BaseQuadModelLearning + MountainGoat) + +add_library(tgCPGGMGActuatorControl + tgCPGMGActuatorControl.cpp) + +add_library(tgCPGMGCableControl + tgCPGMGCableControl.cpp) + +add_library(JSONMGCPGGeneralControl + JSONMGCPGGeneralControl.cpp) + +add_library(JSONMGFeedbackControl + JSONMGFeedbackControl.cpp) + +add_executable(AppMGControl + AppMGControl.cpp + JSONMGFeedbackControl.cpp + JSONMGCPGGeneralControl.cpp + tgCPGMGCableControl.cpp + tgCPGMGActuatorControl.cpp) + +target_link_libraries(AppMGControl ${ENV_LIB_DIR}/libjsoncpp.a FileHelpers boost_program_options obstacles) diff --git a/src/dev/dhustigschultz/MG_Controller/JSONMGCPGGeneralControl.cpp b/src/dev/dhustigschultz/MG_Controller/JSONMGCPGGeneralControl.cpp new file mode 100644 index 000000000..7cee854a7 --- /dev/null +++ b/src/dev/dhustigschultz/MG_Controller/JSONMGCPGGeneralControl.cpp @@ -0,0 +1,496 @@ +/* + * 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 JSONMGCPGGeneralControl.cpp + * @brief A controller for the template class BaseSpineModelLearning + * @author Brian Mirletz, Dawn Hustig-Schultz + * @version 1.1.0 + * $Id$ + */ + +#include "JSONMGCPGGeneralControl.h" + +#include + + +// 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 "controllers/tgImpedanceController.h" +#include "tgCPGMGActuatorControl.h" +#include "tgCPGMGCableControl.h" +#include "examples/learningSpines/BaseSpineCPGControl.h" + +#include "helpers/FileHelpers.h" + +#include "util/CPGEquations.h" +#include "util/CPGNode.h" + +// JSON +#include + +#include + +//#define LOGGING + +using namespace std; + +JSONMGCPGGeneralControl::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) : + segmentSpan(ss), + theirMuscles(tm), + ourMuscles(om), + params(param), + segmentNumber(segnum), + controlTime(ct), + lowAmp(la), + highAmp(ha), + lowFreq(lf), + highFreq(hf), + lowPhase(lp), + highPhase(hp), + tension(kt), + kPosition(kp), + kVelocity(kv), + useDefault(def), + controlLength(cl) +{ + if (ss <= 0) + { + throw std::invalid_argument("segmentSpan parameter is negative."); + } + else if (tm <= 0) + { + throw std::invalid_argument("theirMuscles parameter is negative."); + } + else if (om <= 0) + { + throw std::invalid_argument("Our Muscles parameter is negative."); + } + else if (param <= 0) + { + throw std::invalid_argument("Edge parameters is negative."); + } + else if (segnum < 0) + { + throw std::invalid_argument("Segment number is negative."); + } + else if (ct < 0.0) + { + throw std::invalid_argument("control time is negative."); + } + else if (kt < 0.0) + { + throw std::invalid_argument("impedance control tension is negative."); + } + else if (kp < 0.0) + { + throw std::invalid_argument("impedance control position is negative."); + } + else if (kv < 0.0) + { + throw std::invalid_argument("impedance control velocity is negative."); + } + else if (cl < 0.0) + { + throw std::invalid_argument("Control Length is negative."); + } +} + +/** + * 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 + */ +JSONMGCPGGeneralControl::JSONMGCPGGeneralControl(JSONMGCPGGeneralControl::Config config, + std::string args, + std::string resourcePath) : +m_pCPGSys(NULL), +m_config(config), +m_dataObserver("logs/TCData"), +m_updateTime(0.0), +bogus(false) +{ + if (resourcePath != "") + { + controlFilePath = FileHelpers::getResourcePath(resourcePath); + } + else + { + controlFilePath = ""; + } + + controlFilename = controlFilePath + args; +} + +JSONMGCPGGeneralControl::~JSONMGCPGGeneralControl() +{ + scores.clear(); +} + +void JSONMGCPGGeneralControl::onSetup(BaseQuadModelLearning& subject) +{ + // Maximum number of sub-steps allowed by CPG + m_pCPGSys = new CPGEquations(2000); + //Initialize the Learning Adapters + + 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"); + + nodeVals = nodeVals.get("params", "UTF-8"); + edgeVals = edgeVals.get("params", "UTF-8"); + + array_3D edgeParams = scaleEdgeActions(edgeVals); + array_2D nodeParams = scaleNodeActions(nodeVals); + + setupCPGs(subject, nodeParams, edgeParams); + + initConditions = subject.getSegmentCOM(m_config.segmentNumber); +#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 JSONMGCPGGeneralControl::setupCPGs(BaseQuadModelLearning& subject, array_2D nodeActions, array_3D edgeActions) +{ + + std::vector allMuscles = subject.getAllMuscles(); + + for (std::size_t i = 0; i < allMuscles.size(); i++) + { +#if (1) + tgPIDController::Config config(20000.0, 0.0, 5.0, true); // Non backdrivable + tgCPGMGCableControl* pStringControl = new tgCPGMGCableControl(config); +#else + tgCPGMGActuatorControl* pStringControl = new tgCPGMGActuatorControl(); +#endif // Update for kinematic cables + allMuscles[i]->attach(pStringControl); + + m_allControllers.push_back(pStringControl); + } + + /// @todo: redo with for_each + // First assign node numbers to the info Classes + for (std::size_t i = 0; i < m_allControllers.size(); i++) + { + m_allControllers[i]->assignNodeNumber(*m_pCPGSys, nodeActions); + } + + // Then determine connectivity and setup string + for (std::size_t i = 0; i < m_allControllers.size(); i++) + { + tgCPGMGActuatorControl * const pStringInfo = m_allControllers[i]; + assert(pStringInfo != NULL); + pStringInfo->setConnectivity(m_allControllers, edgeActions); + + //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 JSONMGCPGGeneralControl::onStep(BaseQuadModelLearning& subject, double dt) +{ + m_updateTime += dt; + if (m_updateTime >= m_config.controlTime) + { + std::size_t numControllers = subject.getNumberofMuslces(); + + double descendingCommand = 2.0; + std::vector desComs (numControllers, descendingCommand); + + m_pCPGSys->update(desComs, m_updateTime); +#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]; + + /// @todo add to config + if (currentHeight > 25 || currentHeight < 1.0) + { + /// @todo if bogus, stop trial (reset simulation) + bogus = true; + } +} + +void JSONMGCPGGeneralControl::onTeardown(BaseQuadModelLearning& 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)); + + 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.getAllMuscles(); + + 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 i = 0; i < m_allControllers.size(); i++) + { + delete m_allControllers[i]; + } + m_allControllers.clear(); +} + +const double JSONMGCPGGeneralControl::getCPGValue(std::size_t i) const +{ + // Error handling on input done in CPG_Equations + return (*m_pCPGSys)[i]; +} + +double JSONMGCPGGeneralControl::getScore() const +{ + if (scores.size() == 2) + { + return scores[0]; + } + else + { + throw std::runtime_error("Called before scores were obtained!"); + } +} + + +array_3D JSONMGCPGGeneralControl::scaleEdgeActions + (Json::Value edgeParam) +{ + assert(edgeParam[0].size() == 2); + + double lowerLimit = m_config.lowPhase; + double upperLimit = m_config.highPhase; + double range = upperLimit - lowerLimit; + + array_3D actionList(boost::extents[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 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 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; + + + while(j < m_config.theirMuscles) + { + while(k < m_config.ourMuscles) + { + if (edgeIt == edgeParam.begin()) + { + std::cout << "ran out before table populated!" + << std::endl; + /// @todo consider adding exception here + break; + } + else + { + if (j == k) + { + // std::cout << "Skipped identical muscle" << std::endl; + //Skip since its the same muscle + } + else + { + edgeIt--; + Json::Value edgeParam = *edgeIt; + assert(edgeParam.size() == 2); + // Weight from 0 to 1 + actionList[j][k][0] = edgeParam[0].asDouble(); + //std::cout << actionList[j][k][0] << " "; + // Phase offset from -pi to pi + actionList[j][k][1] = edgeParam[1].asDouble() * + (range) + lowerLimit; + //std::cout << actionList[j][k][1] << std::endl; + count++; + } + } + k++; + } + j++; + k = j; + + } + + + std::cout<< "Params used: " << count << std::endl; + + assert(edgeParam.begin() == edgeIt); + + return actionList; +} +array_2D JSONMGCPGGeneralControl::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 == 2); + + limits[0][0] = m_config.lowFreq; + limits[1][0] = m_config.highFreq; + limits[0][1] = m_config.lowAmp; + limits[1][1] = m_config.highAmp; + + Json::Value::iterator nodeIt = actions.begin(); + + // This one is square + for( std::size_t i = 0; i < numControllers; i++) + { + 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; +} diff --git a/src/dev/dhustigschultz/MG_Controller/JSONMGCPGGeneralControl.h b/src/dev/dhustigschultz/MG_Controller/JSONMGCPGGeneralControl.h new file mode 100644 index 000000000..4280dcfa6 --- /dev/null +++ b/src/dev/dhustigschultz/MG_Controller/JSONMGCPGGeneralControl.h @@ -0,0 +1,168 @@ +/* + * 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_MG_CPG_GENERAL_CONTROL_H +#define JSON_MG_CPG_GENERAL_CONTROL_H + +/** + * @file JSONMGCPGGeneralControl.h + * @brief An adaptation of JSONCPGControl that utilizes JSON for parameters + * @brief Modified for use with quadrupeds, so that certain metrics can be gathered. Modified to use 3D multi_array, + * since can't override 4D in the original base class. + * @author Brian Mirletz, Dawn Hustig-Schultz + * @date April 2016 + * @version 1.1.0 + * $Id$ + */ + +#include "dev/dhustigschultz/MountainGoat/MountainGoat.h" + +#include +#include "boost/multi_array.hpp" + +#include "core/tgSubject.h" +#include "core/tgObserver.h" +#include "sensors/tgDataObserver.h" + +#include + +// Forward Declarations +class tgImpedanceController; +class tgCPGMGActuatorControl; +class CPGEquations; +class tgCPGLogger; +class BaseSpineModelLearning; +class BaseSpineCPGControl; + +typedef boost::multi_array array_2D; +typedef boost::multi_array array_3D; + +/** + * JSONMGCPGGeneralControl learns the parameters for a CPG system on a + * spine like tensegrity structure specified as a BaseQuadModelLearning. 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 JSONMGCPGGeneralControl : public tgObserver, public tgSubject +{ +public: + +struct 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); + + // Learning Parameters + const int segmentSpan; // 3 possible muscles touching two rigid bodies + const int theirMuscles; // 8 muscles in a segment + const int ourMuscles; // same as above + const int params; // Number of parameters per edge + const int segmentNumber; + + // CPG control frequency + const double controlTime; + + // Limit Params + const double lowAmp; + const double highAmp; + const double lowFreq; + const double highFreq; + const double lowPhase; + const double highPhase; + + // Parameters for Impedance Controllers + const double tension; + const double kPosition; + const double kVelocity; + const bool useDefault; + const double controlLength; + }; + + JSONMGCPGGeneralControl(JSONMGCPGGeneralControl::Config config, + std::string args, + std::string resourcePath = ""); + + virtual ~JSONMGCPGGeneralControl(); + + virtual void onStep(BaseQuadModelLearning& subject, double dt); + + virtual void onSetup(BaseQuadModelLearning& subject); + + virtual void onTeardown(BaseQuadModelLearning& subject); + + const double getCPGValue(std::size_t i) const; + + double getScore() const; + +protected: + /** + * Takes a vector of parameters reported by learning, and then + * converts it into a format used to assign to the CPGEdges + * Note that if the CPG edges change, this will need to change + */ + virtual array_3D scaleEdgeActions (Json::Value edgeParam); + virtual array_2D scaleNodeActions (Json::Value actions); + + virtual void setupCPGs(BaseQuadModelLearning& subject, array_2D nodeActions, array_3D edgeActions); + + CPGEquations* m_pCPGSys; + + std::vector m_allControllers; + + JSONMGCPGGeneralControl::Config m_config; + + std::vector initConditions; + + std::size_t segments; + + tgDataObserver m_dataObserver; + + double m_updateTime; + double m_totalTime; + + std::vector scores; + std::vector metrics; + + bool bogus; + + std::string controlFilename; + std::string controlFilePath; +}; + +#endif // JSON_MG_CPG_GENERAL_CONTROL_H diff --git a/src/dev/dhustigschultz/MG_Controller/JSONMGFeedbackControl.cpp b/src/dev/dhustigschultz/MG_Controller/JSONMGFeedbackControl.cpp new file mode 100644 index 000000000..2a36408bb --- /dev/null +++ b/src/dev/dhustigschultz/MG_Controller/JSONMGFeedbackControl.cpp @@ -0,0 +1,630 @@ +/* + * 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 JSONMGFeedbackControl.cpp + * @brief A controller for the template class BaseSpineModelLearning. + * @Includes more metrics, such as center of mass of entire structure. + * @author Brian Mirletz, Dawn Hustig-Schultz + * @version 1.1.0 + * $Id$ + */ + +#include "JSONMGFeedbackControl.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 "tgCPGMGActuatorControl.h" +#include "tgCPGMGCableControl.h" + +#include "dev/dhustigschultz/MountainGoat/MountainGoat.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; + +JSONMGFeedbackControl::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) : +JSONMGCPGGeneralControl::Config::Config(ss, tm, om, param, segnum, ct, la, ha, + lp, hp, kt, kp, kv, def, cl, lf, hf), +freqFeedbackMin(ffMin), +freqFeedbackMax(ffMax), +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 + */ +JSONMGFeedbackControl::JSONMGFeedbackControl(JSONMGFeedbackControl::Config config, + std::string args, + std::string resourcePath) : +JSONMGCPGGeneralControl(config, args, resourcePath), +m_config(config) +{ + // Path and filename handled by base class + +} + +JSONMGFeedbackControl::~JSONMGFeedbackControl() +{ + delete nn; +} + +void JSONMGFeedbackControl::onSetup(BaseQuadModelLearning& subject) +{ + m_pCPGSys = new CPGEquationsFB(1000000); + + 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"); + + array_3D edgeParams = scaleEdgeActions(edgeVals); + array_2D nodeParams = scaleNodeActions(nodeVals); + + setupCPGs(subject, nodeParams, edgeParams); + + 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; + m_totalTime = 0.0; //For metrics. + bogus = false; + + metrics.clear(); + + //Getting the center of mass of the entire structure. + std::vector structureCOM = subject.getCOM(m_config.segmentNumber); + + for(std::size_t i=0; i<3; i++) + { + metrics.push_back(structureCOM[i]); + } + + //"metrics" is a new section of the controller's JSON file that is + //added in the getNewFile function in evolution_job_master.py + Json::Value prevMetrics = root.get("metrics", Json::nullValue); + + Json::Value subMetrics; + subMetrics["initial COM x"] = metrics[0]; + subMetrics["initial COM y"] = metrics[1]; + subMetrics["initial COM z"] = metrics[2]; + + prevMetrics.append(subMetrics); + root["metrics"] = prevMetrics; + + ofstream payloadLog; + payloadLog.open(controlFilename.c_str(),ofstream::out); + + payloadLog << root << std::endl; +} + +void JSONMGFeedbackControl::onStep(BaseQuadModelLearning& subject, double dt) +{ + m_updateTime += dt; + m_totalTime += 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"); + } + //every 100 steps, get the COM and tensions of active muscles and store them in the JSON file. + if(0){ + static int count = 0; + if(count > 100) { + std::cout << m_totalTime << std::endl; + + //Getting the center of mass of the entire structure. + std::vector structureCOM = subject.getCOM(m_config.segmentNumber); + std::cout << "COM: " << structureCOM[0] << " " << structureCOM[1] << " " << structureCOM[2] << " "; + std::cout << std::endl; + //Clear the metrics vector for ease of adding tensions. + //metrics.clear(); + + std::vector tmpStrings = subject.find ("all "); + + for(std::size_t i=0; igetTension() << std::endl; + } + std::cout << std::endl; + + for(std::size_t i=0; igetCurrentLength() << std::endl; + } + std::cout << std::endl; + + count = 0; + } + else { + count++; + } + } +} + +void JSONMGFeedbackControl::onTeardown(BaseQuadModelLearning& subject) +{ + scores.clear(); + metrics.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)); + + 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 ("all "); + + 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); + + //Getting the center of mass of the entire structure. + std::vector structureCOM = subject.getCOM(m_config.segmentNumber); + + for(std::size_t i=0; i<3; i++) + { + metrics.push_back(structureCOM[i]); + } + + std::cout << "Dist travelled " << scores[0] << std::endl; + + Json::Value root; // will contain 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 prevMetrics = root.get("metrics", Json::nullValue); + + Json::Value subScores; + subScores["distance"] = scores[0]; + subScores["energy"] = scores[1]; + + Json::Value subMetrics; + subMetrics["final COM x"] = metrics[0]; + subMetrics["final COM y"] = metrics[1]; + subMetrics["final COM z"] = metrics[2]; + + prevScores.append(subScores); + prevMetrics.append(subMetrics); + + root["scores"] = prevScores; + root["metrics"] = prevMetrics; + + ofstream payloadLog; + payloadLog.open(controlFilename.c_str(),ofstream::out); + + payloadLog << root << std::endl; + + delete m_pCPGSys; + m_pCPGSys = NULL; + + for(size_t i = 0; i < m_spineControllers.size(); i++) + { + delete m_spineControllers[i]; + } + m_spineControllers.clear(); +} + +void JSONMGFeedbackControl::setupCPGs(BaseQuadModelLearning& subject, array_2D nodeActions, array_3D edgeActions) +{ + + std::vector spineMuscles = subject.find ("all "); + + CPGEquationsFB& m_CPGFBSys = *(tgCast::cast(m_pCPGSys)); + + for (std::size_t i = 0; i < spineMuscles.size(); i++) + { + + tgPIDController::Config config(20000.0, 0.0, 5.0, true); // Non backdrivable + tgCPGMGCableControl* pStringControl = new tgCPGMGCableControl(config); + + spineMuscles[i]->attach(pStringControl); + + // First assign node numbers + pStringControl->assignNodeNumberFB(m_CPGFBSys, nodeActions); + + m_spineControllers.push_back(pStringControl); + } + + // Then determine connectivity and setup string + for (std::size_t i = 0; i < m_spineControllers.size(); i++) + { + tgCPGMGActuatorControl * const pStringInfo = m_spineControllers[i]; + assert(pStringInfo != NULL); + pStringInfo->setConnectivity(m_spineControllers, edgeActions); //May need a new function, written in a subclass, to deal with the long muscles, though this will be fine for segments 1 through 5. + + //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); + } + } + +} + +array_2D JSONMGFeedbackControl::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++) + { + 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; +} + + +array_3D JSONMGFeedbackControl::scaleEdgeActions + (Json::Value edgeParam) +{ + assert(edgeParam[0].size() == 2); + + double lowerLimit = m_config.lowPhase; + double upperLimit = m_config.highPhase; + double range = upperLimit - lowerLimit; + + array_3D actionList(boost::extents[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 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; + + while(j < m_config.theirMuscles) + { + while(k < m_config.ourMuscles) + { + if (edgeIt == edgeParam.begin()) + { + std::cout << "ran out before table populated!" + << std::endl; + /// @todo consider adding exception here + break; + } + else + { + if (j == k) + { + // std::cout << "Skipped identical muscle" << std::endl; + //Skip since its the same muscle + } + else + { + edgeIt--; + Json::Value edgeParam = *edgeIt; + assert(edgeParam.size() == 2); + // Weight from 0 to 1 + actionList[j][k][0] = edgeParam[0].asDouble(); + //std::cout << actionList[j][k][0] << " "; + // Phase offset from -pi to pi + actionList[j][k][1] = edgeParam[1].asDouble() * + (range) + lowerLimit; + //std::cout << actionList[j][k][1] << std::endl; + count++; + } + } + k++; + } + j++; + k = j; + + } + + + std::cout<< "Params used: " << count << std::endl; + + assert(edgeParam.begin() == edgeIt); + + return actionList; +} +std::vector JSONMGFeedbackControl::getFeedback(BaseQuadModelLearning& subject) +{ + // Placeholder + std::vector feedback; + + const std::vector& spineCables = subject.find ("all "); + + double *inputs = new double[m_config.numStates]; + + std::size_t n = spineCables.size(); + for(std::size_t i = 0; i != n; i++) + { + std::vector< std::vector > actions; + + const tgSpringCableActuator& cable = *(spineCables[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()); + } + + + return feedback; +} + +std::vector JSONMGFeedbackControl::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 JSONMGFeedbackControl::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/dhustigschultz/MG_Controller/JSONMGFeedbackControl.h b/src/dev/dhustigschultz/MG_Controller/JSONMGFeedbackControl.h new file mode 100644 index 000000000..507c5fdba --- /dev/null +++ b/src/dev/dhustigschultz/MG_Controller/JSONMGFeedbackControl.h @@ -0,0 +1,138 @@ +/* + * 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_MG_FEEDBACK_CONTROL_H +#define JSON_MG_FEEDBACK_CONTROL_H + +/** + * @file JSONGeneralFeedbackControl.h + * @brief A controller for the template class BaseSpineModelLearning + * @Includes more metrics, such as center of mass of entire structure. + * @author Brian Mirletz, Dawn Hustig-Schultz + * @version 1.1.0 + * $Id$ + */ + +#include "JSONMGCPGGeneralControl.h" + +#include + +// Forward Declarations +class neuralNetwork; +class tgSpringCableActuator; + +typedef boost::multi_array array_3D; //Will treat entire spine (and eventually body) as one big segment, so reducing the multi_array by one dimension. + +/** + * 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 JSONMGFeedbackControl : public JSONMGCPGGeneralControl +{ +public: + +struct Config : public JSONMGCPGGeneralControl::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 + ); + + const double freqFeedbackMin; + const double freqFeedbackMax; + const double ampFeedbackMin; + const double ampFeedbackMax; + const double phaseFeedbackMin; + const double phaseFeedbackMax; + + const double maxHeight; + const double minHeight; + + // Values to be filled in by JSON file during onSetup + int numStates; + int numActions; + + }; + + JSONMGFeedbackControl(JSONMGFeedbackControl::Config config, + std::string args, + std::string resourcePath = ""); + + virtual ~JSONMGFeedbackControl(); + + virtual void onSetup(BaseQuadModelLearning& subject); + + virtual void onStep(BaseQuadModelLearning& subject, double dt); + + virtual void onTeardown(BaseQuadModelLearning& subject); + +protected: +//ToDo: Need to restructure the for loops in here, so that have separate cases for the first and last segments (long muscles).... + virtual void setupCPGs(BaseQuadModelLearning& subject, array_2D nodeActions, array_3D edgeActions); + + virtual array_3D scaleEdgeActions (Json::Value edgeParam); + virtual array_2D scaleNodeActions (Json::Value actions); + +//ToDo: May have to write a new function in this subclass, to handle the fact that we'll be skipping segments now. Not sure if scale edge actions will work in all cases.... and maybe there's something better? + + std::vector getFeedback(BaseQuadModelLearning& subject); + + std::vector getCableState(const tgSpringCableActuator& cable); + + std::vector transformFeedbackActions(std::vector< std::vector >& actions); + + JSONMGFeedbackControl::Config m_config; + + std::vector m_spineControllers; + + /// @todo generalize this if we need more than one + neuralNetwork* nn; + +}; + +#endif // JSON_MG_FEEDBACK_CONTROL_H diff --git a/src/dev/dhustigschultz/MG_Controller/tgCPGMGActuatorControl.cpp b/src/dev/dhustigschultz/MG_Controller/tgCPGMGActuatorControl.cpp new file mode 100644 index 000000000..a1e7fde33 --- /dev/null +++ b/src/dev/dhustigschultz/MG_Controller/tgCPGMGActuatorControl.cpp @@ -0,0 +1,192 @@ +/* + * 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 tgCPGGMGActuatorControl.cpp + * @brief Implementation of the tgCPGGeneralActuatorControl observer class. + * So far, minor changes to setConnectivity, more to come. + * @author Brian Mirletz, Dawn Hustig-Schultz + * @date April 2016 + * $Id$ + */ + +#include "tgCPGMGActuatorControl.h" + +#include "core/tgSpringCable.h" +#include "core/tgSpringCableAnchor.h" +#include "core/tgSpringCableActuator.h" +#include "core/tgBasicActuator.h" +#include "core/tgBulletSpringCableAnchor.h" +#include "controllers/tgImpedanceController.h" +#include "util/CPGEquations.h" +#include "core/tgCast.h" + +// The C++ Standard Library +#include +#include +#include + +tgCPGMGActuatorControl::tgCPGMGActuatorControl(const double controlStep) : +m_controlTime(0.0), +m_totalTime(0.0), +m_controlStep(controlStep), +m_commandedTension(0.0), +m_pFromBody(NULL), +m_pToBody(NULL) +{ + if (m_controlStep < 0.0) + { + throw std::invalid_argument("Negative control step"); + } +} + +tgCPGMGActuatorControl::~tgCPGMGActuatorControl() +{ + // We don't own these + m_pFromBody = NULL; + m_pToBody = NULL; +} + +void tgCPGMGActuatorControl::onAttach(tgSpringCableActuator& subject) +{ + m_controlLength = subject.getStartLength(); + + // tgSpringCable doesn't know about bullet anchors, so we have to cast here to get the rigid bodies + std::vector anchors = + tgCast::filter(subject.getSpringCable()->getAnchors()); + + std::size_t n = anchors.size(); + assert(n >= 2); + + m_pFromBody = anchors[0]->attachedBody; + m_pToBody = anchors[n - 1]->attachedBody; +} + +void tgCPGMGActuatorControl::onStep(tgSpringCableActuator& subject, double dt) +{ + m_controlTime += dt; + m_totalTime += dt; + /// @todo this fails if its attached to multiple controllers! + /// is there a way to track _global_ time at this level + + // Workaround until we implement PID + tgBasicActuator& m_sca = *(tgCast::cast(subject)); + + if (m_controlTime >= m_controlStep) + { + + m_commandedTension = motorControl().control(m_sca, m_controlTime, controlLength(), getCPGValue()); + + m_controlTime = 0; + } + else + { + m_sca.moveMotors(dt); + } +} + +void tgCPGMGActuatorControl::assignNodeNumber (CPGEquations& CPGSys, array_2D nodeParams) +{ + // Ensure that this hasn't already been assigned + assert(m_nodeNumber == -1); + + m_pCPGSystem = &CPGSys; + + std::vector params (7); + params[0] = nodeParams[0][0]; // Frequency Offset + params[1] = nodeParams[0][0]; // Frequency Scale + params[2] = nodeParams[0][1]; // Radius Offset + params[3] = nodeParams[0][1]; // Radius Scale + params[4] = 20.0; // rConst (a constant) + params[5] = 0.0; // dMin for descending commands + params[6] = 5.0; // dMax for descending commands + + m_nodeNumber = m_pCPGSystem->addNode(params); +} + +void +tgCPGMGActuatorControl::setConnectivity(const std::vector& allStrings, + array_3D edgeParams) +{ + assert(m_nodeNumber >= 0); + + //int muscleSize = edgeParams.shape()[1]; + + std::vector connectivityList; + std::vector weights; + std::vector phases; + + // Assuming all coupling is two way, there ought to be a way + // to search faster than O((2N)^2) since every other + // string has to call this. Ideas are welcome + for (int i = 0; i < allStrings.size(); i++) + { + if (this != allStrings[i]) + { + // Make sure we're dealing with the same system + assert(m_pCPGSystem == allStrings[i]->getCPGSys()); + + const btRigidBody* theirFromGroup = allStrings[i]->getFromBody(); + const btRigidBody* theirToGroup = allStrings[i]->getToBody(); + + // "All to all" connectivity for shared rigid bodies + if(m_pFromBody == theirFromGroup || + m_pToBody == theirToGroup || + m_pFromBody == theirToGroup || + m_pToBody == theirFromGroup) + { + int theirMuscle = allStrings[i]->getNodeNumber(); + // Integer division: -1 is behind, 0 is same row 1 is ahead + //int rp = ((m_nodeNumber - theirMuscle) / muscleSize) + 1; + int j = m_nodeNumber; + int k = theirMuscle; + connectivityList.push_back(theirMuscle); + // Upper triangular matrix, lower number always goes first + if(j > k){ + weights.push_back(edgeParams[k][j][0]); + phases.push_back(edgeParams[k][j][1]); + } + else + { + weights.push_back(edgeParams[j][k][0]); + phases.push_back(edgeParams[j][k][1]); + } + } + } + } + + m_pCPGSystem->defineConnections(m_nodeNumber, connectivityList, weights, phases); +} + +void tgCPGMGActuatorControl::setupControl(tgImpedanceController& ipc) +{ + tgBaseCPGNode::setupControl(ipc); +} + +void tgCPGMGActuatorControl::setupControl(tgImpedanceController& ipc, + double controlLength) +{ + if (controlLength < 0.0) + { + throw std::invalid_argument("Negative control length"); + } + + m_controlLength = controlLength; + tgBaseCPGNode::setupControl(ipc); +} + diff --git a/src/dev/dhustigschultz/MG_Controller/tgCPGMGActuatorControl.h b/src/dev/dhustigschultz/MG_Controller/tgCPGMGActuatorControl.h new file mode 100644 index 000000000..1e3c746d2 --- /dev/null +++ b/src/dev/dhustigschultz/MG_Controller/tgCPGMGActuatorControl.h @@ -0,0 +1,128 @@ +/* + * 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 TG_CPG_MG_ACTUATOR_CONTRL_H +#define TG_CPG_MG_ACTUATOR_CONTRL_H + +/** + * @file tgCPGMGActuatorControl.h + * @brief Definition of the tgCPGStringControl observer class + * @author Brian Mirletz, Dawn Hustig-Schultz + * @date April 2016 + * $Id$ + */ + +#include "examples/learningSpines/tgCPGActuatorControl.h" +#include "util/tgBaseCPGNode.h" +#include "core/tgSpringCableActuator.h" +// The Boost library +#include "boost/multi_array.hpp" + +typedef boost::multi_array array_2D; +typedef boost::multi_array array_3D; + +// Forward declarations +class btRigidBody; +class CPGEquations; +class CPGEquationsFB; +class tgImpedanceController; + +class tgCPGMGActuatorControl : public tgObserver, + public tgBaseCPGNode +{ +public: + + tgCPGMGActuatorControl(const double controlStep = 1.0/10000.0); + + virtual ~tgCPGMGActuatorControl(); + + virtual void onAttach(tgSpringCableActuator& subject); + + virtual void onStep(tgSpringCableActuator& subject, double dt); + + /** + * Can call these any time, but they'll only have the intended effect + * after all of the strings have been constructed. + */ + + void assignNodeNumber (CPGEquations& CPGSys, array_2D nodeParams); + + /** + * Iterate through all other tgSpringCableActuatorCPGInfos, and determine + * CPG network by rigid body connectivity + */ + void setConnectivity(const std::vector& allStrings, + array_3D edgeParams); + + const int getNodeNumber() const + { + return m_nodeNumber; + } + + /** + * Pointer to the CPG system. Owned by the higher level controller + */ + const CPGEquations* getCPGSys() const + { + return m_pCPGSystem; + } + + const double getCommandedTension() const + { + return m_commandedTension; + } + + virtual void setupControl(tgImpedanceController& ipc); + + void setupControl(tgImpedanceController& ipc, + double controlLength); + + const btRigidBody* getFromBody() const + { + return m_pFromBody; + } + + const btRigidBody* getToBody() const + { + return m_pToBody; + } + +protected: + /** + * Member variable for keeping track of how long its been since the + * last update step + */ + double m_controlTime; + /** + * How often this controller updates. Must be non-negative. Zero + * means it updates every timestep. Units are seconds + */ + const double m_controlStep; + + double m_totalTime; + + double m_commandedTension; + + btRigidBody* m_pFromBody; + + btRigidBody* m_pToBody; +}; + + +#endif + diff --git a/src/dev/dhustigschultz/MG_Controller/tgCPGMGCableControl.cpp b/src/dev/dhustigschultz/MG_Controller/tgCPGMGCableControl.cpp new file mode 100644 index 000000000..5c7dd00a8 --- /dev/null +++ b/src/dev/dhustigschultz/MG_Controller/tgCPGMGCableControl.cpp @@ -0,0 +1,121 @@ +/* + * 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. +*/ + +#include "tgCPGMGCableControl.h" + +#include "core/tgSpringCableActuator.h" +#include "core/tgBulletSpringCableAnchor.h" +#include "core/tgBasicActuator.h" +#include "controllers/tgImpedanceController.h" +#include "controllers/tgPIDController.h" +#include "core/tgCast.h" +#include "util/CPGEquations.h" +#include "dev/CPG_feedback/CPGEquationsFB.h" + +// The C++ Standard Library +#include +#include +#include + +tgCPGMGCableControl::tgCPGMGCableControl(tgPIDController::Config pid_config, const double controlStep) : +m_config(pid_config), +tgCPGMGActuatorControl(controlStep), +m_PID(NULL), +usePID(true) +{ + if (m_controlStep < 0.0) + { + throw std::invalid_argument("Negative control step"); + } +} + +tgCPGMGCableControl::~tgCPGMGCableControl() +{ + delete m_PID; +} + +void tgCPGMGCableControl::onSetup(tgSpringCableActuator& subject) +{ + m_PID = new tgPIDController(&subject, m_config); + + tgBasicActuator* basicAct = tgCast::cast(&subject); + + if (basicAct != NULL) + { + usePID = false; + } +} + +void tgCPGMGCableControl::onStep(tgSpringCableActuator& subject, double dt) +{ + assert(&subject == m_PID->getControllable()); + + m_controlTime += dt; + m_totalTime += dt; + + if (m_controlTime >= m_controlStep) + { + if (usePID) + { + m_commandedTension = motorControl().control(*m_PID, dt, controlLength(), getCPGValue()); + } + else + { + tgBasicActuator* basicAct = tgCast::cast(&subject); + m_commandedTension = motorControl().control(*basicAct, dt, controlLength(), getCPGValue()); + } + m_controlTime = 0; + } + else + { + const double currentTension = subject.getTension(); + if(usePID) + { + m_PID->control(dt, m_commandedTension, currentTension); + } + else + { + tgBasicActuator* basicAct = tgCast::cast(&subject); + basicAct->moveMotors(dt); + } + } +} + +void tgCPGMGCableControl::assignNodeNumberFB (CPGEquationsFB& CPGSys, array_2D nodeParams) +{ + // Ensure that this hasn't already been assigned + assert(m_nodeNumber == -1); + + m_pCPGSystem = &CPGSys; + + std::vector params (11); + params[0] = nodeParams[0][0]; // Frequency Offset + params[1] = nodeParams[0][0]; // Frequency Scale + params[2] = nodeParams[0][1]; // Radius Offset + params[3] = nodeParams[0][1]; // Radius Scale + params[4] = 1.0; // rConst (a constant) + params[5] = 0.0; // dMin for descending commands + params[6] = 5.0; // dMax for descending commands + params[6] = 5.0; // dMax for descending commands + params[7] = nodeParams[0][0]; // Omega (initialize variable) + params[8] = nodeParams[0][2]; // Frequency feedback + params[9] = nodeParams[0][3]; // Amplitude feedback + params[10] = nodeParams[0][4]; // Phase feedback + + m_nodeNumber = CPGSys.addNode(params); +} diff --git a/src/dev/dhustigschultz/MG_Controller/tgCPGMGCableControl.h b/src/dev/dhustigschultz/MG_Controller/tgCPGMGCableControl.h new file mode 100644 index 000000000..d87b5fa97 --- /dev/null +++ b/src/dev/dhustigschultz/MG_Controller/tgCPGMGCableControl.h @@ -0,0 +1,59 @@ +/* + * 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 TG_CPG_MG_CABLE_CONTROL_H +#define TG_CPG_MG_CABLE_CONTROL_H + +#include "tgCPGMGActuatorControl.h" +#include "core/tgSpringCableActuator.h" +#include "controllers/tgPIDController.h" +// The Boost library +#include "boost/multi_array.hpp" + +//Have to modify, because otherwise there are type conversion errors in the tgCPGGeneralActuatorControl class (subclass of tgCPGActuatorControl). +//Made a new copy so won't totally break Brian's code (and some of my own previous code). + +class tgCPGMGCableControl : public tgCPGMGActuatorControl +{ +public: + + tgCPGMGCableControl(const tgPIDController::Config pid_config, const double controlStep = 1.0/10000.0); + + virtual ~tgCPGMGCableControl(); + + virtual void onSetup(tgSpringCableActuator& subject); + + virtual void onStep(tgSpringCableActuator& subject, double dt); + + /** + * Account for the larger number of parameters the nodes have + * with a feedback CPGSystem + */ + void assignNodeNumberFB (CPGEquationsFB& CPGSys, array_2D nodeParams); + +protected: + const tgPIDController::Config m_config; + + tgPIDController* m_PID; + + bool usePID; + +}; + + +#endif diff --git a/src/dev/dhustigschultz/MountainGoat/AppMountainGoat.cpp b/src/dev/dhustigschultz/MountainGoat/AppMountainGoat.cpp new file mode 100644 index 000000000..08dbb4928 --- /dev/null +++ b/src/dev/dhustigschultz/MountainGoat/AppMountainGoat.cpp @@ -0,0 +1,85 @@ +/* + * 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 AppMountainGoat.cpp + * @brief Implementing the Flemons quadruped model. + * @author Dawn Hustig-Schultz + * @date Mar. 2016 + * @version 1.0.0 + * $Id$ + */ + +// This application +#include "MountainGoat.h" +// This library +#include "core/terrain/tgBoxGround.h" +#include "core/tgModel.h" +#include "core/tgSimViewGraphics.h" +#include "core/tgSimulation.h" +#include "core/tgWorld.h" +// Bullet Physics +#include "LinearMath/btVector3.h" +// The C++ Standard Library +#include + +/** + * 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 << "AppMountainGoat" << std::endl; + + // First create the ground and world. Specify ground rotation in radians + const double yaw = 0.0; + const double pitch = 0.0; + const double roll = 0.0; + const tgBoxGround::Config groundConfig(btVector3(yaw, pitch, roll)); + // the world will delete this + tgBoxGround* ground = new tgBoxGround(groundConfig); + + const tgWorld::Config config(981); // gravity, cm/sec^2 981 + tgWorld world(config, ground); + + // Second create the view + const double timestep_physics = 0.001; // seconds + const double timestep_graphics = 1.f/60.f; // seconds + tgSimViewGraphics view(world, timestep_physics, timestep_graphics); + + // Third create the simulation + tgSimulation simulation(view); + + //Parameters for the structure: + const int segments = 7; + const int hips = 4; + const int legs = 4; + + MountainGoat* const myModel = new MountainGoat(segments, hips, legs); + + // Add the model to the world + simulation.addModel(myModel); + + simulation.run(); + + //Teardown is handled by delete, so that should be automatic + return 0; +} diff --git a/src/dev/dhustigschultz/MountainGoat/CMakeLists.txt b/src/dev/dhustigschultz/MountainGoat/CMakeLists.txt new file mode 100644 index 000000000..14c3d2ce1 --- /dev/null +++ b/src/dev/dhustigschultz/MountainGoat/CMakeLists.txt @@ -0,0 +1,10 @@ +link_libraries(tgcreator + core + BaseQuadModelLearning) + +add_library(MountainGoat + MountainGoat.cpp) + +add_executable(AppMountainGoat + MountainGoat.cpp + AppMountainGoat.cpp) diff --git a/src/dev/dhustigschultz/MountainGoat/MountainGoat.cpp b/src/dev/dhustigschultz/MountainGoat/MountainGoat.cpp new file mode 100644 index 000000000..2145e3786 --- /dev/null +++ b/src/dev/dhustigschultz/MountainGoat/MountainGoat.cpp @@ -0,0 +1,646 @@ +/* + * 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 MountainGoat.cpp + * @brief Implementing a quadruped based off the Flemons BigPuppy model. + * Trying to stiffen up leg actuators more, to prevent falling. + * @author Dawn Hustig-Schultz + * @date Mar. 2016 + * @version 1.1.0 + * $Id$ + */ + + +//This application +#include "MountainGoat.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 + +MountainGoat::MountainGoat(int segments, int hips, int legs) : +BaseQuadModelLearning(segments, hips), +m_legs(legs) +{ + m_subStructures = segments + hips + legs; +} + +MountainGoat::~MountainGoat() +{ +} + +void MountainGoat::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. +} + +void MountainGoat::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"); +} + +void MountainGoat::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 MountainGoat::addRodsHip(tgStructure& s){ + s.addPair(0,1,"rod"); + s.addPair(0,2,"rod"); + s.addPair(0,3,"rod"); +} + +void MountainGoat::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 MountainGoat::addRodsVertebra(tgStructure& s){ + s.addPair(0,1,"rod"); + s.addPair(0,2,"rod"); + s.addPair(0,3,"rod"); + s.addPair(0,4,"rod"); +} + +void MountainGoat::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 left 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 right 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++) {//left 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++) {//right 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 + } + +} + +void MountainGoat::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 all main front upper right muscleAct1 seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[3], n1[4], tgString("spine all main front upper left muscleAct1 seg", i-2) + tgString(" seg", i-1)); + + puppy.addPair(n0[4], n1[3], tgString("spine all main front lower right muscleAct2 seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[4], n1[4], tgString("spine all main front lower left muscleAct2 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 all main front lower right muscleAct2 seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[1], n1[4], tgString("spine all main front lower left muscleAct2 seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[2], n1[3], tgString("spine all main front upper right muscleAct1 seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[2], n1[4], tgString("spine all main front upper left muscleAct1 seg", i-2) + tgString(" seg", i-1)); + } + else{ //rear + puppy.addPair(n0[1], n1[3], tgString("spine all main rear upper left muscleAct1 seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[1], n1[4], tgString("spine all main rear lower left muscleAct2 seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[2], n1[3], tgString("spine all main rear upper right muscleAct1 seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[2], n1[4], tgString("spine all main rear lower right muscleAct2 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 muscleAct2 seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[2], n2[3], tgString("spine2 top muscleAct1 seg", i-2) + tgString(" seg", i-1)); + } + else{ + puppy.addPair(n0[1], n2[4], tgString("spine2 lateral left muscleAct1 seg", i-2) + tgString(" seg", i-1)); + puppy.addPair(n0[2], n2[3], tgString("spine2 lateral right muscleAct1 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 all main rear upper left muscleAct1 seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[1], n2[4], tgString("spine all main rear lower left muscleAct2 seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[2], n2[3], tgString("spine all main rear upper right muscleAct1 seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[2], n2[4], tgString("spine all main rear lower right muscleAct2 seg", i-1) + tgString(" seg", i)); + } + else{//front + + puppy.addPair(n1[1], n2[3], tgString("spine all main front lower right muscleAct2 seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[1], n2[4], tgString("spine all main front lower left muscleAct2 seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[2], n2[3], tgString("spine all main front upper right muscleAct1 seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[2], n2[4], tgString("spine all main front upper left muscleAct1 seg", i-1) + tgString(" seg", i)); + } + } + if (i >= 2 && i < 7){ + puppy.addPair(n1[3], n2[3], tgString("spine all spiral muscleAct1 seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[4], n2[3], tgString("spine all spiral muscleAct1 seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[3], n2[4], tgString("spine all spiral muscleAct1 seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[4], n2[4], tgString("spine all spiral muscleAct1 seg", i-1) + tgString(" seg", i)); + } + if(i == 6){ + //rear + puppy.addPair(n1[1], n2[2], tgString("spine all rear lower left muscleAct2 seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[2], n2[2], tgString("spine all rear lower right muscleAct2 seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[1], n2[1], tgString("spine all rear upper left muscleAct1 seg", i-1) + tgString(" seg", i)); + puppy.addPair(n1[2], n2[1], tgString("spine all rear upper right muscleAct1 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(); + + //Adding long muscles to spine, for bending/arching: + //puppy.addPair(n0[2], n6[3], tgString("spine secondary top arching muscleAct seg", 0) + tgString(" seg", 6)); //Change these to something other than "spine " or "spine2" when it's time to implement new code for them! + //puppy.addPair(n0[1], n6[4], tgString("spine bottom arching muscleAct seg", 0) + tgString(" seg", 6)); + //puppy.addPair(n1[4], n5[1], tgString("spine right lateral arching muscleAct seg", 1) + tgString(" seg", 5)); + //puppy.addPair(n1[3], n5[2], tgString("spine left lateral arching muscleAct seg", 1) + tgString(" seg", 5)); + + //Left shoulder muscles + puppy.addPair(n7[1], n1[1], tgString("all left shoulder rear upper muscleAct1 seg", 7) + tgString(" seg", 1)); + puppy.addPair(n7[1], n1[4], tgString("all left shoulder front upper muscleAct1 seg", 7) + tgString(" seg", 1)); + puppy.addPair(n7[1], n0[2], tgString("all left shoulder front top muscleAct1 seg", 7) + tgString(" seg", 0)); + puppy.addPair(n7[1], n2[3], tgString("all left shoulder rear top muscleAct1 seg", 7) + tgString(" seg", 2)); + + puppy.addPair(n7[3], n1[1], tgString("all left shoulder rear lower muscleAct1 seg", 7) + tgString(" seg", 1)); + puppy.addPair(n7[3], n1[4], tgString("all left shoulder front lower muscleAct1 seg", 7) + tgString(" seg", 1)); + puppy.addPair(n7[3], n0[1], tgString("all left shoulder front bottom muscleAct1 seg", 7) + tgString(" seg", 0)); + puppy.addPair(n7[3], n2[4], tgString("all left shoulder rear bottom muscleAct1 seg", 7) + tgString(" seg", 2)); + + //Extra muscles, to move left shoulder forward and back: + puppy.addPair(n7[0], n1[1], tgString("all left shoulder rear mid muscleAct1 seg", 7) + tgString(" seg", 1)); + puppy.addPair(n7[0], n1[4], tgString("all left shoulder front mid muscleAct1 seg", 7) + tgString(" seg", 1)); + + //Left hip muscles + puppy.addPair(n8[1], n5[1], tgString("all left hip rear upper muscleAct1 seg", 8) + tgString(" seg", 5)); + puppy.addPair(n8[1], n5[4], tgString("all left hip front upper muscleAct1 seg", 8) + tgString(" seg", 5)); + puppy.addPair(n8[1], n4[2], tgString("all left hip front top muscleAct1 seg", 8) + tgString(" seg", 4)); + puppy.addPair(n8[1], n6[3], tgString("all left hip rear top muscleAct1 seg", 8) + tgString(" seg", 6)); + + puppy.addPair(n8[3], n5[1], tgString("all left hip rear lower muscleAct1 seg", 8) + tgString(" seg", 5)); + puppy.addPair(n8[3], n5[4], tgString("all left hip front lower muscleAct1 seg", 8) + tgString(" seg", 5)); + puppy.addPair(n8[3], n4[1], tgString("all left hip front bottom muscleAct1 seg", 8) + tgString(" seg", 4)); + puppy.addPair(n8[3], n6[4], tgString("all left hip front bottom muscleAct1 seg", 8) + tgString(" seg", 6)); + + //Extra muscles, to move left hip forward and back: + puppy.addPair(n8[0], n5[1], tgString("all left hip rear mid muscleAct1 seg", 8) + tgString(" seg", 5)); + puppy.addPair(n8[0], n5[4], tgString("all left hip front mid muscleAct1 seg", 8) + tgString(" seg", 5)); + + //Right shoulder muscles + puppy.addPair(n9[1], n1[2], tgString("all right shoulder rear upper muscleAct1 seg", 9) + tgString(" seg", 1)); + puppy.addPair(n9[1], n1[3], tgString("all right shoulder front upper muscleAct1 seg", 9) + tgString(" seg", 1)); + puppy.addPair(n9[1], n0[2], tgString("all right shoulder front top muscleAct1 seg", 9) + tgString(" seg", 0)); + puppy.addPair(n9[1], n2[3], tgString("all right shoulder rear top muscleAct1 seg", 9) + tgString(" seg", 2)); + + puppy.addPair(n9[3], n1[2], tgString("all right shoulder rear lower muscleAct1 seg", 9) + tgString(" seg", 1)); + puppy.addPair(n9[3], n1[3], tgString("all right shoulder front lower muscleAct1 seg", 9) + tgString(" seg", 1)); + puppy.addPair(n9[3], n0[1], tgString("all right shoulder front bottom muscleAct1 seg", 9) + tgString(" seg", 0)); + puppy.addPair(n9[3], n2[4], tgString("all right shoulder rear bottom muscleAct1 seg", 9) + tgString(" seg", 2)); + + //Extra muscles, to move right shoulder forward and back: + puppy.addPair(n9[0], n1[2], tgString("all right shoulder rear mid muscleAct1 seg", 9) + tgString(" seg", 1)); + puppy.addPair(n9[0], n1[3], tgString("all right shoulder front mid muscleAct1 seg", 9) + tgString(" seg", 1)); + + //Right hip muscles + puppy.addPair(n10[1], n5[2], tgString("all right hip rear upper muscleAct1 seg", 10) + tgString(" seg", 5)); + puppy.addPair(n10[1], n5[3], tgString("all right hip front upper muscleAct1 seg", 10) + tgString(" seg", 5)); + puppy.addPair(n10[1], n4[2], tgString("all right hip front top muscleAct1 seg", 10) + tgString(" seg", 4)); + puppy.addPair(n10[1], n6[3], tgString("all right hip rear top muscleAct1 seg", 10) + tgString(" seg", 4)); + + puppy.addPair(n10[3], n5[2], tgString("all right hip rear lower muscleAct1 seg", 10) + tgString(" seg", 5)); + puppy.addPair(n10[3], n5[3], tgString("all right hip front lower muscleAct1 seg", 10) + tgString(" seg", 5)); + puppy.addPair(n10[3], n4[1], tgString("all right hip bottom muscleAct1 seg", 10) + tgString(" seg", 4)); + puppy.addPair(n10[3], n6[4], tgString("all right hip bottom muscleAct1 seg", 10) + tgString(" seg", 6)); + + //Extra muscles, to move right hip forward and back: + puppy.addPair(n10[0], n5[2], tgString("all right hip rear mid muscleAct1 seg", 10) + tgString(" seg", 5)); + puppy.addPair(n10[0], n5[3], tgString("all right hip front mid muscleAct1 seg", 10) + tgString(" seg", 5)); + + //Leg/hip connections: + + //Left front leg/shoulder + puppy.addPair(n11[4], n7[3], tgString("left foreleg outer bicep muscle seg", 11) + tgString(" seg", 7)); + puppy.addPair(n11[4], n7[2], tgString("left foreleg inner bicep muscle seg", 11) + tgString(" seg", 7)); + puppy.addPair(n11[4], n1[4], tgString("left foreleg front abdomen connection muscle seg", 11) + tgString(" seg", 1)); + puppy.addPair(n11[3], n1[1],tgString("left foreleg front abdomen connection muscle3 seg", 11) + tgString(" seg", 1)); + puppy.addPair(n11[2], n1[4],tgString("left foreleg front abdomen connection muscle3 seg", 11) + tgString(" seg", 1)); + + puppy.addPair(n11[3], n7[3], tgString("left foreleg outer tricep muscle seg", 11) + tgString(" seg", 7)); + puppy.addPair(n11[3], n7[2], tgString("left foreleg inner tricep muscle seg", 11) + tgString(" seg", 7)); + + puppy.addPair(n11[2], n7[3], tgString("left foreleg outer front tricep muscle seg", 11) + tgString(" seg", 7)); + puppy.addPair(n11[2], n7[2], tgString("left foreleg inner front tricep muscle seg", 11) + tgString(" seg", 7)); + + //Adding muscle to pull up on right front leg: + puppy.addPair(n11[4], n7[1], tgString("left foreleg mid bicep muscle3 seg", 11) + tgString(" seg", 7)); + + //Right front leg/shoulder + puppy.addPair(n13[4], n9[2], tgString("right foreleg inner bicep muscle seg", 13) + tgString(" seg", 9)); + puppy.addPair(n13[4], n9[3], tgString("right foreleg outer bicep muscle seg", 13) + tgString(" seg", 9)); + puppy.addPair(n13[4], n1[3], tgString("right foreleg front abdomen connection muscle seg", 13) + tgString(" seg", 1)); + puppy.addPair(n13[3], n1[2], tgString("right foreleg front abdomen connection muscle3 seg", 13) + tgString(" seg", 1)); + puppy.addPair(n13[2], n1[3], tgString("right foreleg front abdomen connection muscle3 seg", 13) + tgString(" seg", 1)); + + + puppy.addPair(n13[3], n9[2], tgString("right foreleg inner tricep muscle seg", 13) + tgString(" seg", 9)); + puppy.addPair(n13[3], n9[3], tgString("right foreleg outer tricep muscle seg", 13) + tgString(" seg", 9)); + + puppy.addPair(n13[2], n9[2], tgString("right foreleg inner front tricep muscle seg", 13) + tgString(" seg", 9)); + puppy.addPair(n13[2], n9[3], tgString("right foreleg outer front tricep muscle seg", 13) + tgString(" seg", 9)); + + //Adding muscle to pull up on left front leg: + puppy.addPair(n13[4], n9[1], tgString("right foreleg mid bicep muscle3 seg", 13) + tgString(" seg", 9)); + + //Left rear leg/hip + puppy.addPair(n12[4], n8[3], tgString("left hindleg outer thigh muscle seg", 12) + tgString(" seg", 8)); + puppy.addPair(n12[4], n8[2], tgString("left hindleg inner thigh muscle seg", 12) + tgString(" seg", 8)); + + puppy.addPair(n12[4], n3[1],tgString("left hindleg rear abdomen connection muscle seg", 12) + tgString(" seg", 3)); + puppy.addPair(n12[3], n5[1],tgString("left hindleg rear abdomen connection muscle3 seg", 12) + tgString(" seg", 5)); + puppy.addPair(n12[2], n5[4],tgString("left hindleg rear abdomen connection muscle3 seg", 12) + tgString(" seg", 5)); + + puppy.addPair(n12[3], n8[3], tgString("left hindleg outer calf muscle seg", 12) + tgString(" seg", 8)); + puppy.addPair(n12[3], n8[2], tgString("left hindleg inner calf muscle seg", 12) + tgString(" seg", 8)); + + puppy.addPair(n12[2], n8[3], tgString("left hindleg outer front calf muscle seg", 12) + tgString(" seg", 8)); + puppy.addPair(n12[2], n8[2], tgString("left hindleg inner front calf muscle seg", 12) + tgString(" seg", 8)); + + //Adding muscle to pull rear right leg up: + puppy.addPair(n12[4], n8[1], tgString("left hindleg central thigh muscle3 seg", 12) + tgString(" seg", 8)); + + //Right rear leg/hip + puppy.addPair(n14[4], n10[2], tgString("right hindleg inner thigh muscle seg", 14) + tgString(" seg", 10)); + puppy.addPair(n14[4], n10[3], tgString("right hindleg outer thigh muscle seg", 14) + tgString(" seg", 10)); + + puppy.addPair(n14[4], n3[2], tgString("right hindleg rear abdomen connection muscle seg", 14) + tgString(" seg", 3)); + puppy.addPair(n14[3], n5[2], tgString("right hindleg rear abdomen connection muscle3 seg", 14) + tgString(" seg", 5)); + puppy.addPair(n14[2], n5[3], tgString("right hindleg rear abdomen connection muscle3 seg", 14) + tgString(" seg", 5)); + + + puppy.addPair(n14[3], n10[2], tgString("right hindleg inner calf muscle seg", 14) + tgString(" seg", 10)); + puppy.addPair(n14[3], n10[3], tgString("right hindleg outer calf muscle seg", 14) + tgString(" seg", 10)); + + puppy.addPair(n14[2], n10[2], tgString("right hindleg inner front calf muscle seg", 14) + tgString(" seg", 10)); + puppy.addPair(n14[2], n10[3], tgString("right hindleg outer front calf muscle seg", 14) + tgString(" seg", 10)); + + //Adding muscle to pull rear left leg up: + puppy.addPair(n14[4], n10[1], tgString("right hindleg central thigh muscle3 seg", 14) + tgString(" seg", 10)); + +} + +void MountainGoat::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; //4000 + const double stiffnessPassive2 = 4000.0; + const double stiffnessPassive3 = 10000.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; + const double passivePretension4 = 4000.0; + +#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(stiffness, 20, passivePretension, + mRad, motorFriction, motorInertia, backDrivable, + history, maxTens, maxSpeed); + tgKinematicActuator::Config motorConfigOther(stiffnessPassive, damping, passivePretension2, + mRad, motorFriction, motorInertia, backDrivable, + history, maxTens, maxSpeed); + + tgKinematicActuator::Config motorConfigStomach(stiffnessPassive2, damping, passivePretension4, + mRad, motorFriction, motorInertia, backDrivable, + history, maxTens, maxSpeed); + tgKinematicActuator::Config motorConfigLegs(stiffnessPassive3, damping, 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 motorConfigStomach(stiffnessPassive2, damping, passivePretension4, + mRad, motorFriction, motorInertia, backDrivable, + history, maxTens, maxSpeed); + tgKinematicActuator::Config motorConfigLegs(stiffnessPassive3, damping, passivePretension3, + mRad, motorFriction, motorInertia, backDrivable, + history, maxTens, maxSpeed); + #endif + +#else + + #ifdef PASSIVE_STRUCTURE + tgSpringCableActuator::Config muscleConfig(2000, 20, passivePretension); + tgSpringCableActuator::Config muscleConfigOther(stiffnessPassive, damping, passivePretension2); + tgSpringCableActuator::Config muscleConfigStomach(stiffnessPassive2, damping, passivePretension4); + tgSpringCableActuator::Config muscleConfigLegs(stiffnessPassive, damping, passivePretension3); + + #else + tgSpringCableActuator::Config muscleConfigSpine(stiffness, damping, pretension, history, maxTens, 2*maxSpeed); + tgSpringCableActuator::Config muscleConfigOther(stiffnessPassive, damping, passivePretension2); + tgSpringCableActuator::Config muscleConfigStomach(stiffnessPassive2, damping, passivePretension4); + tgSpringCableActuator::Config muscleConfigLegs(stiffnessPassive, damping, passivePretension3); + #endif + +#endif + + //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); //,m_segments,m_hips,m_legs,m_feet + + 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("muscleAct1", new tgKinematicContactCableInfo(motorConfig)); + spec.addBuilder("muscle ", new tgKinematicContactCableInfo(motorConfigOther)); + spec.addBuilder("muscleAct2 ", new tgKinematicContactCableInfo(motorConfigStomach)); + spec.addBuilder("muscle3 ", new tgKinematicContactCableInfo(motorConfigLegs)); + #else + spec.addBuilder("muscleAct1", new tgKinematicContactCableInfo(motorConfigSpine)); + spec.addBuilder("muscle ", new tgKinematicContactCableInfo(motorConfigOther)); + spec.addBuilder("muscleAct2 ", new tgKinematicContactCableInfo(motorConfigStomach)); + spec.addBuilder("muscle3 ", new tgKinematicContactCableInfo(motorConfigLegs)); + + #endif + +#else + #ifdef PASSIVE_STRUCTURE + spec.addBuilder("muscleAct1", new tgBasicActuatorInfo(muscleConfig)); + spec.addBuilder("muscle " , new tgBasicActuatorInfo(muscleConfigOther)); + spec.addBuilder("muscleAct2 " , new tgBasicActuatorInfo(muscleConfigStomach)); + spec.addBuilder("muscle3 " , new tgBasicActuatorInfo(muscleConfigLegs)); + #else + spec.addBuilder("muscleAct1" , new tgBasicActuatorInfo(muscleConfigSpine)); + spec.addBuilder("muscle " , new tgBasicActuatorInfo(muscleConfigOther)); + spec.addBuilder("muscleAct2 " , new tgBasicActuatorInfo(muscleConfigStomach)); + 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 ("segment"); + + // Actually setup the children, notify controller that the setup has finished + BaseQuadModelLearning::setup(world); + + children.clear(); +} + +void MountainGoat::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 + BaseQuadModelLearning::step(dt); + } +} + +void MountainGoat::teardown() +{ + BaseQuadModelLearning::teardown(); +} diff --git a/src/dev/dhustigschultz/MountainGoat/MountainGoat.h b/src/dev/dhustigschultz/MountainGoat/MountainGoat.h new file mode 100644 index 000000000..cc91a2346 --- /dev/null +++ b/src/dev/dhustigschultz/MountainGoat/MountainGoat.h @@ -0,0 +1,156 @@ +/* + * 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 MOUNTAIN_GOAT_H +#define MOUNTAIN_GOAT_H + +/** + * @file MountainGoat.h + * @brief Implementing the Flemons quadruped model (roughly), but as a subclass of Brian's BaseSpineModelLearning module. + * Trying to stiffen up the legs more, to prevent falling when actuated. + * @author Dawn Hustig-Schultz + * @date Mar. 2016 + * @version 1.0.0 + * $Id$ + */ + +#include "dev/dhustigschultz/BigPuppy_SpineOnly_Stats/BaseQuadModelLearning.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; +class tgStructureInfo; +class tgBasicActuator; + +class MountainGoat: public BaseQuadModelLearning +{ +public: + + MountainGoat(int segments, int hips, int legs); + + virtual ~MountainGoat(); + + /** + * 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_legs; + +private: + + /** + * 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); + +}; + +#endif