Page 1 of 1
App to move 3D with Quaternion to Pitch,Yaw,Roll
Posted: Wed Nov 20, 2024 1:09 pm
by 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 4525 times
Re: App to move 3D with Quaternion to Pitch,Yaw,Roll
Posted: Fri Nov 22, 2024 2:44 pm
by 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,
You can also view and edit the quaternion value directly in the properties window.
- Quaternions.jpg (45.93 KiB) Viewed 4296 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).
Re: App to move 3D with Quaternion to Pitch,Yaw,Roll
Posted: Fri Nov 22, 2024 5:28 pm
by 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
Re: App to move 3D with Quaternion to Pitch,Yaw,Roll
Posted: Mon Nov 25, 2024 12:43 pm
by 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]);
}
}