Revision history [back]

To answer your direct question, you can get the quaternion of the vector from one end of the pen to the other by calculating that vector (i.e. subtract one endpoint from the other) and then using trigonometry to calculate the angles. There are many ways in the TF API to get a quaternion from that; one of them is to construct a quaternion from those angles using the appropriate constructor.

To answer your implied question, you need a full pose of the end of the pen. You can construct that as above. This will give you a pose in space for one end of the pen. You can offset that by the distance away from the end of the pen that you want the robot to hold it. This would give you a pose for the robot to grip at, which is what MoveIt! likes best.

To make this easy, you should publish the pose at the end of the pen using TF. Then you can add a second pose as a child of that end-of-pen pose as shown in this tutorial on publishing a child frame. That way TF will take care of all the messy geometry for you and you can just read the latest pose for the point on the pen where the robot should hold it.