# Detecting pose of a pen

I'm able to get the 3d coordinates of two endpoints of a pen but I'm not sure how to convert that into a 3d pose. I saw the documentation and the 3d pose is in quaternions; is there a way to convert 3d coordinates to quaternions? Also for this problem, I don't need the orientation info just the vector in 3d space. I'm thinking of publishing 3 2d poses for x, y, and z axes; is there any other solutions to this?

Originally posted by paul_shuvo on ROS Answers with karma: 45 on 2019-02-03

Post score: 0

Comment by PeteBlackerThe3rd on 2019-02-03:
Two points do not define a pose, in your case the rotation around the axis of the pen is undefined. It sounds like a pose isn't really what you need. Can you describe the actual problem you're working on?

Comment by paul_shuvo on 2019-02-03:
I'm trying to make a robot manipulator hold the pen and write something on the paper. So, yes I don't need the rotation info around the axis rather the angle so that the robot manipulator can hold the pen from the base.

Okay, I understand your problem now, this task is slightly more complicated than simply producing a pose and sending it to Moveit.

Because of the rotational symmetry of the pen there are technically an infinite number of ways for the robot to grasp it, and although an arbitrarily chosen angle may work. The solution you really want, is to find which of these potential poses is the easiest for the arm to reach, and then move to that pose.

When I've looked at similar tasks in the past we computed 12 different poses, representing 12 evenly spaced rotations around the undefined axis and computed paths to all of them. We then chose the quickest of the motion plans as the 'easiest' grasp and executed that. The number 12 is fairly arbitrary here, you're trading processing time computing plans against the efficiency of the final motion you execute.

Hope this makes sense.

Originally posted by PeteBlackerThe3rd with karma: 9529 on 2019-02-03

This answer was ACCEPTED on the original site

Post score: 1

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.

Originally posted by Geoff with karma: 4203 on 2019-02-03

This answer was NOT ACCEPTED on the original site

Post score: 2