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