Feature/kinbodyparser

[Shushman]

A first pass at a Kinbody Parser under aikido::util. Points to note:

1. Only works if the KinBody has one Body element with 1 or 2 Geom elements, and Render and Data both defined, and refer to some file.
2. I've mostly followed the template in SkelParser
3. A problem - The plastic bowl and glass are two files that are read in just fine, but I get a strange error (originating from dart::dynamics::MeshShap::loadMesh) upon trying to load say the Pop tarts or kinbody. The error message is:

Warning [MeshShape.cpp:349] [MeshShape::loadMesh] Failed loading mesh 'package://pr_ordata/data/objects/kinova_tool.stl 0.1'.
Error [KinBodyParser.cpp:423] [KinBodyParser] Fail to load model[kinova_tool.stl 0.1].

Sometimes, even without the error message, the Pop tarts looks like:

---

This change is

[coveralls]

Coverage decreased (-4.08%) to 85.867% when pulling f11bb86 on feature/kinbodyparser into 293de45 on master.

[coveralls]

Coverage decreased (-5.1%) to 84.821% when pulling ef368c9 on feature/kinbodyparser into 293de45 on master.

[mkoval]

Reviewed 2 of 3 files at r1, 1 of 1 files at r2.
Review status: all files reviewed at latest revision, 12 unresolved discussions.

---

aikido/include/aikido/util/KinBodyParser.h, line 8 [r1] (raw file):

#include "dart/common/Uri.h"

//Currently assumes only one body node

Move this comment into the docstring of readKinBodyXMLFile.

---

aikido/include/aikido/util/KinBodyParser.h, line 14 [r1] (raw file):

namespace util {

namespace KinBodyParser {

The namespace hierarchy should exactly match the filesystem hierarchy. I suggest removing this namespace and leaving the file where it is unless readKinBodyXMLFile relies on an optional dependency.

---

aikido/include/aikido/util/KinBodyParser.h, line 18 [r1] (raw file):

  dart::dynamics::SkeletonPtr readKinBodyXMLFile(
    const dart::common::Uri& _fileUri,
    const dart::common::ResourceRetrieverPtr& _retriever = nullptr);

• Missing docstring.
• Don't indent the contents of a namespace.

---

aikido/src/util/KinBodyParser.cpp, line 159 [r1] (raw file):

