std::vector<double> SpineFeedbackControl::getFeedback(BaseSpineModelLearning& subject) { // Placeholder std:vector<double> feedback; // Adapter doesn't use this anyway, so just do zero here for now (will trigger errors if it starts to use it =) ) const double dt = 0; const std::vector<tgSpringCableActuator*>& allCables = subject.getAllMuscles(); std::size_t n = allCables.size(); for(std::size_t i = 0; i != n; i++) { const tgSpringCableActuator& cable = *(allCables[i]); std::vector<double > state = getCableState(cable); std::vector< std::vector<double> > actions = feedbackAdapter.step(m_updateTime, state); std::vector<double> cableFeedback = transformFeedbackActions(actions); feedback.insert(feedback.end(), cableFeedback.begin(), cableFeedback.end()); #if (0) for (std::size_t j = 0; j < cableFeedback.size(); j++) { std::cout << cableFeedback[j] << " "; } } std::cout << std::endl; #else }
void LearningSpineSine::onTeardown(BaseSpineModelLearning& subject) { std::vector<double> scores; // @todo - check to make sure we ran for the right amount of time std::vector<double> 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)); scores.push_back(distanceMoved); /// @todo - consolidate with other controller classes. /// @todo - return length scale as a parameter double totalEnergySpent=0; vector<tgSpringCableActuator* > tmpSCAs = subject.getAllMuscles(); vector<tgBasicActuator* > tmpStrings = tgCast::filter<tgSpringCableActuator, tgBasicActuator>(tmpSCAs); for(int i=0; i<tmpStrings.size(); i++) { tgSpringCableActuator::SpringCableActuatorHistory stringHist = tmpStrings[i]->getHistory(); for(int j=1; j<stringHist.tensionHistory.size(); j++) { const double previousTension = stringHist.tensionHistory[j-1]; const double previousLength = stringHist.restLengths[j-1]; const double currentLength = stringHist.restLengths[j]; //TODO: examine this assumption - free spinning motor may require more power double motorSpeed = (currentLength-previousLength); if(motorSpeed > 0) // Vestigial code motorSpeed = 0; const double workDone = previousTension * motorSpeed; totalEnergySpent += workDone; } } scores.push_back(totalEnergySpent); edgeAdapter.endEpisode(scores); nodeAdapter.endEpisode(scores); for(size_t i = 0; i < m_sineControllers.size(); i++) { delete m_sineControllers[i]; } m_sineControllers.clear(); m_allControllers.clear(); }
void JSONCPGControl::setupCPGs(BaseSpineModelLearning& subject, array_2D nodeActions, array_4D edgeActions) { std::vector <tgSpringCableActuator*> 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 tgCPGCableControl* pStringControl = new tgCPGCableControl(config); #else tgCPGActuatorControl* pStringControl = new tgCPGActuatorControl(); #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++) { tgCPGActuatorControl * 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 SpineFeedbackControl::setupCPGs(BaseSpineModelLearning& subject, array_2D nodeActions, array_4D edgeActions) { std::vector <tgSpringCableActuator*> allMuscles = subject.getAllMuscles(); CPGEquationsFB& m_CPGFBSys = *(tgCast::cast<CPGEquations, CPGEquationsFB>(m_pCPGSys)); for (std::size_t i = 0; i < allMuscles.size(); i++) { tgPIDController::Config config(20000.0, 0.0, 5.0, true); // Non backdrivable tgCPGCableControl* pStringControl = new tgCPGCableControl(config); allMuscles[i]->attach(pStringControl); // First assign node numbers pStringControl->assignNodeNumberFB(m_CPGFBSys, nodeActions); m_allControllers.push_back(pStringControl); } // Then determine connectivity and setup string for (std::size_t i = 0; i < m_allControllers.size(); i++) { tgCPGActuatorControl * 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); } } }
std::vector<double> JSONMixedLearningControl::getFeedback(BaseSpineModelLearning& subject) { // Placeholder std::vector<double> feedback; const std::vector<tgSpringCableActuator*>& allCables = subject.getAllMuscles(); double *inputs = new double[m_config.numStates]; std::size_t n = allCables.size(); for(std::size_t i = 0; i != n; i++) { std::vector< std::vector<double> > actions; const tgSpringCableActuator& cable = *(allCables[i]); std::vector<double > 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<double> tmpAct; for(int j=0;j<m_config.numActions;j++) { tmpAct.push_back(output[j]); } actions.push_back(tmpAct); std::vector<double> cableFeedback = transformFeedbackActions(actions); feedback.insert(feedback.end(), cableFeedback.begin(), cableFeedback.end()); } return feedback; }
void JSONCPGControl::onTeardown(BaseSpineModelLearning& subject) { scores.clear(); // @todo - check to make sure we ran for the right amount of time std::vector<double> 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<tgSpringCableActuator* > tmpStrings = subject.getAllMuscles(); for(std::size_t i=0; i<tmpStrings.size(); i++) { tgSpringCableActuator::SpringCableActuatorHistory stringHist = tmpStrings[i]->getHistory(); for(std::size_t j=1; j<stringHist.tensionHistory.size(); j++) { const double previousTension = stringHist.tensionHistory[j-1]; const double previousLength = stringHist.restLengths[j-1]; const double currentLength = stringHist.restLengths[j]; //TODO: examine this assumption - free spinning motor may require more power double motorSpeed = (currentLength-previousLength); if(motorSpeed > 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(); }
void LearningSpineJSON::setupCPGs(BaseSpineModelLearning& subject, array_2D nodeActions, array_4D edgeActions) { std::vector <tgSpringCableActuator*> allMuscles = subject.getAllMuscles(); for (std::size_t i = 0; i < allMuscles.size(); i++) { tgCPGStringControl_mod* pStringControl = new tgCPGStringControl_mod(); allMuscles[i]->attach(pStringControl); std::cout << allMuscles[i]->getTags() << std::endl; m_allControllers.push_back(pStringControl); } assert(m_allControllers.size() == allMuscles.size()); /// @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); } double tension; double kPosition; double kVelocity; double controlLength; // Then determine connectivity and setup string for (std::size_t i = 0; i < m_allControllers.size(); i++) { tgCPGActuatorControl * const pStringInfo = m_allControllers[i]; assert(pStringInfo != NULL); pStringInfo->setConnectivity(m_allControllers, edgeActions); if (allMuscles[i]->hasTag("inner top")) { tension = 2000.0; kPosition = 500.0; kVelocity = 100.0; controlLength = allMuscles[i]->getStartLength(); //controlLength = 19.5; } else if (allMuscles[i]->hasTag("outer top")) { tension = 1000.0; kPosition = 500.0; kVelocity = 100.0; controlLength = 18.5; } else if (allMuscles[i]->hasTag("inner")) { tension = 1500.0; kPosition = 300.0; kVelocity = 100.0; controlLength = allMuscles[i]->getStartLength(); //controlLength = 21.5; } else if (allMuscles[i]->hasTag("outer")) { tension = 800.0; kPosition = 300.0; kVelocity = 100.0; controlLength = 19.0 ; } else { throw std::runtime_error("Missing tags!"); } tgImpedanceController* p_ipc = new tgImpedanceController( tension, kPosition, kVelocity); pStringInfo->setupControl(*p_ipc, controlLength); } }
void TetraSpineCPGControl::setupCPGs(BaseSpineModelLearning& subject, array_2D nodeActions, array_4D edgeActions) { std::vector <tgSpringCableActuator*> allMuscles = subject.getAllMuscles(); for (std::size_t i = 0; i < allMuscles.size(); i++) { tgCPGStringControl* pStringControl = new tgCPGStringControl(); 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); } double tension; double kPosition; double kVelocity; double controlLength; // Then determine connectivity and setup string for (std::size_t i = 0; i < m_allControllers.size(); i++) { tgCPGStringControl * const pStringInfo = m_allControllers[i]; assert(pStringInfo != NULL); pStringInfo->setConnectivity(m_allControllers, edgeActions); //String will own this pointer #if (0) // origninal params if (allMuscles[i]->hasTag("outer")) { tension = 0.0; kPosition = 1000.0; kVelocity = 100.0; controlLength = 17.0; } else { tension = 0.0; kPosition = 1000.0; kVelocity = 100.0; controlLength = 15.0 ; } #else // Params for In Won if (allMuscles[i]->hasTag("outer")) { tension = 100.0; kPosition = 100.0; kVelocity = 200.0; controlLength = 19.5; } else { tension = 100.0; kPosition = 500.0; kVelocity = 200.0; controlLength = 16.5 ; } #endif tgImpedanceController* p_ipc = new tgImpedanceController( tension, kPosition, kVelocity); pStringInfo->setupControl(*p_ipc, controlLength); } }
void LearningSpineSine::setupWaves(BaseSpineModelLearning& subject, array_2D nodeActions, array_2D edgeActions) { std::vector <tgSpringCableActuator*> allMuscles = subject.getAllMuscles(); double tension; double kPosition; double kVelocity; double controlLength; for (std::size_t i = 0; i < allMuscles.size(); i++) { if (allMuscles[i]->hasTag("inner top")) { tension = 2000.0; kPosition = 500.0; kVelocity = 100.0; controlLength = allMuscles[i]->getStartLength(); } else if (allMuscles[i]->hasTag("outer top")) { tension = 1000.0; kPosition = 500.0; kVelocity = 100.0; controlLength = 19.5; } else if (allMuscles[i]->hasTag("inner")) { tension = 1500.0; kPosition = 100.0; kVelocity = 100.0; controlLength = allMuscles[i]->getStartLength(); } else if (allMuscles[i]->hasTag("outer")) { tension = 800.0; kPosition = 100.0; kVelocity = 100.0; controlLength = 19.5 ; } else { throw std::runtime_error("Missing tags!"); } tgImpedanceController* p_ipc = new tgImpedanceController( tension, kPosition, kVelocity); tgSineStringControl* pStringControl = new tgSineStringControl(m_config.controlTime, p_ipc, nodeActions[0][0], nodeActions[0][1], edgeActions[i][0], 0.0, // Repeat learning this too? Unlikely to be helpful controlLength); allMuscles[i]->attach(pStringControl); m_sineControllers.push_back(pStringControl); } assert(m_sineControllers.size() == allMuscles.size()); }