Calculate picking pose from mesh origin pose (using tf?)
I would like to pick and place multiple times an object I have in the collision world. The object gets moved in different states, and at the moment I don't have cameras on the scene so I can't infer the picking pose from them. I however always know the origin point of the mesh describing the object before the first P&P happens, and I know the distance from there to the point where the picking pose should lie. It's also worth noting the mesh origin pose doesn't have the same orientation as the picking pose (which should be with opposite verse z axis (=approach) and y axis according to the sliding direction of the grippers, like always).
Since P&P happens multiple times, I need to be able to set the new origin point for the mesh after placing the object, and afterwards calculate the picking pose based on this newly set origin point. This all seems theoretically feasible in some way with tf, however I'm having a hard time wrapping my head around a couple things:
Quaternions: given the initial orientation for the mesh {0.808, 0.000, -0.589, 0.000} (right hand with palm up, three fingers like axes, rotate y of maybe 15° counterclockwise), I know the picking pose will have to be oriented with a 180° rotation around the x axis from the previous pose. To rotate a quaternion I multiply it by that rotation (which is {0.0, 1.0, 0.0, 0.0}) and obtain the wrong result, however, and the same happens with the robot if I try to change the orientation like this. What did I misunderstand?
Transform: should this be published as a transform (in some shape or form) or is there a better way? Right now the object to be picked is instantiated with its origin pose, and I have ready a setup for a
getPickPose()for the robot's class to interface with, but this all sounds confusing. The rest of the code for P&P is ready and working and I tested were I to set manually the pick pose for all the places I place the object the code is reliable, but I reckon it should be doable, even without vision, like this somehow.
