2
$\begingroup$

I am using Bullet Physic library to program some function, where I have difference between orientation from gyroscope given in quaternion and orientation of my object, and time between each frame in milisecond. All I want is set the orientation from my gyroscope to orientation of my object in 3D space. But all I can do is set angular velocity to my object. I have orientation difference and time, that is why I need vector of angular velocity [Wx,Wy,Wz] from that.

After reading this: angularVelocityArticle1 and this: angularVelocityArticle2

I did something like:

btQuaternion diffQuater = gyroQuater - boxQuater;
btQuaternion diffConjQuater;

diffConjQuater.setX(-(diffQuater.x()));
diffConjQuater.setY(-(diffQuater.y()));
diffConjQuater.setZ(-(diffQuater.z()));

////////////////
//W(t) = 2 * dq(t) /dt * conj(q(t))

btQuaternion velQuater;

velQuater = ((diffQuater * 2) / d_time) * diffConjQuater;

But it is not working as I expect, I mean, there is written, vector part of the result quaternion should be vector of angular velocity, and scalar part should be 0, but my result is not like that.

angular velocity vector represented as quaternion with zero scalar part, i.e W (t ) = (0, W x (t ), W y (t ), W z (t ))

Any ideas?

$\endgroup$

2 Answers 2

3
$\begingroup$

This question might go better over at the GameDev SE site; at first glance, though, this formula:

velQuater = ((diffQuater * 2) / d_time) * diffConjQuater;

is not representing what's in the comment above it. The formula you're looking for is $\omega(t) = 2q'(t)\bar{q}(t)$ (where as typical I've written $q'(t) = \frac{dq(t)}{dt}$), but what's written in that code is $\omega(t) = 2q'(t)\bar{q'}(t)$; in other words, you're not multiplying by the conjugate of your original orientation quaternion (as you should be) but by the conjugate of (the approximation of) the derivative. The line you want should be something like

velQuater = ((diffQuater * 2) / d_time) * conjBoxQuater;

but note that this is all predicated on another assumption - that gyroQuater and boxQuater represent the orientation of the same object at two nearby points in time. From your description it sounds like this might not be quite the case, and if it's not so then you may have to be more explicit about just what behavior you're after.

$\endgroup$
7
  • $\begingroup$ Thank you for your suggestion. Yes, these two objects have almost the same orientation, difference is very small, frames between each angular velocity setting is in milliseconds. But, unfortunately it is still not working as I expect (as it is described on info pages). $\endgroup$
    – Andrew
    Jun 20, 2012 at 21:01
  • $\begingroup$ @Andrew My revised line of code had a bug in it, too - I forgot to conjugate $q$ itself before multiplying! I've corrected it now; note that you might want to conjugate gyroQuater (instead of boxQuater), but that should matter a bit less. $\endgroup$ Jun 20, 2012 at 21:04
  • $\begingroup$ Thank you very much, although scalar part is not 0.0, but I can use vector part to set velocity. Unfortunately I have some error when I get do 120 degrees: $\endgroup$
    – Andrew
    Jun 20, 2012 at 21:31
  • $\begingroup$ The scalar part won't be 0.0 exactly, because of the approximations inherent in the estimate; still, you should find that it's no larger than (roughly) your time interval d_time. $\endgroup$ Jun 20, 2012 at 21:34
  • $\begingroup$ Can you tell me why it is problematic, when difference is not so small? (when speed of rotation gyroscope is high) And why it is problematic on 120 degrees angle and how to avoid that? I mean, I did condition if Q(w) is no larger than (roughly) my time interval d_time, but it makes another problems. Do you think working on morientation matrices instead of quaternions would be better? $\endgroup$
    – Andrew
    Jun 20, 2012 at 21:45
0
$\begingroup$

The way I'm doing this by using following code. Its inefficient however seems to be working.

static Vector3T toAngularVelocity(const QuaternionT& start, const QuaternionT& end, RealT delta_sec)
{
    RealT p_s, r_s, y_s;
    toEulerianAngle(start, p_s, r_s, y_s);

    RealT p_e, r_e, y_e;
    toEulerianAngle(end, p_e, r_e, y_e);

    RealT p_rate = (p_e - p_s) / delta_sec;
    RealT r_rate = (r_e - r_s) / delta_sec;
    RealT y_rate = (y_e - y_s) / delta_sec;

    //TODO: optimize below
    //Sec 1.3, https://ocw.mit.edu/courses/mechanical-engineering/2-154-maneuvering-and-control-of-surface-and-underwater-vehicles-13-49-fall-2004/lecture-notes/lec1.pdf
    RealT wx = r_rate       + 0                             - y_rate * sinf(p_e);
    RealT wy = 0            + p_rate * cosf(r_e)            + y_rate * sinf(r_e) * cosf(p_e);
    RealT wz = 0            - p_rate * sinf(r_e)            + y_rate * cosf(r_e) * cosf(p_e);

    return Vector3T(wx, wy, wz);
}


static void toEulerianAngle(const QuaternionT& q
    , RealT& pitch, RealT& roll, RealT& yaw)
{
    RealT ysqr = q.y() * q.y();

    // roll (x-axis rotation)
    RealT t0 = +2.0f * (q.w() * q.x() + q.y() * q.z());
    RealT t1 = +1.0f - 2.0f * (q.x() * q.x() + ysqr);
    roll = std::atan2f(t0, t1);

    // pitch (y-axis rotation)
    RealT t2 = +2.0f * (q.w() * q.y() - q.z() * q.x());
    t2 = ((t2 > 1.0f) ? 1.0f : t2);
    t2 = ((t2 < -1.0f) ? -1.0f : t2);
    pitch = std::asinf(t2);

    // yaw (z-axis rotation)
    RealT t3 = +2.0f * (q.w() * q.z() + q.x() * q.y());
    RealT t4 = +1.0f - 2.0f * (ysqr + q.z() * q.z());  
    yaw = std::atan2f(t3, t4);
}
$\endgroup$

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .