Use the segmented block information to recover point clouds of the objects. Estimate their reference frames using a similar SVD technique as for the tabletop plane (or maybe a PCA / eigenvector one will work).

Use rviz to visualize the reference frames at the centroids. It might be best to have the centroid be the robust mean in the (x,y) tabletop plane coordinates and the median or mode of (z) tabletop plane coordinates. That way the top surface is captured, not the edges.

Show that can change coordinates to manipulator frame and send hand to a block for pick-up. Will involve establishing the above-object pre-grasp pose, then moving down to grasp, etc. There is a similar Manipulator project activity for this that might provide some explanatory text).

Back – Main