Hi,
we had the same problem for our tendon-driven robot simuator and implemented set/getSegmentLength methods using the link resting length. Here's the code - it needs some tweaking to get it to work with the default Bullet version as we are using some custom extension classes:
Code:
void
btMuscle::setSegmentLength(const unsigned int& idxFirstLink,
const unsigned int& idxLastLink, const btScalar& length)
{
// if length equals current length -> return
if (length - getSegmentLength(idxFirstLink, idxLastLink) == 0.0)
{
return;
}
// maxSegmentsKiteLine: maximum number of rope segments
// maxSegmentSizeKiteLine: maximum length of one segment
if (length > maxSegmentsKiteLine * maxSegmentSizeKiteLine)
{ /// too long to accommodate with maxSegmentSize-sized segments
btScalar segmentSize = length / maxSegmentsKiteLine;
for (int i = idxFirstLink; i <= idxLastLink; i++)
{
softBody->m_nodes[i + 1].m_x
= lerp(softBody->m_nodes[i].m_x, softBody->m_nodes[i + 1].m_x,
segmentSize / softBody->m_links[i].m_rl);
softBody->m_nodes[i + 1].m_q
= lerp(softBody->m_nodes[i].m_q, softBody->m_nodes[i + 1].m_q,
segmentSize / softBody->m_links[i].m_rl);
softBody->m_links[i].m_rl = segmentSize;
}
softBody->updateNormals();
softBody->updateBounds();
updateConstants();
softBody->setTotalMass(mass);
return;
}
else
{
for (int i = idxFirstLink; i <= idxLastLink; i++)
{
if (softBody->m_links[i].m_rl > maxSegmentSizeKiteLine)
{
softBody->m_nodes[i + 1].m_x = lerp(softBody->m_nodes[i].m_x,
softBody->m_nodes[i + 1].m_x, maxSegmentSizeKiteLine
/ softBody->m_links[i].m_rl);
softBody->m_nodes[i + 1].m_q = lerp(softBody->m_nodes[i].m_q,
softBody->m_nodes[i + 1].m_q, maxSegmentSizeKiteLine
/ softBody->m_links[i].m_rl);
softBody->m_links[i].m_rl = maxSegmentSizeKiteLine;
}
}
}
btScalar deltaLength = length - getSegmentLength(idxFirstLink, idxLastLink);
btSoftBody::Link &lastLink = softBody->m_links[idxLastLink];
btSoftBody::Node &beforeLastNode = *(lastLink.m_n[0]);
btSoftBody::Node &lastNode = *(lastLink.m_n[1]);
btScalar nodeDistance = lastLink.m_rl;
btScalar scale = 1 + deltaLength / nodeDistance;
if (nodeDistance + deltaLength <= maxSegmentSizeKiteLine && nodeDistance
+ deltaLength > 0)
{ /// change link length
lastNode.m_x = lerp(beforeLastNode.m_x, lastNode.m_x, scale);
lastNode.m_q = lerp(beforeLastNode.m_q, lastNode.m_q, scale);
lastLink.m_rl += deltaLength;
}
else if (nodeDistance + deltaLength > maxSegmentSizeKiteLine)
{ /// Need to add node
if (nodeDistance + deltaLength > 2 * maxSegmentSizeKiteLine)
{
std::cerr
<< " *** WARNING *** The kite length is growing too fast. This could lead to trouble!"
<< std::endl;
}
btVector3 m_x = lerp(beforeLastNode.m_x, lastNode.m_x, scale);
// Fix the previous last node
lastNode.m_x = lerp(beforeLastNode.m_x, lastNode.m_x,
maxSegmentSizeKiteLine / nodeDistance);
lastNode.m_q = lerp(beforeLastNode.m_q, lastNode.m_q,
maxSegmentSizeKiteLine / nodeDistance);
lastLink.m_rl = maxSegmentSizeKiteLine;
// Add new node
softBody->appendNode(m_x, 1 / lastNode.m_im);
softBody->m_springAnchor->setSpringStartNode(softBody->m_nodes.size() - 1);
softBody->m_springAnchor->setSpringEndNode(softBody->m_nodes.size() - 1);
softBody->appendLink(softBody->m_nodes.size() - 2, softBody->m_nodes.size()
- 1);
softBody->m_links[softBody->m_links.size() - 1].m_rl = nodeDistance
+ deltaLength - maxSegmentSizeKiteLine;
}
/// Need to remove a node
else
{
if (idxFirstLink != idxLastLink)
{
softBody->m_springAnchor->setSpringEndNode(softBody->m_nodes.size() - 2);
softBody->m_springAnchor->setSpringStartNode(softBody->m_nodes.size() - 2);
softBody->m_ndbvt.remove(lastNode.m_leaf);
for (int i = idxLastLink + 1; i < softBody->m_nodes.size() - 1; i++)
{
softBody->m_nodes.swap(i, i + 1);
}
softBody->m_nodes.pop_back();
for (int i = idxLastLink; i < softBody->m_links.size() - 1; i++)
{
softBody->m_links.swap(i, i + 1);
}
softBody->m_links.pop_back();
std::cout << "Recursive call" << std::endl;
setSegmentLength(idxFirstLink, idxLastLink - 1, length);
}
else
{
std::cerr
<< " *** WARNING *** Attempt to have a zero-length or smaller kite. Request ignored!"
<< std::endl;
}
}
softBody->updateNormals();
softBody->updateBounds();
updateConstants();
softBody->setTotalMass(mass);
}
Code:
btScalar
btMuscle::getSegmentLength(const unsigned int& idxFirstLink,
const unsigned int& idxLastLink)
{
btScalar length = 0.0;
if (idxFirstLink >= 0 && idxFirstLink < softBody->m_links.size()
&& idxLastLink >= 0 && idxLastLink < softBody->m_links.size()
&& idxFirstLink <= idxLastLink)
{
for (int i = idxFirstLink; i <= idxLastLink; i++)
{
length += softBody->m_links[i].m_rl;
}
}
return length;
}