C++ BaseSpineModelLearning::getAllMuscles方法代码示例

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());
  


    
}