void tgCPGActuatorControl::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<const tgBulletSpringCableAnchor*> anchors =
tgCast::filter<const tgSpringCableAnchor, const tgBulletSpringCableAnchor>(subject.getSpringCable()->getAnchors());
std::size_t n = anchors.size();
assert(n >= 2);
m_pFromBody = anchors[0]->attachedBody;
m_pToBody = anchors[n - 1]->attachedBody;
}
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;
}
