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