  {
    dterr << "[KinBodyParser] KinBody  file[" << _fileUri.toString()
          << "] does not contain <KinBody> as the element.\n";

Typo: "the element" should be "the root element"?

---

aikido/src/util/KinBodyParser.cpp, line 178 [r1] (raw file):

{

  assert(_KinBodyElement != nullptr);

Handle this by returningnullptr instead of triggering an assertion.

---

aikido/src/util/KinBodyParser.cpp, line 185 [r1] (raw file):

  // Name attribute
  std::string name = dart::utils::getAttributeString(_KinBodyElement, "name");
  newSkeleton->setName(name);

What happens if there is no name attribute?

---

aikido/src/util/KinBodyParser.cpp, line 209 [r1] (raw file):

      Eigen::make_aligned_shared<dart::dynamics::FreeJoint::Properties>(
            dart::dynamics::Joint::Properties("root_joint", newBodyNode.initTransform));
  rootJoint.type = "free";

What is the point of creating this temporary SkelJoint structure? It seems like you could directly pass the FreeJoint::Properties into the createJointAndBodyNodePair function without all of this additional indirection.

---

aikido/src/util/KinBodyParser.cpp, line 235 [r1] (raw file):

  const dart::common::ResourceRetrieverPtr& _retriever)
{
  assert(_bodyNodeElement != nullptr);

Handle this gracefully instead of triggering an assertion.

---

aikido/src/util/KinBodyParser.cpp, line 241 [r1] (raw file):

  // Name attribute
  newBodyNode->mName = dart::utils::getAttributeString(_bodyNodeElement, "name");

What happens if there is no name attribute?

---

aikido/src/util/KinBodyParser.cpp, line 308 [r2] (raw file):

      assert(0);
    }
    else if(element_count == 1){

Can you add a comment explaining the logic in here? I am not sure what this is trying to accomplish.

---

aikido/src/util/KinBodyParser.cpp, line 413 [r2] (raw file):

  {
    std::string filename_ext = dart::utils::getValueString(vizOrColEle, fieldName);
    std::string filename = filename_ext.substr(0,filename_ext.find(" "));

What is the purpose of this? Please add a comment.

---

aikido/src/util/KinBodyParser.cpp, line 416 [r2] (raw file):

    const std::string meshUri = dart::common::Uri::getRelativeUri(_baseUri, filename);
    const aiScene* model = dart::dynamics::MeshShape::loadMesh(meshUri, _retriever);
    const Eigen::Vector3d scale(1.0,1.0,1.0); //Default scale as kinbody does not have info

Shouldn't we read the scale from the file, if it is specified?

---

Comments from Reviewable

[jslee02]

Edit: Sorry, nvm. This parser is a simplified version only for loading single body skeletons for specific use in Aikido.

[mkoval]

@Shushman What is the current status of this? Do you intend to address the remaining comments so we can merge this into master?

While I think this is a great addition, I am also fine with dropping support if you think we are at a point where we no longer need to regularly load .kinbody.xml files into DART. Do we have URDF versions of the critical files we need for pantry loading, etc?

[coveralls]

Coverage decreased (-4.0%) to 82.292% when pulling 8f4345a on feature/kinbodyparser into 67747e1 on master.

[jslee02]

I'm completing this PR as @Shushman almost finished.

My little concern is that there are some discrepancies between the spec and kinbody files in our repo. For example, the spec has <collision> for collision shape but it seems our kinbody files uses for it instead. I'll make the parser to be able to successfully parse our kinbody files then try to obey the spec.

[coveralls]

Coverage decreased (-2.0%) to 84.289% when pulling 1d9e9da on feature/kinbodyparser into 67747e1 on master.

[jslee02]

Replied in the each comment.

---

Review status: 0 of 10 files reviewed at latest revision, 12 unresolved discussions.

---

src/util/KinBodyParser.cpp, line 159 at r1 (raw file):

Previously, mkoval (Michael Koval) wrote…

Typo: "the element" should be "the root element"?

done.

---

src/util/KinBodyParser.cpp, line 178 at r1 (raw file):

Previously, mkoval (Michael Koval) wrote…

Handle this by returningnullptr instead of triggering an assertion.

The nullptr handling should be done by the caller. Added comment for the reason.

---

src/util/KinBodyParser.cpp, line 185 at r1 (raw file):

Previously, mkoval (Michael Koval) wrote…

What happens if there is no name attribute?

getAttributeStrin() returns an empty string for that case, and empty named skeleton will not cause any problem in parsing. Will add a message here to warn people just in case that's not their intention.

---

src/util/KinBodyParser.cpp, line 209 at r1 (raw file):

Previously, mkoval (Michael Koval) wrote…

What is the point of creating this temporary SkelJoint structure? It seems like you could directly pass the FreeJoint::Properties into the createJointAndBodyNodePair function without all of this additional indirection.

Removed the indirection.

---

src/util/KinBodyParser.cpp, line 235 at r1 (raw file):

Previously, mkoval (Michael Koval) wrote…

Handle this gracefully instead of triggering an assertion.

Added comment.

---

src/util/KinBodyParser.cpp, line 241 at r1 (raw file):

Previously, mkoval (Michael Koval) wrote…

What happens if there is no name attribute?

An empty string will be returned. We prints error message because an empty-named BodyNode causes problems in parsing (especially for parsing ). If bodyNodeInfo.valid == false, the caller will return nullptr skeleton.

---

src/util/KinBodyParser.cpp, line 308 at r2 (raw file):

Previously, mkoval (Michael Koval) wrote…

Can you add a comment explaining the logic in here? I am not sure what this is trying to accomplish.

The logic is modified with some reasonable comments.

---

src/util/KinBodyParser.cpp, line 413 at r2 (raw file):

Previously, mkoval (Michael Koval) wrote…

What is the purpose of this? Please add a comment.

I believe the original code is to parse and that contains the mesh file name and optionally the mesh scale. Added comment.

---

src/util/KinBodyParser.cpp, line 416 at r2 (raw file):

Previously, mkoval (Michael Koval) wrote…

Shouldn't we read the scale from the file, if it is specified?

Done.

---

aikido/include/aikido/util/KinBodyParser.h, line 8 at r1 (raw file):

Previously, mkoval (Michael Koval) wrote…

Move this comment into the docstring of readKinBodyXMLFile.

Done.

---

aikido/include/aikido/util/KinBodyParser.h, line 14 at r1 (raw file):

Previously, mkoval (Michael Koval) wrote…

The namespace hierarchy should exactly match the filesystem hierarchy. I suggest removing this namespace and leaving the file where it is unless readKinBodyXMLFile relies on an optional dependency.

Done. The parser doesn't require any optional dependency.

---

aikido/include/aikido/util/KinBodyParser.h, line 18 at r1 (raw file):

Previously, mkoval (Michael Koval) wrote…

• Missing docstring.
• Don't indent the contents of a namespace.

Done.

---

Comments from Reviewable

[jslee02]

@mkoval @Shushman I believe all the issues pointed out @mkoval are addressed in the recent commits.

The visual test is not done. I'm not familiar with using rviz so it would be grateful if someone could do that if the visualization issue @Shushman mentioned is still there. 😅

[coveralls]

Coverage decreased (-0.2%) to 86.063% when pulling 77d7499 on feature/kinbodyparser into 67747e1 on master.

[mkoval]

My little concern is that there are some discrepancies between the spec and kinbody files in our repo. For example, the spec has <collision> for collision shape but it seems our kinbody files uses for it instead. I'll make the parser to be able to successfully parse our kinbody files then try to obey the spec.

Can you compile a list of those discrepancies as you encounter them? Sadly, the spec for .kinbody.xml files is incomplete and actively misleading in some sections (particularly: how collision and visual geometry interact, especially for primitive shapes). The reality is that "the spec" is "whatever OpenRAVE and QtCoin do." I mostly developed or_rviz by tweaking the logic until it produced the same output as QtCoin on a variety of objects. 😓

[coveralls]

Coverage decreased (-0.2%) to 86.063% when pulling 96a19ef on feature/kinbodyparser into 3942ff7 on master.

[coveralls]

Coverage decreased (-0.2%) to 85.224% when pulling 873b72f on feature/kinbodyparser into 963a5d7 on master.

[mkoval]

box, sphere, cylinder selectively have the subfields among <extents>, <height>, <radius>, and they [are] assumed to have both of collision and render shapes in the simulation. There is [a] render attribute of <geom>, but this is usually omitted in our files.

The bolded part is incorrect. The <geom> tag directly defines collision geometry. The following logic determines what is used as the visual geometry:

1. If <geom>'s render attribute is false, display nothing. Otherwise...
2. If <render> is present, render the trimesh at the specified path. Otherwise...
3. Render the collision geometry.

The easiest way to understand this is realize that each <geom> tag is parsed into one instance of the GeometryInfo class by the GeometryInfoReader class. Note that GeometryInfo has both a _filenamerender member variable (for case (2) above) and _meshcollision (which is loaded from _filenamecollision for type="trimesh" collision geometry).

There is render attribute of <geom>, but this is usually omitted in our files.

This is correct. However, note that the render="true" attribute is a boolean that indicates whether or not the <geom> should be rendered at all (case (1) above). The <render> element specifies a path (and, optionally, a scale) for a trimesh file that should be used for visual geometry (case (2) above). The attribute defaults to true, so we do not typically specify it.

trimesh has at least one of <collision> and <render>.

It may have both and they have different meanings. <collision> (or <data>, see below) are used for <geom> with type="trimesh" to specify the path to a trimesh file to use for collision geometry. <render> is used by all <geom>s to specify a visual trimesh (case (2) above).

Use of <Data> instead of <collision>.

These are treated as equivalent in GeometryInfoReader.

kinova_tool.kinbody.xml contains <Render> field while the <geom> type is sphere. I modified the <geom> type to <trimesh> for the test file in this repo.

This is not correct. From the above description, it is impossible to create visual geometry without any associated collision geometry. There is a type="none" which theoretically supports this, but (if I remember correctly) it is not handled correctly everywhere in OpenRAVE (see rdiankov/openrave#306 for some effort I made in that direction). As a workaround, we often create a zero-radius sphere to represent "visual-only" geometry (e.g. see this comment in or_urdf).

---

I hope this helps. TL;DR: Your best bet is to mimic GeometryInfoReader as closely as possible.

[jslee02]

Thank you for helping me to understand the mysterious visualization logic of <Geom>. 😅

I believe the parser now correctly reads KinBody files; it could read the files in pr-ordata without any modifications.

[coveralls]

Coverage decreased (-0.1%) to 85.248% when pulling c9d371d on feature/kinbodyparser into 4cd2795 on master.

[mkoval]

I left a bunch of comments, but the only critical ones are:

1. Report an error if there is more than one Body in the KinBody, since that is not currently supported.
2. Fix handling of extents for boxes.
3. Handle three-element scale vectors for meshes.

Great work @jslee02! 🎉

[mkoval]

Note that scale may be a single float or three float`s, for (respectively) the x, y, and z-axes. This is implemented in OpenRAVE in this cryptic snippet:

_ss >> _pgeom->_filenamerender;
_ss >> _pgeom->_vRenderScale.x; _pgeom->_vRenderScale.y = _pgeom->_vRenderScale.z = _pgeom->_vRenderScale.x;
_ss >> _pgeom->_vRenderScale.y >> _pgeom->_vRenderScale.z;

[mkoval]

See my note above about scale. The same applies here (see this snippet).

[mkoval]

Extra ). at the end of this line.

[mkoval]

What does the default value of baseUri do?

[jslee02]

The default value of baseUri is empty. An empty base URI means that the URIs for mesh files should be absolute URIs or absolute paths. Added this to the docstring.

[mkoval]

Why must this be a file:// URI?

[jslee02]

It shouldn't need to be. Updated the comment: "The URI to a KinBody file. If the URI scheme is not file (i.e., file://), a relevant ResourceRetriever should be passed to retrieve the KinBody file."

[mkoval]

Check for non-positive extents.

[mkoval]

returning here also bypasses creation of visual geometry.

[jslee02]

I thought we don't want to create visual geometry for invalid geom type but changed not to so (create visual geometry) because, technically, we still can create one as long as <Render> element is provided. (wrong card)

[mkoval]

returning here also bypasses creation of visual geometry.

[jslee02]

I thought we don't want to create visual geometry for invalid geom type but changed not to so (create visual geometry) because, technically, we still can create one as long as <Render> element is provided.

[mkoval]

Very nice! 👍

[mkoval]

Nice helper function. I would be open to moving this to util, since it may be useful elsewhere.

[coveralls]

Coverage decreased (-0.4%) to 85.021% when pulling 8582a52 on feature/kinbodyparser into 4cd2795 on master.

[jslee02]

I believe I've addressed all the @mkoval's comments. @mkoval could you do another round of review?

[coveralls]

Coverage decreased (-0.4%) to 85.021% when pulling 1151ac9 on feature/kinbodyparser into 4cd2795 on master.

[coveralls]

Coverage decreased (-0.3%) to 85.048% when pulling 1151ac9 on feature/kinbodyparser into 4cd2795 on master.

[coveralls]

Coverage decreased (-0.3%) to 85.048% when pulling 1151ac9 on feature/kinbodyparser into 4cd2795 on master.

[mkoval]

I left a few nitpicks, but this otherwise looks good.

[mkoval]

Nit: Followings -> The following.

[mkoval]

Nit: "are required ones" -> "are required".

[mkoval]

Nits:

• "contains" -> "contain"
• "uni-scale" -> "for all three axes" (I thought the term uni-scale was a bit confusing, but it's fine to keep this if you

[mkoval]

Nits:

• Unscale typo? Also, see my comment above.
• Incorrect closing comment -->

[mkoval]

Add a note that any character this string is considered a delimiter, i.e. the string is interpreted as a set of delimiter characters, not as a single multi-character delimiter.

[mkoval]

Add \t as a default delimiter as well?

[mkoval]

Nit: Remove comment.

[mkoval]

Nit: Remove comment.

[mkoval]

Nit: Change this file to use the same spacing as the other .kinbody.xml files.

[coveralls]

Coverage decreased (-0.3%) to 85.048% when pulling 6104c21 on feature/kinbodyparser into 4cd2795 on master.