Revision history [back]

It looks like you're on the right track, and what you wrote made sense. You need to apply vector math and rotations. Rotations can be a bit tricky to sort through, but they are not rocket science either. Just remember that the order (and side) of multiplication counts and work your way through it. It helps to visualize orientations with tools like this or this. Remember that you can and should visualize TF frames in Rviz.

Remember also that the goal_pose defines the position and orientation that your endEffectorLink will be at. I find it helpful to define a frame at the tip of the gripper and use that.

You already use tf.transformations and tf_conversions which are probably the most appropriate tools for rotations in ROS and Python. You can define an orientation in a local frame (e.g. table) and assign it to the goal pose to have a "known good" orientation for your end effector.

Remember that you can also chain together elementary rotations to obtain the one you want. For example, you can first find the rotation in the ground plane, and then the angle at which you might look down at the object. Or if you don't want to do the math, you can just define the orientation manually as explained above. That's what I usually do.

And remember that the roll-pitch-yaw convention applies in the non-rotated coordinate system (the default order in the tf libraries you are using).

Maybe this and this example code can be of help (not that it's best practice, but it's a point of reference).

Good luck!