class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
{
btCompoundShape* compoundShape = new btCompoundShape();
m_data->m_allocatedCollisionShapes.push_back(compoundShape);
compoundShape->setMargin(0.001);
#if USE_ROS_URDF_PARSER
for (int v=0;v<(int)m_data->m_links[linkIndex]->collision_array.size();v++)
{
const Collision* col = m_data->m_links[linkIndex]->collision_array[v].get();
btCollisionShape* childShape = convertURDFToCollisionShape(col ,pathPrefix);
if (childShape)
{
btVector3 childPos(col->origin.position.x, col->origin.position.y, col->origin.position.z);
btQuaternion childOrn(col->origin.rotation.x, col->origin.rotation.y, col->origin.rotation.z, col->origin.rotation.w);
btTransform childTrans;
childTrans.setOrigin(childPos);
childTrans.setRotation(childOrn);
compoundShape->addChildShape(localInertiaFrame.inverse()*childTrans,childShape);
}
}
#else
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
for (int v=0;v<link->m_collisionArray.size();v++)
{
const UrdfCollision& col = link->m_collisionArray[v];
btCollisionShape* childShape = convertURDFToCollisionShape(&col ,pathPrefix);
if (childShape)
{
btTransform childTrans = col.m_linkLocalFrame;
compoundShape->addChildShape(localInertiaFrame.inverse()*childTrans,childShape);
}
}
}
#endif
return compoundShape;
}
class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
{
btCompoundShape* compoundShape = new btCompoundShape();
m_data->m_allocatedCollisionShapes.push_back(compoundShape);
compoundShape->setMargin(gUrdfDefaultCollisionMargin);
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
for (int v=0;v<link->m_collisionArray.size();v++)
{
const UrdfCollision& col = link->m_collisionArray[v];
btCollisionShape* childShape = convertURDFToCollisionShape(&col ,pathPrefix);
m_data->m_allocatedCollisionShapes.push_back(childShape);
if (childShape)
{
btTransform childTrans = col.m_linkLocalFrame;
compoundShape->addChildShape(localInertiaFrame.inverse()*childTrans,childShape);
}
}
}
return compoundShape;
}
