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;
}
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);
}
}
}
void tgBulletRenderer::render(const tgSpringCableActuator& mSCA) const
{
#ifndef BT_NO_PROFILE
BT_PROFILE("tgBulletRenderer::renderString");
#endif //BT_NO_PROFILE
// Fetch the btDynamicsWorld
btDynamicsWorld& dynamicsWorld =
tgBulletUtil::worldToDynamicsWorld(m_world);
btIDebugDraw* const pDrawer = dynamicsWorld.getDebugDrawer();
const tgSpringCable* const pSpringCable = mSCA.getSpringCable();
if(pDrawer && pSpringCable)
{
const std::vector<const tgSpringCableAnchor*>& anchors = pSpringCable->getAnchors();
std::size_t n = anchors.size() - 1;
for (std::size_t i = 0; i < n; i++)
{
const btVector3 lineFrom =
anchors[i]->getWorldPosition();
const btVector3 lineTo =
anchors[i+1]->getWorldPosition();
// Should this be normalized??
const double stretch =
mSCA.getCurrentLength() - mSCA.getRestLength();
const btVector3 color =
(stretch < 0.0) ?
btVector3(0.0, 0.0, 1.0) :
btVector3(0.5 + stretch / 3.0,
0.5 - stretch / 2.0,
0.0);
pDrawer->drawLine(lineFrom, lineTo, color);
}
}
}
