void tgCPGCableControl::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<tgSpringCableActuator, tgBasicActuator>(&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<tgSpringCableActuator, tgBasicActuator>(&subject); basicAct->moveMotors(dt); } } }
std::vector<double> JSONMetricsFeedbackControl::getCableState(const tgSpringCableActuator& cable) { // For each string, scale value from -1 to 1 based on initial length or max tension of motor std::vector<double> 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; }