App to move 3D with Quaternion to Pitch,Yaw,Roll

[stefan.erni]

Hi Ben

Quaternions are an effective way to describe rotations in 3D space, as they avoid gimbal lock and allow smooth interpolations.
A quaternion has four components (x,y,z,w)

I already receive fused values from an IMU as a quaternion by BT.
I want to use these in with an App and move (rotate) a 3D model.
To make this work I will write a macro Quat_to_PYR

For begin I have a testfile with quaternion and the pitch, yaw, roll values. It is a simple rotation around the Z-axis.
So in my App,
Rotating the cubes with calculated data works but I guess I can't read the excel file in the App correct.

2024-11-20_13-51-24.PNG (23.33 KiB) Viewed 4522 times

FC10_Quat_to_PYR1.fcsx

(18.13 KiB) Downloaded 128 times

quat_pitch_yaw_roll.csv

(15.45 KiB) Downloaded 125 times

[BenR]

Hello,

I control the robot arm component using quaternions and they work very well.

If you want to see how I'm handling them then look towards the end of the ForwardKinematics macro here in the component source,

KinematicsArm.fcfx

(96.39 KiB) Downloaded 111 times

You can also view and edit the quaternion value directly in the properties window.

Quaternions.jpg (45.93 KiB) Viewed 4293 times

You can probably read from an excel .csv save fairly easily but reading direct from a .xlsx file is probably tricky (though probably not impossible).

[stefan.erni]

Hi Ben
I suspect that I cannot read the CSV file in my App correct

I guess I can't read the excel file in the App correct.

I have looked at your example, but have not yet understood it. I've already started with a macro and it's not working properly yet.
It would be nice if you could bring your suggestion into a Marco or make mine work. I'll keep trying

FC10_Quat_to_PYR1.fcsx

(58.89 KiB) Downloaded 110 times

[stefan.erni]

Hi Ben



For the Macro FP32_to_PYR_movment I have received these formulas calculations from STM.
I transferred these to the macro, but unfortunately it doesn't move properly

The LSM6DSV16X’s SFLP algorithm simplifies sensor fusion by offering pre-calibrated outputs for device orientation, gravity, and gyroscope bias.

Code: Select all

static void sflp2q(float quat[4], uint16_t sflp[3])
{
     float sumsq = 0;
     quat[0] = npy_half_to_float(sflp[0]);
     quat[1] = npy_half_to_float(sflp[1]);
     quat[2] = npy_half_to_float(sflp[2]);

     for (uint8_t i = 0; i < 3; i++)
     sumsq += quat[i] * quat[i];

     if (sumsq > 1.0f) {
     float n = sqrtf(sumsq);
     quat[0] /= n;
     quat[1] /= n;
     quat[2] /= n;
     sumsq = 1.0f;
}
     quat[3] = sqrtf(1.0f - sumsq);
}

Code: Select all

void quat_2_euler(float *euler, float *quat)

{
  float sx = quat[1];
  float sy = quat[2];
  float sz = quat[0];
  float sw = quat[3];
 
  if (sw < 0.0f) {

    sx *= -1.0f;
    sy *= -1.0f;
    sz *= -1.0f;
    sw *= -1.0f;

  }

  float sqx = sx * sx;
  float sqy = sy * sy;
  float sqz = sz * sz;

  euler[0] = -atan2f(2.0f * (sy * sw + sx * sz), 1.0f - 2.0f * (sqy + sqx));
  euler[1] = -atan2f(2.0f * (sx * sy + sz * sw), 1.0f - 2.0f * (sqx + sqz));
  euler[2] = -asinf(2.0f * (sx * sw - sy * sz));

   if (euler[0] < 0.0f)

    euler[0] += 2.0f * M_PI;
 
  for (uint8_t i = 0; i < 3; i++) {

    euler[i] = RAD2DEG(euler[i]);

  }

}