fixed a subtle bug in urdf interpretation of kdl_parser_py

[eugene-katsevman]

rpy might be missing in origin tag and origin still might be non-identity

[clalancette]

I'm not overly familiar with KDL, so take some of this with a grain of salt. I quickly looked at what would happen before and after this change. Before, if either RPY or XYZ were unspecified, you would get the Identity matrix by calling Frame.Identity(). After this change, if both RPY and XYZ are unspecified, you still get the Identity matrix (by making a new Frame with all zeros). If either RPY or XYZ gets specified, you get just that part of the matrix filled in. Overall, this seems like a decent change to me. @sloretz any thoughts here?

@eugene-katsevman I would like to see tests added to make sure passing neither RPY or XYZ, passing just RPY, passing just XYZ, and passing both RPY and XYZ do the right thing. Other than that (and additional thoughts from @sloretz), I think I would be comfortable merging this. Thanks for the contribution!

[sloretz]

@eugene-katsevman This is a good catch. The roswiki documentation for urdf does say xyz or rpy default to "0 0 0" if they're not set.

I think the bug is further upstream in urdf_parser_py. The Pose object it creates should set rpy or xyz to a vector [0,0,0] rather than leaving it as None. This would match the behavior in the c++ counterpart ros/urdf_parser Pose.

@clalancette Thoughts about closing this and opening a bug in urdf_parser_py?

[eugene-katsevman]

@clalancette I'm not sure where to add a new test. test.urdf doesn't seem to be the right place since its links aren't supposed to have origin offset. What to do?

[eugene-katsevman]

@clalancette Is s3 back online? Can we rebuild this last commit?

[clalancette]

@sloretz Actually, yeah, I see what you mean about fixing it further upstream. That's not a bad idea, though I'm slightly concerned about backwards compatibility. Particularly, if there is some out-of-tree code that has something like:

p = Pose() if p.xyz is None: p.xyz = make_my_pose()

They are now going to get the Identity Pose, not the thing from make_my_pose(). That being said, I think the above is somewhat contrived. @sloretz Are you concerned about backwards compatibility, or should we just fix it in urdf_parser_py?

[sloretz]

@clalancette I vote to fix it in urdf_parser_py. It's true a user won't know if the pose was not set or set to zero vectors, but defaulting to zero vectors is the promised behavior on the roswiki.

I'm referring to <origin> tags on link, sensor, and joint http://wiki.ros.org/urdf/XML.

[sloretz]

@clalancette Do you know how to tell which ROS packages depend on urdfdom_py? We could ask downstream if the fix would affect them.

[clalancette]

@sloretz The way I've been finding out what are downstream projects is by going to repositories.ros.org, finding the package, clicking on one of the builds, and then looking at the "downstream projects". Of course, this only shows packages that are released into ROS (not, for instance, proprietary packages), but its a start.

In this particular case, the downstream packages to urdfdom_py seem to be: calibration_estimation calibration_launch jsk_rviz_plugins kdl_parser_py srdfdom

I'll take a quick look at them and see if any of them would be affected by a change to the Pose class.

[eugene-katsevman]

@clalancette any updates?

[clalancette]

Sorry, I haven't had time to get back to this in a while. I will try to this week.

[eugene-katsevman]

@clalancette I'm adding defaults for xyz/rpy at urdf_parser_py, now with tests. Should I continue? btw, is there any gitter channel or other way of communication? I have some questions regarding urdf_parser_py design

[gavanderhoorn]

@clalancette, @sloretz: is anything more needed from @eugene-katsevman to get this (or ros/urdf_parser_py#16) merged?

[sloretz]

@eugene-katsevman Now that ros/urdf_parser_py#16 is merged is there anything from this PR you'd like to keep? In particular does adding the check self.assertAlmostEqual(inertia.getCOG().z(), 3.0) mean the test will catch a type of bug it wouldn't have caught before?

[eugene-katsevman]

You see, the whole origin might be omitted and there is no default for the origin in urdf_parser_py. So this test still makes sense, at least for now.

[sloretz]

Makes sense this looks good to me then. I think it could be cherry-picked to indigo-devel as well.