Retrieving Path Information for STL Models in a URDF File

[lgzid]

I hope to use RL to obtain the path of the STL file corresponding to each link. How can I achieve this? Here is my code, and I hope to add a segment of code to implement the above functionality.

URDF file:

<link name="base_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/base.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/base.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="4.0"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
    </inertial>
  </link>

I wish to obtain the information like: package://ur_description/meshes/ur5/collision/base.stl

MY CODE:

void rbsRobotModel::loadRobot(string urdf_path, QString stl_path){
    rl::mdl::UrdfFactory factory;
    m_rlModel= factory.create(urdf_path);

    std::cout<<urdf_path<<std::endl;
    std::cout<<"m_rlModel->getDof()"<<m_rlModel->getDof()<<std::endl; // 7
    std::cout<<"m_rlModel->getBodies()"<<m_rlModel->getBodies()<<std::endl; // 9

    m_rlJointAngle = rl::math::constants::rad2deg * m_rlModel->getPosition();  //7

    if(rl::mdl::Kinematic* kinematic = dynamic_cast<rl::mdl::Kinematic*>(m_rlModel.get())){
        kinematic->setPosition(m_rlJointAngle * rl::math::constants::deg2rad);
        kinematic->forwardPosition();

        for(int i=0; i<8; i++){
            rl::math::Transform result = kinematic->getBodyFrame(i);
            Eigen::Matrix4d matrix = result.matrix();
            Eigen::Matrix3d rotationMatrix = result.rotation();
            Eigen::Vector3d translationVector = result.translation();
            robot->robotLinksmatrix.push_back(matrix);
            robot->robotLinksRotation.push_back(rotationMatrix);
            robot->robotLinksTranslation.push_back(translationVector);
        }
    }

[rickertm]

rl::mdl::UrdfFactory only extracts kinematic data from a URDF file, for the geometry part you can take a look at rl::sg::UrdfFactory. This class is able to extract meshes from STL file as long as SoSTLFileKit is available, however filenames have to be absolute or relative paths instead of the package:// style. It does not have an API for extracting the filenames, but you could modify it accordingly for your needs or adapt the related XPath queries in your own program.

rl/src/rl/sg/UrdfFactory.cpp

Lines 380 to 395 in c2f8621

	if (!boost::iends_with(meshFilename, "stl"))
	{
	throw Exception("rl::sg::UrdfFactory::load() - Only STL meshes currently supported");
	}
	::SoSTLFileKit* stlFileKit = new ::SoSTLFileKit();
	stlFileKit->ref();
	if (!stlFileKit->readFile(meshFilename.c_str()))
	{
	throw Exception("rl::sg::UrdfFactory::load() - Failed to open file '" + meshFilename + "'");
	}
	::SoSeparator* stl = stlFileKit->convert();
	stl->ref();
	stlFileKit->unref();