void JSONCPGControl::onStep(BaseSpineModelLearning& subject, double dt)
{
m_updateTime += dt;
if (m_updateTime >= m_config.controlTime)
{
std::size_t numControllers = subject.getNumberofMuslces();
double descendingCommand = 2.0;
std::vector<double> 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 JSONGoalTensionNNW::onStep(BaseSpineModelLearning& subject, double dt)
{
m_updateTime += dt;
m_totalTime += dt;
double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];
if (m_updateTime >= m_config.controlTime)
{
#if (1) // Goal and cable
std::vector<double> desComs = getFeedback(subject);
const BaseSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, BaseSpineModelGoal>(subject);
getGoalFeedback(goalSubject);
#else // Just goal
std::size_t numControllers = subject.getNumberofMuslces() * 3;
double descendingCommand = 0.0;
std::vector<double> desComs (numControllers, descendingCommand);
const BaseSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, BaseSpineModelGoal>(subject);
getGoalFeedback(goalSubject);
#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;
//std::cout << m_totalTime << " " << currentHeight<< std::endl;
}
#if (0)
/// @todo add to config
if (currentHeight > 25 || currentHeight < 1.0)
{
/// @todo if bogus, stop trial (reset simulation)
bogus = true;
throw std::runtime_error("Height out of range");
}
#endif
}
void JSONQuadFeedbackControl::onStep(BaseSpineModelLearning& subject, double dt)
{
m_updateTime += dt;
if (m_updateTime >= m_config.controlTime)
{
#if (1)
std::vector<double> desComs = getFeedback(subject);
#else
std::size_t numControllers = subject.getNumberofMuslces() * 3;
double descendingCommand = 0.0;
std::vector<double> 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");
}
}