Issue w/ Collision Checking with HeightFieldOBBRSS Constructed from Point Cloud

[esvaran-eureka]

Height_Field_Collision_Checking_Final.zip

The above folder contains 'height_field_script.py', which serves as a standalone script that reproduces the issue we're encountering currently in our environment. So, our task is to check for valid gripper positions for picking up objects in a given surrounding. Currently, the mechanism we have in place detects collisions between the gripper and the surrounding environment, which we're depicting as a point cloud using hppfcl.makeOctree() and the ComputeCollision function. The gripper, in this scenario, consists of three distinct objects (it's a two-finger gripper, so the base and the two fingers) positioned at different locations in the environment, depending on where the objects are placed. Now, these objects are placed inside of a bin, so grasp positions under the bin edges, despite not coming in contact with the point cloud itself are considered valid, whereas they should be invalid. To correct this behavior, we attempted to create a height field from the point cloud following this script.

I can confidently verify that the point cloud and height field are overlapping in my representation as I've visually confirmed it. I've included the code for visualizing it with plotly as well, just in case. As far as I'm aware, the collision checking mechanism with HeightFieldOBBRSS doesn't seem to work as intended, as I'm getting quite an unexpected result for the valid gripper positions. However, I will delve into the math slightly to explain the approach I took.

The point cloud is constructed from a top-down view with the translation matrix [0, 0.4, 1.5], so z-values increase towards the ground. The height field construction won't work from this reference frame as the z-axis is inverted, so I simply altered the z-values using a simple transformation, so now I'm operating from the reference frame [0, 0.4, 0] with the normal z-axis with z-values increasing upwards, which is what you'll see in the heights array and the transform that I've set. I can verify that collision checking does work because if I replace the heights array with a plane that exists above all the gripper positions, it rejects everything, so I'm quite unsure as to why it doesn't work in this scenario, especially considering that the point cloud and height field are overlapping (collision checking with the point cloud does work).

Here's a picture of what it considers valid in our environment, but that position is most certainly underneath the bin edge, whereas a few positions (most notably the bottommost position on the central object) definitely aren't in contact with any object or the height field.

If you could provide some clarity on how collisions are being computed with HeightFieldOBBRSS or better yet, why this specific case is quite problematic, that would be much appreciated. If you need any other information, please let me know.