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

# Applying a quaternion rotation to a vector

Hello,

I want to apply a quaternion rotation on a vector e.g. on v_orig = [0,0,1] a q = [0.5,0.5,0.5,0.5] rotation that the rotated vector v_new = [1,0,0]. How is this possible with ROS? Are there any functions to archive this? In the tf2 library I can define quaternions and poses but that doesn't do the thing, as far as I have seen.

The background is, that I have an approach vector in a defined orientation and I want to calculate a point which is e.g. 0.5 m away from the origin in this direction.

edit retag close merge delete

Are you using C++ or Python?

( 2019-06-02 23:09:39 -0500 )edit

I'm using C++

( 2019-06-03 01:08:26 -0500 )edit

How would one do it in python with ROS2?

( 2021-06-10 20:53:42 -0500 )edit

Sort by ยป oldest newest most voted

In tf2 you would need to use one of the supported datatypes. Suppose the vector you are talking about is a geometry_msgs/Vector3, then you could use this version of doTransform to transform your vector using a geometry_msgs/TransformStamped to represent the rotation. In your case the transform would have zeros for the translational components and you would use your desired quaterion for the orientation portion. Also note, you could use the fromMsg and toMsg functions to convert between tf2 datatypes and any of the other supported datatypes.

Also, I'll point out that the * operator has been overloaded in several ways for the tf2 Quaternions such that you could also directly perform quaternion multiplication to accomplish the rotation you are describing. Example 1 and Example 2.

more

tf2 has a built-in function for this called quatRotate(). It is defined here on line 436: Quaternion.h.

Usage:

tf2::Vector3 v_orig = ... ;
tf2::Quaternion q = ... ;
tf2::Vector3 v_new = quatRotate(q, v_orig);


To convert your quaternion/vector3 from the geometry_msgs format to tf2::Quaternion or tf2::Vector3, do

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

tf2::Quaternion q;
tf2::Vector3 v_orig;
tf2::fromMsg(my_msg.my_orientation, q); // q is now loaded with data
tf2::fromMsg(my_msg.my_vector, v_orig); // v_orig is now loaded with data

more