Print Link Origin Frame for Robot

[varunagrawal]

When we print a robot with robot.print(), is it useful to print the link origin frame as well?

As per the URDF docs, a joint's frame coincides with it's child link's origin frame, so printing this out would help with creating mental correspondences and ensuring the users understanding is correct.

All we would need to do is print out bMlink similar to how we print out bMcom.