# How to calculate pose given a start point and end point?

Lets say I have two points(co-ordinates) defined with respect to a frame. If I can starting and ending point of the pair we can get direction similar to how arrow marker works in rviz. Is there anyway to get pose information out this specifically quaternion ?

Originally posted by Shiva_uchiha on ROS Answers with karma: 117 on 2022-12-30

Post score: 0

You can translate both points, so the first is in (0,0,0) global coordinates, and then calculate the necessary angles of the second point from, and then convert it to a quaternion representation.

E.g. you can use this tool to get hand of this before implementing it by yourself in the code. I would also check the tf_transformations package and similar (Eigen), as these types of calculations appear frequently and are probably already implemented somewhere.

Originally posted by ljaniec with karma: 3064 on 2022-12-30

This answer was NOT ACCEPTED on the original site

Post score: 1

I did something like this before to populate a geometry_msgs/PoseArray to visualize a cloud of vectors in Rviz. Is this what you're trying to do as well? On its own, a vector does not completely define an orientation, so you can't just directly calculate a quaternion from a vector.

However, you can make the vector the first basis vector (first column) in a rotation matrix and then arbitrarily construct the other two orthogonal basis vectors to make a complete orientation. This is physically meaningless, but it does result in a geometry_msgs/PoseArray that renders the vector properly in Rviz.

Here is how you can do this and get a quaternion:

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
...
tf2::Vector3 p1(x1, y1, z1); // Your start point
tf2::Vector3 p2(x2, y2, z2); // Your end point

tf2::Vector3 v = (p2 - p1).normalized();

tf2::Quaternion q;
tf2::Matrix3x3 rot_mat;
rot_mat[0] = v;
if (std::abs(v.z()) > 0.01) {
rot_mat[1] = tf2::Vector3(0, -v.z(), v.y()).normalized();
} else {
rot_mat[1] = tf2::Vector3(-v.y(), v.x(), 0).normalized();
}
rot_mat[2] = rot_mat[0].cross(rot_mat[1]);
rot_mat.getRotation(q);


Originally posted by robustify with karma: 956 on 2022-12-31

This answer was NOT ACCEPTED on the original site

Post score: 2