Quaternion of a vector between two points

asked 2020-12-04 12:29:45 -0600

Bjb0403
1 ●3 ●7 ●3

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[0]
    y = a[1]
    z = a[2]
    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.header.frame_id = "world"
    pose.header.stamp = rospy.Time.now()
    pose.pose.position.x = A[0]
    pose.pose.position.y = A[1]
    pose.pose.position.z = A[2]
    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 flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2021-06-06 07:47:46 -0600

kajzuw
1

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 ARROWHEADDIAMETER = 20;
const double ARROWHEADLENGTH = 15;
const double ARROWTRANSPARENCY = 1;
const double ARROWCOLORRED = 1;

visualization_msgs::Marker vector_msg;

vector_msg.header.frame_id = "YouFrameId";

vector_msg.type = MARKERTYPE;
vector_msg.id = 0; //any number except existing id
vector_msg.scale.x = ARROWSHAFTDIAMETER;
vector_msg.scale.y = ARROWHEADDIAMETER;
vector_msg.scale.z = ARROWHEADLENGTH;
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|)

edit flag offensive delete link

add a comment

answered 2021-06-06 11:14:19 -0600

JohnStechschulte
43 ●1 ●6 ●8

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.

edit flag offensive delete link

add a comment

Quaternion of a vector between two points

2 Answers

Question Tools

Stats

Related questions

Quaternion of a vector between two points edit

2 Answers

Question Tools

Stats

Related questions

Quaternion of a vector between two points