ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

# Quaternion of a vector between two points

Having 2 points A and B in 3D space, im trying to workout the orientation of the vector (as a quaternion) between them, so that i can publish a PoseStamped message, which will essentially be the direction of one point to the other. I've been working on this for a bit and can't seem to get it to work properly.

i've provided a minimum working example of the code, I cant add the full python script as the formatting gets screwed up. I'm trying to get some more Karma so that I can add an image to illustrate the point and the code as an attachment.

Any help would be much appreciated.

The coordinate frame is "world". Could the coordinate system be the issue?

def publish_pose_from_vector_two(quat_pose, A, B):
a = np.cross(A, B);
x = a
y = a
z = a
A_length = np.linalg.norm(A)
B_length = np.linalg.norm(B)
w = math.sqrt((A_length ** 2) * (B_length ** 2)) + np.dot(A, B);

norm = math.sqrt(x ** 2 + y ** 2 + z ** 2 + w ** 2)
if norm == 0:
norm = 1

x /= norm
y /= norm
z /= norm
w /= norm

pose = PoseStamped()
pose.pose.position.x = A
pose.pose.position.y = A
pose.pose.position.z = A
pose.pose.orientation.x = x
pose.pose.orientation.y = y
pose.pose.orientation.z = z
pose.pose.orientation.w = w
quat_pose.publish(pose)

return

edit retag close merge delete

Sort by » oldest newest most voted

If you're fine not using PoseStamped, I'd recommand you using Marker. You can choose an arrow as marke type and add two points, which then will be used to create this arrow, therefore no orientation coordinates are needed. (LINK: http://wiki.ros.org/rviz/DisplayTypes...)

Supposed you have Point A and Point B with x, y, z position properties in C++:

#include <geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h>

const int MARKERTYPE = 0; // 0 = Arrow
//Example values
const double ARROWSHAFTDIAMETER = 10;
const double ARROWTRANSPARENCY = 1;
const double ARROWCOLORRED = 1;

visualization_msgs::Marker vector_msg;

vector_msg.type = MARKERTYPE;
vector_msg.id = 0; //any number except existing id
vector_msg.scale.x = ARROWSHAFTDIAMETER;
vector_msg.scale.a = ARROWTRANSPARENCY;
vector_msg.scale.r = ARROWREDCOLORRED;

geometry_msgs::Point originPoint;
geometry_msgs::Point destinyPoint;

//check if your data is in metric units before
originPoint.x = A.x;
originPoint.y = A.y;
originPoint.z = A.z;

destinyPoint.x = B.x;
destinyPoint.y = B.y;
destinyPoint.z = B.z;

vector_msg.points.push_back(originPoint);
vector_msg.points.push_back(destinyPoint);

//don't forget to change data type of the publisher to "visualization_msgs::Marker"
yourPublisher.publish(vector_msg);


if you want to change the size of the vector you can do it by using this formular

k: Wanted length of Vector
A: Origin Point
B: Original Destiniy Point
AB: Vector from A to B
|AB|: Length of Vector AB (determinted by sqrt((AB.x)² + (AB.y)² + (AB.z)²)
C: New Destiniy Point

A = k⋅A
C = k⋅(A + |AB|)

more

Quaternions are used to represent rotations, not directions. What does it mean to have a rotation in a direction?

A rotation does have an axis, and given two points you could create a rotation around the axis connecting the two points. You'd still have to choose an angle for the rotation.

Since you want to communicate the direction from one point to another, it's best to use the datatype intended for communicating directions, geometry_msgs/Vector3.

more