Having trouble applying quaternions correctly to a live simulation of a golf club using IMU data

Questions
, ,
1

I’ve got an IMU mounted into a golf club with axes as shown below:

Frame 1 (10)
Frame 1 (10)1839×1713 62.9 KB

I’ve got an STL file as well that has the same coordinate system as in the image.

My IMU code is sending quaternion data for the rotation of the club to the browser and I’m trying to get the club to rotate the correct way with the real club.

I’m having problems where the club isn’t rotating on the proper axes and also doesn’t start out at the position I want it to.

So for example, if I leave the club with the y axis pointing to the ground (club laying flat) I want that to be shown in three.js with the club laying flat. If I lift it and stand it up vertically, I want three.js to rotate it to the correct vertical position. If I start three.js and my IMU from the beginning and now I start it vertically, I want the club to start at the correct position.

To do this, I figured I’d use the accelerometer at the beginning when the club is not being touched or moved. I’d have a vector Ax, Ay, Az which I normalize to the direction of gravity. I then feed that in at every loop to rotate the club to that direction before I apply the rotation quaternion. The rotation quaternion is applied like this because it’s the quaternion that rotates FROM the initial position when the IMU was started to the current position. If I don’t rotate the club back to it’s initial position before rotating my the quaternion, it starts to go crazy and rotate faster and faster.

So my problem is that the axes aren’t lining up properly and I’m not sure where the problem lies.

Here’s the relevant code:

Animation function

function animate() {

  requestAnimationFrame(animate)

  controls.update()

  currentTime = Date.now()

  const dt = currentTime - prevTime

  //console.log(dt)

  if (dt > 5) {

    const quaternion = new THREE.Quaternion(

      currentQuat[0],

      currentQuat[1],

      currentQuat[2],

      currentQuat[3],

    )

    club.lookAt(initQuat.x, initQuat.y, initQuat.z)

    wireframe.lookAt(initQuat.x, initQuat.y, initQuat.z)

    quaternion.normalize()

    club.applyQuaternion(quaternion)

    wireframe.applyQuaternion(quaternion)

    i += 1

    prevTime = currentTime

  }

  renderer.render(scene, camera)

}

animate()

Here is where the quaternion get’s set from my websocket message:

   socket.addEventListener('message', function (event) {

      const msg = event.data.split(',')

      currentAccel[0] = msg[7]

      currentAccel[1] = msg[8]

      currentAccel[2] = msg[9]

      currentQuat[0] = msg[10]

      currentQuat[1] = msg[11]

      currentQuat[2] = msg[12]

      currentQuat[3] = msg[13]

      initQuat.x = msg[14]

      initQuat.y = msg[15]

      initQuat.z = msg[16]

      initQuat.normalize()


      console.log(initQuat)

    })

Here’s some C++ for where I’m converting the gyro readings to q_body. q_body is the quaternion that I send to three.js to rotate the object:

omega[0] = gyro[0] * DEG_TO_RAD;
    omega[1] = gyro[1] * DEG_TO_RAD;
    omega[2] = gyro[2] * DEG_TO_RAD;

    q_body_mag = sqrt(sq(omega[0]) + sq(omega[1]) + sq(omega[2]));
    gyro_dt = ((gyro_current_time - gyro_past_time) / 1000000.0);

    theta = q_body_mag * gyro_dt;
    q_gyro[0] = cos(theta / 2);
    q_gyro[1] = -(omega[0] / q_body_mag * sin(theta / 2));
    q_gyro[2] = -(omega[1] / q_body_mag * sin(theta / 2));
    q_gyro[3] = -(omega[2] / q_body_mag * sin(theta / 2));

    q[0] = q_body[0];
    q[1] = q_body[1];
    q[2] = q_body[2];
    q[3] = q_body[3];

    q_body[0] = q_gyro[0] * q[0] - q_gyro[1] * q[1] - q_gyro[2] * q[2] - q_gyro[3] * q[3];
    q_body[1] = q_gyro[0] * q[1] + q_gyro[1] * q[0] + q_gyro[2] * q[3] - q_gyro[3] * q[2];
    q_body[2] = q_gyro[0] * q[2] - q_gyro[1] * q[3] + q_gyro[2] * q[0] + q_gyro[3] * q[1];
    q_body[3] = q_gyro[0] * q[3] + q_gyro[1] * q[2] - q_gyro[2] * q[1] + q_gyro[3] * q[0];

Here’s some extra code for how I’m getting the acceleration vector initially. I take a bunch of readings and take the average of them. I apply this to q_body and also these are the Ax,Ay,Az values I send as the initial orientation:

void normalizeQuat(float *quat) {
  float magnitude = sqrt(quat[0] * quat[0] + quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3]);
  quat[0] /= magnitude;
  quat[1] /= magnitude;
  quat[2] /= magnitude;
}

void getInitialQBody() {
  float accelMean[3] = {0, 0, 0};
  for (int i = 0; i < 200; i += 1) {
    getIMU();
    accelMean[0] += accel[0] / 200.0f;
    accelMean[1] += accel[1] / 200.0f;
    accelMean[2] += accel[2] / 200.0f;
    delay(10);
  }

  gravQuat[0] = 0;
  gravQuat[1] = accelMean[0];
  gravQuat[2] = accelMean[1];
  gravQuat[3] = accelMean[2];
  normalizeQuat(gravQuat);
  q_body[0] = gravQuat[0];
  q_body[1] = gravQuat[1];
  q_body[2] = gravQuat[2];
  q_body[3] = gravQuat[3];
}
2

Sounds a bit like you’re trying to apply velocity in place where you should be applying acceleration?

3

Oh no, I’m not applying velocity or acceleration anywhere. I’m applying a rotation.

4

Well, angular velocity is still a velocity. :racing_car:

But regardless, can you describe the issue a bit shorter / share a video or a demo of it? It’s a bit hard to tell from the long post what exactly the issue is.

5

I don’t really get how you are getting your quaternion, but it’s possible that your gyro does not use the same coordinate system than three.js. Three.js use a right-handed system, if your gyro use a left-handed system I guess you would get issues similar to what you mentioned.

Also a lot of CAD tools are Z-up, whereas three.js is Z-forward

1 Like