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

Quaternion of a 3D vector

asked 2016-03-13 01:48:05 -0600

swethmandava gravatar image

My drone is to reach position x,y, z and orient itself along vector3D (ax, by, cz) wrt origin. In order for me to visualize this on rviz I must represent them in quaternion form. I understand that quaternions and vectors don't represent the same thing, but if I take my arbitrary axis as x-axis (vector [1,0,0]) and calculate quaternions between the two vectors, I get this result:

image description

As you can see here, the poses aren't all normal to the faces of the cube (my vectors are!) on rviz. What must I do to simply get the vector representation in quaternion form to visualize them?

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2016-04-13 04:14:21 -0600

swethmandava gravatar image
def  pose_from_vector3D(waypoint):
#http://lolengine.net/blog/2013/09/18/beautiful-maths-quaternion-from-vectors
pose= Pose()
pose.position.x = waypoint[0]
pose.position.y = waypoint[1]
pose.position.z = waypoint[2] 
#calculating the half-way vector.
u = [1,0,0]
norm = linalg.norm(waypoint[3:])
v = asarray(waypoint[3:])/norm 
if (array_equal(u, v)):
    pose.orientation.w = 1
    pose.orientation.x = 0
    pose.orientation.y = 0
    pose.orientation.z = 0
elif (array_equal(u, negative(v))):
    pose.orientation.w = 0
    pose.orientation.x = 0
    pose.orientation.y = 0
    pose.orientation.z = 1
else:
    half = [u[0]+v[0], u[1]+v[1], u[2]+v[2]]
    pose.orientation.w = dot(u, half)
    temp = cross(u, half)
    pose.orientation.x = temp[0]
    pose.orientation.y = temp[1]
    pose.orientation.z = temp[2]
norm = math.sqrt(pose.orientation.x*pose.orientation.x + pose.orientation.y*pose.orientation.y + 
    pose.orientation.z*pose.orientation.z + pose.orientation.w*pose.orientation.w)
if norm == 0:
    norm = 1
pose.orientation.x /= norm
pose.orientation.y /= norm
pose.orientation.z /= norm
pose.orientation.w /= norm
return pose

I realized my mistake, I didn't normalize the quaternion. This code works!

image description

edit flag offensive delete link more
3

answered 2016-03-13 17:41:53 -0600

robustify gravatar image

Technically, no unique quaternion can represent an arbitrary 3D vector. Assuming the vector represents the x axis of the transformed frame, you're essentially solving the following equation for the quaternion parameters given your vector [a_x, a_y, a_z]

image description

From this, it is clear that there is no unique solution, simply because there are four unknowns (the quaternion parameters), and only three independent equations. This can be verified intuitively as well. Imagine the x axis of a frame aligned along a particular orientation. The roll angle around that x axis is arbitrary, as it doesn't affect the orientation of the x axis relative to the base frame.

Therefore, the solution needs to be constrained in some way. One way is to constrain the y axis to be in a level plane w.r.t. the base frame (z = 0). Here is some code that I wrote using the Eigen library to construct a rotation matrix out of basis vectors that define the orientation of a vector. It assumes you have a using namespace Eigen; in your source file.

Vector3d u1, u2, u3;
Vector3d a;
a << 1, 2, 3; // Some arbitrary vector

u1 = a.normalized();

if ((fabs((double)u1(0)) > 0.001) || (fabs((double)u1(1)) > 0.001)) {
    u2 << -u1(1), u1(0), 0;
} else {
    u2 << 0, u1(2), -u1(1);
}
u2.normalize();

u3 = u1.cross(u2);

Matrix3d R;  // Rotation matrix defining orientation
R.col(0) = u1;
R.col(1) = u2;
R.col(2) = u3;

There are algorithms to convert a rotation matrix to a quaternion. Probably the simplest way to do this is to use a tf::Transform. First, convert the Eigen::Matrix3d to a tf::Matrix3x3 and call the setBasis() method of the tf::Transform. Then call the getRotation() method to get a tf::Quaternion with the parameters.

I hope this helps!

edit flag offensive delete link more

Comments

That makes sense! But since I only want the visual representation of the vector on rviz, I don't think I need a unique quaternion. As long as my x axis of the frame always points towards the vector, y and z shouldn't matter right. But again, let me know if I'm way off here @robustify

swethmandava gravatar image swethmandava  ( 2016-03-16 03:58:52 -0600 )edit

If you only want a visual arrow in Rviz, I would just make an Arrow marker. If you put the starting and ending points of the arrow you want to display in index 0 and 1 of the points field, it will just draw an arrow between those points instead of using the pose field of the marker message.

robustify gravatar image robustify  ( 2016-03-19 16:03:27 -0600 )edit

Also, even though you don't care about the alignment of the y and z axes, you still need to select a valid solution that yields the correct x axis alignment. While there are infinite solutions that provide the correct x axis alignment, there are also infinite incorrect solutions!

robustify gravatar image robustify  ( 2016-03-19 16:09:19 -0600 )edit

Question Tools

2 followers

Stats

Asked: 2016-03-13 01:48:05 -0600

Seen: 4,586 times

Last updated: Apr 13 '16