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