ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

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

asked 2022-12-30 05:13:06 -0600

Shiva_uchiha gravatar image

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 ?

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
2

answered 2022-12-31 09:38:49 -0600

robustify gravatar image

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);
edit flag offensive delete link more
1

answered 2022-12-30 09:25:17 -0600

ljaniec gravatar image

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.

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2022-12-30 05:13:06 -0600

Seen: 496 times

Last updated: Dec 31 '22