Search code examples
drake

How to manually access the vertices and surface normals of a model in the plant?


Is there a workflow to extract vertices/surface normals from the plant without using the standard workflow. What I have tried is using plant.get_geometry_query_input_port(), then use member functions of the query_object to get the pairs. query_object.ComputeSignedDistancePairwiseClosestPoints() or signed_distance_pairs = query_object.ComputeSignedDistancePairwiseClosestPoints(); From one of the returned many pairs, then the point can be extracted using p_BgCb = signed_distance_pair.p_BCb in the model registered frame. My question is: how to get the points from any geometry in the scene in the body's registered frame manually? I tried to look for it in the documentation but to no avail.


Solution

  • Drake doesn't currently support those kinds of queries. SceneGraph knows about geometry "specifications" but doesn't deal directly with the implementations. It defers the details to the various underlying engines (as each engine has different requirements; for example, the proximity engine doesn't require a mesh to have normals. So, a mesh acceptable for signed distance queries may not have any normals specified at all.)

    From here on, I'm going to assume that you're thinking of geometries that are only meshes (as the ideas of "vertices" doesn't apply to primitives), then the only recourse you currently have would be to parse the mesh yourself. You can find the transforms that pose the geometry in its frame from SceneGraphInspector::GetPoseInFrame() or in the world from QueryObject::GetPoseInWorld(), but ultimately, you would have to consume the mesh definition, extract the information you care about and then measure and express it in whatever frame you need.

    Sorry for the inconvenience.

    The related question is: what end are you trying to achieve? Maybe there's a way to accomplish the same (or equivalent result) via a different mechanism.