Ask Your Question
0

Quaternion of a vector between two points

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

Bjb0403 gravatar image

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

2 Answers

Sort by » oldest newest most voted
0

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

kajzuw gravatar image

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 more
0

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

JohnStechschulte gravatar image

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 more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2020-12-04 12:29:45 -0500

Seen: 159 times

Last updated: Jun 06