-
Notifications
You must be signed in to change notification settings - Fork 11
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Code error in Geometry? #13
Comments
Hey @dondonddon, thanks for your interest in my library. Let me take a run at your questions:
That if-statement is meant to avoid divide-by-zero errors that would occur on line 56 when So as far as I can tell both
Angular velocity represents the amount by which a rigid body will rotate per unit time (just as linear velocity represents the distance a body will travel per unit time). If you want to calculate how much a body will rotate in some For what it's worth, if you have a rigid body starting at some initial rotation Rotation R_t;
AngularVelocity w;
double dt;
Rotation R_t_plus_dt = exp(w * dt) * R_t; // rotation of some rigid body at time t + dt Hope that helps! |
Thanks Tom for taking the time to respond.
Here is your exp method code:
Rotation exp(const AngularVelocity& w)
{
auto theta = Norm((BLA::Matrix<3>)w);
if (fabs(theta) < 1e-5)
{
theta = 1.0;
}
auto so3_skew = skew(w / theta);
return BLA::Identity<3>() + so3_skew * sin(theta) + so3_skew * so3_skew * (1 - cos(theta));
}
What you say below (which I can understand) is that what you really want to pass into this method is the set of angles : W*dt. Then the exp method normalizes those angles. I can see your plan to make sure not to divide by zero. Then you show the matrix that has the sin and cos terms, again indicating that theta represents an angle and not angular rates. So what I had to do in my use of your code is pass in (w*dt) . Actually what I did was grab my attitude rates from the IMU and multiply by dt, then create an instance of “AngularVelocity” composed of those “delta angles”. Passing that into your exp method gives the correct response.
Looking at the case for small theta, I think the goal was really to use a small angle approximation, where sin(theta) -> theta, and (1- cos(theta)) -> 1 – (1 – theta^2/2). Since you skew method was passed the (W*dt)/theta, then your small-angle equation should have returned the Identity matrix + (skew*theta) + so3_skew^2* (theta/2).
Personally, I would have written:
if (fabs(theta) < 1e-5)
{
theta = 1.0;
auto so3_skew = skew(w / theta);
return BLA::Identity<3>() + so3_skew + so3_skew * so3_skew /2
}
auto so3_skew = skew(w / theta);
return BLA::Identity<3>() + so3_skew * sin(theta) + so3_skew * so3_skew * (1 - cos(theta));
}
This would have been correct to at least the theta squared level. But I can see a simpler implementation being to simply replace the “theta = 1.0” with “theta = 1.0e-5”.
Currently you are fixing the denominator for the call to the skew method to avoid division by zero, but you are also using sin (1) . The sine of 1 radian is 0.85, not something more like 1.0e-5 (or whatever small angle was used).
PS - I still ponder why you used the label so3_skew. Trying to imagine what so3 is…
Finally, just a note, but I appreciate you writing your code. I have looked around the I-net quite a bit to find similar algorithms that address gyro rate integration for Arduino micro-controllers and haven’t found anything yet that compares to your work. So thanks for doing it!
Don Pearson.
From: tomstewart89 ***@***.***>
Sent: Sunday, February 13, 2022 1:24 AM
To: tomstewart89/Geometry ***@***.***>
Cc: dondonddon ***@***.***>; Mention ***@***.***>
Subject: Re: [tomstewart89/Geometry] Code error in Geometry? (Issue #13)
Hey @dondonddon <https://github.com/dondonddon> , thanks for your interest in my library. Let me take a run at your questions:
Is line 53 in Geometry.cpp supposed to say: theta = 1.0e-5 ??
That if-statement is meant to avoid divide-by-zero errors that would occur on line 56 when AngularVelocitys that are close to zero are passed to exp. In those cases, with theta = 1.0 on line 53, so3_skew will be close to zero and the terms involving theta on line 57 will evaluate to ~zero. With theta = 1e-5 then so3_skew won't be close to zero but the same terms on line 57 will evaluate to ~zero anyway since sin(1e-5)~= 0 and 1 - cos(1e-5) ~=0.
So as far as I can tell both 1.0 and 1e-5 will both produce more or less the same result. Are you having some trouble with that function? Is there something I'm missing there?
Also, you have a class called AngularVelocity, but I think the values are supposed to be gyro rates * integration time ? Otherwise, how do you account for the delta-T in order to integrate the angular rates?
Angular velocity represents the amount by which a rigid body will rotate per unit time (just as linear velocity represents the distance a body will travel per unit time). If you want to calculate how much a body will rotate in some integration time then yeah you'll need to scale accordingly.
For what it's worth if you have a rigid body starting at some initial rotation R_t which rotates with a constant angular velocity w over some time interval dt then the code to integrate this velocity looks like this:
Rotation R_t;
AngularVelocity w;
double dt;
Rotation R_t_plus_dt = exp(w * dt) * R_t; // rotation of some rigid body at time t + dt
Hope that helps!
—
Reply to this email directly, view it on GitHub <#13 (comment)> , or unsubscribe <https://github.com/notifications/unsubscribe-auth/ARIBG6TYPY7YSHYGRX4JE7DU252D5ANCNFSM5OH5ZUMA> .
Triage notifications on the go with GitHub Mobile for iOS <https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675> or Android <https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub> .
You are receiving this because you were mentioned.Message ID: ***@***.***>
|
Hi Don,
That's a sensible guess, but to be honest I just wanted to avoid the potential divide-by-zero error on line 56 by setting theta to an arbitrary (non-zero) value. Since On reflection though, I can see how that can give quite inaccurate results when integrating rates from a gyro. How would you feel about the following implementation? Here we explicitly return an identity rotation when the magnitude of theta is below Rotation exp(const AngularVelocity& w)
{
auto theta = Norm((BLA::Matrix<3>)w);
if (fabs(theta) < 1e-5)
{
return BLA::Identity<3>();
}
else
{
auto so3_skew = skew(w / theta);
return BLA::Identity<3>() + so3_skew * sin(theta) + so3_skew * so3_skew * (1 - cos(theta));
}
} Incidentally, regarding this point:
The |
Yes, I like the idea of just returning the identity matrix. For my application (Arduino Nano 33 iot IMU), the gyro noise seems to be on the order of 0.1 deg/sec. When integrating at 50 ms, that is approximately 8e-5 radians of angle change, still above the 1e-5 level.
Since we are in the code, you might want to look at the attachments. In the aerospace industry, I was more used to PYR and YPR sequences than RPY. The attachment shows that you are correctly rotating vectors and providing the components of the EulerAngle object, but when the user uses the phi() method or theta() or even psi () methods, the values returned are not what I would expect, unless I am only using the RPY sequence.
Perhaps it is better to rename those methods: firstAngle(), secondAngle(), thirdAngle() – and leave the remaining code as is?
From: tomstewart89 ***@***.***>
Sent: Wednesday, February 16, 2022 3:19 AM
To: tomstewart89/Geometry ***@***.***>
Cc: dondonddon ***@***.***>; Mention ***@***.***>
Subject: Re: [tomstewart89/Geometry] Code error in Geometry? (Issue #13)
Hi Don,
Looking at the case for small theta, I think the goal was really to use a small angle approximation
That's a sensible guess, but to be honest I just wanted to avoid the potential divide-by-zero error on line 56 by setting theta to an arbitrary (non-zero) value. Since so3_skew would be a 3x3 matrix whose elements are almost zero in those cases anyway, the logic was that multiplying it by sin / cos of an arbitrary constant wouldn't really affect the output of the exp function.
On reflection though, I can see how that can give quite inaccurate results when integrating rates from a gyro. How would you feel about the following implementation? Here we explicitly return an identity rotation when the magnitude of theta is below 1e-5. Depending the sensitivity and sampling rate of your gyro that 1e-5 might cause some of your readings to be ignored and if that's the case we can change the underlying element type from float to double and then decrease that to say 1e-7.
Rotation exp(const AngularVelocity& w)
{
auto theta = Norm((BLA::Matrix<3>)w);
if (fabs(theta) < 1e-5)
{
return BLA::Identity<3>();
}
else
{
auto so3_skew = skew(w / theta);
return BLA::Identity<3>() + so3_skew * sin(theta) + so3_skew * so3_skew * (1 - cos(theta));
}
}
Incidentally, regarding this point:
I still ponder why you used the label so3_skew
The so there refers to the <https://mathworld.wolfram.com/SpecialOrthogonalGroup.html> special orthogonal group; a lie group which represents the set of all 3D rotations (hence the 3). If you want to read up on that in the context of robotics have a look at chapter 3 of this book <http://hades.mech.northwestern.edu/index.php/Modern_Robotics> , it's a good read!
—
Reply to this email directly, view it on GitHub <#13 (comment)> , or unsubscribe <https://github.com/notifications/unsubscribe-auth/ARIBG6TER23PQE2N5CY4N2TU3OBY7ANCNFSM5OH5ZUMA> .
Triage notifications on the go with GitHub Mobile for iOS <https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675> or Android <https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub> .
You are receiving this because you were mentioned.Message ID: ***@***.***>
don_rotations_checkout
Case #1a: Rotate VECTOR 90 deg about Z axis:
c_mat =
[[0.00,-1.00,0.00],[1.00,0.00,0.00],[0.00,0.00,1.00]]
body vector:
[[0.00],[1.00],[0.00]]
Euler angles (radians) - RPY order:
[[0.00],[0.00],[1.57]]
Roll(phi) =0.00
Pitch (theta) =0.00
Yaw (psi) =90.00
-------------------------------------------------------
Euler angles (radians) - PYR order:
[[0.00],[1.57],[0.00]]
Roll(phi) =0.00
Pitch (theta) =90.00
Yaw (psi) =0.00
-------------------------------------------------------
Case #1b: Rotate VECTOR 90 deg about - Z axis: (same as rotating frame +90 about +Z)
c_mat =
[[0.00,1.00,0.00],[-1.00,0.00,0.00],[0.00,0.00,1.00]]
body vector:
[[0.00],[-1.00],[0.00]]
Euler angles (radians) - RPY order:
[[0.00],[0.00],[-1.57]]
Roll(phi) =0.00
Pitch (theta) =0.00
Yaw (psi) =-90.00
-------------------------------------------------------
Euler angles (radians) - PYR order:
[[0.00],[-1.57],[0.00]]
Roll(phi) =0.00
Pitch (theta) =-90.00
Yaw (psi) =0.00
-------------------------------------------------------
Case #2: Rotate VECTOR 90 deg about X axis:
c_mat =
[[1.00,0.00,0.00],[0.00,0.00,-1.00],[0.00,1.00,0.00]]
body vector:
[[1.00],[0.00],[0.00]]
Euler angles (radians) - RPY order:
[[1.57],[0.00],[0.00]]
Roll(phi) =90.00
Pitch (theta) =0.00
Yaw (psi) =0.00
-------------------------------------------------------
Euler angles (radians) - PYR order:
[[0.00],[0.00],[1.57]]
Roll(phi) =0.00
Pitch (theta) =0.00
Yaw (psi) =90.00
-------------------------------------------------------
Case 3: Rotate VECTOR 45 deg about Z axis:
c_mat =
[[0.71,-0.71,0.00],[0.71,0.71,0.00],[0.00,0.00,1.00]]
body vector:
[[0.71],[0.71],[0.00]]
Euler angles (radians) - RPY order:
[[0.00],[0.00],[0.79]]
Roll(phi) =0.00
Pitch (theta) =0.00
Yaw (psi) =45.00
-------------------------------------------------------
Euler angles (radians) - PYR order:
[[0.00],[0.79],[0.00]]
Roll(phi) =0.00
Pitch (theta) =45.00
Yaw (psi) =0.00
-------------------------------------------------------
|
Hey @dondonddon,
That sounds reasonable, I went ahead and made those changes (I went with just You can check out both of those changes here: https://github.com/tomstewart89/Geometry/releases/tag/2.2 And with that I might close this issue. Thanks again for your feedback, I'm glad you're getting some use out of this library (give it a star if you haven't already!). If you have any other suggestions feel free to open up another issue. |
Tom, I think I found another “concern” with your software.
In the enclosed cases (.ino file) you will see the same checkout runs I did to find the issue with pitch(), yaw(), etc which you fixed with first(), second(), etc.
Here, I am using the rotation matrix and calling your EulerAngles class. When calling it with “RotationFrame: Rotating”, I was expecting to get essentially the inverse of the euler angles (since I am only rotating about a single axis). I figured this is where you change from “rotating vectors” to “rotating the coordinate system”. It appears that the sequencing of the output is messed up. I think it might be due to you not carrying an annotation as an instance variable in your class the idea of static vs rotating? I am not really sure of the issue, but am fairly confident that the results are incorrect.
From: tomstewart89 ***@***.***>
Sent: Friday, February 18, 2022 21:53 PM
To: tomstewart89/Geometry ***@***.***>
Cc: dondonddon ***@***.***>; Mention ***@***.***>
Subject: Re: [tomstewart89/Geometry] Code error in Geometry? (Issue #13)
Closed #13 <#13> .
—
Reply to this email directly, view it on GitHub <#13 (comment)> , or unsubscribe <https://github.com/notifications/unsubscribe-auth/ARIBG6QDC2CMSP2UCIWHCSTU34V5TANCNFSM5OH5ZUMA> .
Triage notifications on the go with GitHub Mobile for iOS <https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675> or Android <https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub> .
You are receiving this because you were mentioned.Message ID: ***@***.***>
don_rotations_checkout
Case #1a: Rotate VECTOR 90 deg about Z axis:
c_mat =
[[0.00,-1.00,0.00],[1.00,0.00,0.00],[0.00,0.00,1.00]]
body vector:
[[0.00],[1.00],[0.00]]
Euler angles (radians) - RPY order:
[[0.00],[0.00],[1.57]]
Roll(phi) =0.00
Pitch (theta) =0.00
Yaw (psi) =90.00
-------------------------------------------------------
Euler angles (radians) - PYR order:
[[0.00],[1.57],[0.00]]
Roll(phi) =0.00
Pitch (theta) =0.00
Yaw (psi) =90.00
-------------------------------------------------------
Euler angles (radians) - PYR order: -- but using RotationFrame::Rotating
[[0.00],[1.57],[0.00]]
Roll(phi) =0.00
Pitch (theta) =90.00
Yaw (psi) =0.00
-------------------------------------------------------
Case #1b: Rotate VECTOR 90 deg about - Z axis: (same as rotating frame +90 about +Z)
c_mat =
[[0.00,1.00,0.00],[-1.00,0.00,0.00],[0.00,0.00,1.00]]
body vector:
[[0.00],[-1.00],[0.00]]
Euler angles (radians) - RPY order:
[[0.00],[0.00],[-1.57]]
Roll(phi) =0.00
Pitch (theta) =0.00
Yaw (psi) =-90.00
-------------------------------------------------------
Euler angles (radians) - PYR order:
[[0.00],[-1.57],[0.00]]
Roll(phi) =0.00
Pitch (theta) =0.00
Yaw (psi) =-90.00
-------------------------------------------------------
Euler angles (radians) - PYR order: -- but using RotationFrame::Rotating
[[0.00],[-1.57],[0.00]]
Roll(phi) =0.00
Pitch (theta) =-90.00
Yaw (psi) =0.00
-------------------------------------------------------
Case #2: Rotate VECTOR 90 deg about X axis:
c_mat =
[[1.00,0.00,0.00],[0.00,0.00,-1.00],[0.00,1.00,0.00]]
body vector:
[[1.00],[0.00],[0.00]]
Euler angles (radians) - RPY order:
[[1.57],[0.00],[0.00]]
Roll(phi) =90.00
Pitch (theta) =0.00
Yaw (psi) =0.00
-------------------------------------------------------
Euler angles (radians) - PYR order:
[[0.00],[0.00],[1.57]]
Roll(phi) =90.00
Pitch (theta) =0.00
Yaw (psi) =0.00
-------------------------------------------------------
Euler angles (radians) - PYR order: -- but using RotationFrame::Rotating
[[1.57],[0.00],[0.00]]
Roll(phi) =90.00
Pitch (theta) =0.00
Yaw (psi) =0.00
-------------------------------------------------------
Case 3: Rotate VECTOR 45 deg about Z axis:
c_mat =
[[0.71,-0.71,0.00],[0.71,0.71,0.00],[0.00,0.00,1.00]]
body vector:
[[0.71],[0.71],[0.00]]
Euler angles (radians) - RPY order:
[[0.00],[0.00],[0.79]]
Roll(phi) =0.00
Pitch (theta) =0.00
Yaw (psi) =45.00
-------------------------------------------------------
Euler angles (radians) - PYR order:
[[0.00],[0.79],[0.00]]
Roll(phi) =0.00
Pitch (theta) =0.00
Yaw (psi) =45.00
-------------------------------------------------------
Euler angles (radians) - PYR order: -- but using RotationFrame::Rotating
[[0.00],[0.79],[0.00]]
Roll(phi) =0.00
Pitch (theta) =45.00
Yaw (psi) =0.00
-------------------------------------------------------
|
I am sorry. I had the wrong attachments. Here are the correct ones….
From: ***@***.*** ***@***.***>
Sent: Tuesday, February 22, 2022 17:29 PM
To: 'tomstewart89/Geometry' ***@***.***>; 'tomstewart89/Geometry' ***@***.***>
Cc: 'Mention' ***@***.***>
Subject: RE: [tomstewart89/Geometry] Code error in Geometry? (Issue #13)
Tom, I think I found another “concern” with your software.
In the enclosed cases (.ino file) you will see the same checkout runs I did to find the issue with pitch(), yaw(), etc which you fixed with first(), second(), etc.
Here, I am using the rotation matrix and calling your EulerAngles class. When calling it with “RotationFrame: Rotating”, I was expecting to get essentially the inverse of the euler angles (since I am only rotating about a single axis). I figured this is where you change from “rotating vectors” to “rotating the coordinate system”. It appears that the sequencing of the output is messed up. I think it might be due to you not carrying an annotation as an instance variable in your class the idea of static vs rotating? I am not really sure of the issue, but am fairly confident that the results are incorrect.
From: tomstewart89 ***@***.*** ***@***.***> >
Sent: Friday, February 18, 2022 21:53 PM
To: tomstewart89/Geometry ***@***.*** ***@***.***> >
Cc: dondonddon ***@***.*** ***@***.***> >; Mention ***@***.*** ***@***.***> >
Subject: Re: [tomstewart89/Geometry] Code error in Geometry? (Issue #13)
Closed #13 <#13> .
—
Reply to this email directly, view it on GitHub <#13 (comment)> , or unsubscribe <https://github.com/notifications/unsubscribe-auth/ARIBG6QDC2CMSP2UCIWHCSTU34V5TANCNFSM5OH5ZUMA> .
Triage notifications on the go with GitHub Mobile for iOS <https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675> or Android <https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub> .
You are receiving this because you were mentioned.Message ID: ***@***.*** ***@***.***> >
don_rotations_checkout
Case #1a: Rotate VECTOR 90 deg about Z axis:
c_mat =
[[0.00,-1.00,0.00],[1.00,0.00,0.00],[0.00,0.00,1.00]]
body vector:
[[0.00],[1.00],[0.00]]
Euler angles (radians) - RPY order:
[[0.00],[0.00],[1.57]]
Roll(phi) =0.00
Pitch (theta) =0.00
Yaw (psi) =90.00
-------------------------------------------------------
Euler angles (radians) - PYR order:
[[0.00],[1.57],[0.00]]
Pitch(theta) =0.00
Yaw (psi) =0.00
Roll (phi) =90.00
-------------------------------------------------------
Euler angles (radians) - PYR order: -- but using RotationFrame::Rotating
[[0.00],[1.57],[0.00]]
Pitch(theta0.00
Yaw (psi) =90.00
Roll (phi) =0.00
-------------------------------------------------------
Case #1b: Rotate VECTOR 90 deg about - Z axis: (same as rotating frame +90 about +Z)
c_mat =
[[0.00,1.00,0.00],[-1.00,0.00,0.00],[0.00,0.00,1.00]]
body vector:
[[0.00],[-1.00],[0.00]]
Euler angles (radians) - RPY order:
[[0.00],[0.00],[-1.57]]
Roll(phi) =0.00
Pitch (theta) =0.00
Yaw (psi) =-90.00
-------------------------------------------------------
Euler angles (radians) - PYR order:
[[0.00],[-1.57],[0.00]]
Pitch(theta) =0.00
Yaw (psi) =0.00
Roll (phi) =-90.00
-------------------------------------------------------
Euler angles (radians) - PYR order: -- but using RotationFrame::Rotating
[[0.00],[-1.57],[0.00]]
Pitch(theta0.00
Yaw (psi) =-90.00
Roll (phi) =0.00
-------------------------------------------------------
Case #2: Rotate VECTOR 90 deg about X axis:
c_mat =
[[1.00,0.00,0.00],[0.00,0.00,-1.00],[0.00,1.00,0.00]]
body vector:
[[1.00],[0.00],[0.00]]
Euler angles (radians) - RPY order:
[[1.57],[0.00],[0.00]]
Roll(phi) =90.00
Pitch (theta) =0.00
Yaw (psi) =0.00
-------------------------------------------------------
Euler angles (radians) - PYR order:
[[0.00],[0.00],[1.57]]
Pitch(theta) =90.00
Yaw (psi) =0.00
Roll (phi) =0.00
-------------------------------------------------------
Euler angles (radians) - PYR order: -- but using RotationFrame::Rotating
[[1.57],[0.00],[0.00]]
Pitch(theta90.00
Yaw (psi) =0.00
Roll (phi) =0.00
-------------------------------------------------------
Case 3: Rotate VECTOR 45 deg about Z axis:
c_mat =
[[0.71,-0.71,0.00],[0.71,0.71,0.00],[0.00,0.00,1.00]]
body vector:
[[0.71],[0.71],[0.00]]
Euler angles (radians) - RPY order:
[[0.00],[0.00],[0.79]]
Roll(phi) =0.00
Pitch (theta) =0.00
Yaw (psi) =45.00
-------------------------------------------------------
Euler angles (radians) - PYR order:
[[0.00],[0.79],[0.00]]
Pitch(theta) =0.00
Yaw (psi) =0.00
Roll (phi) =45.00
-------------------------------------------------------
Euler angles (radians) - PYR order: -- but using RotationFrame::Rotating
[[0.00],[0.79],[0.00]]
Pitch(theta0.00
Yaw (psi) =45.00
Roll (phi) =0.00
-------------------------------------------------------
|
First of all, huge apologies for having a couple of print statement errors in my attachments from last night. I fixed those since it was clouding the issue regarding the RotationFrame::Rotating item.
Here are the fixed files. Again, sorry for so many emails and upfront confusion.
Bottom line: I understand what you are correctly doing when converting a rotation matrix to Euler angles, regardless of whether the sequence is YPR or PYR. Those all are based on the rotation frame being static and you are rotating the vectors. However, if one is instead rotating the frame, I still expect that the simple rotations I am showing would simply be the negative of what the static frame case would be.
From: ***@***.*** ***@***.***>
Sent: Tuesday, February 22, 2022 22:43 PM
To: 'tomstewart89/Geometry' ***@***.***>; 'tomstewart89/Geometry' ***@***.***>
Cc: 'Mention' ***@***.***>
Subject: RE: [tomstewart89/Geometry] Code error in Geometry? (Issue #13)
I am sorry. I had the wrong attachments. Here are the correct ones….
From: ***@***.*** ***@***.***> ***@***.*** ***@***.***> >
Sent: Tuesday, February 22, 2022 17:29 PM
To: 'tomstewart89/Geometry' ***@***.*** ***@***.***> >; 'tomstewart89/Geometry' ***@***.*** ***@***.***> >
Cc: 'Mention' ***@***.*** ***@***.***> >
Subject: RE: [tomstewart89/Geometry] Code error in Geometry? (Issue #13)
Tom, I think I found another “concern” with your software.
In the enclosed cases (.ino file) you will see the same checkout runs I did to find the issue with pitch(), yaw(), etc which you fixed with first(), second(), etc.
Here, I am using the rotation matrix and calling your EulerAngles class. When calling it with “RotationFrame: Rotating”, I was expecting to get essentially the inverse of the euler angles (since I am only rotating about a single axis). I figured this is where you change from “rotating vectors” to “rotating the coordinate system”. It appears that the sequencing of the output is messed up. I think it might be due to you not carrying an annotation as an instance variable in your class the idea of static vs rotating? I am not really sure of the issue, but am fairly confident that the results are incorrect.
From: tomstewart89 ***@***.*** ***@***.***> >
Sent: Friday, February 18, 2022 21:53 PM
To: tomstewart89/Geometry ***@***.*** ***@***.***> >
Cc: dondonddon ***@***.*** ***@***.***> >; Mention ***@***.*** ***@***.***> >
Subject: Re: [tomstewart89/Geometry] Code error in Geometry? (Issue #13)
Closed #13 <#13> .
—
Reply to this email directly, view it on GitHub <#13 (comment)> , or unsubscribe <https://github.com/notifications/unsubscribe-auth/ARIBG6QDC2CMSP2UCIWHCSTU34V5TANCNFSM5OH5ZUMA> .
Triage notifications on the go with GitHub Mobile for iOS <https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675> or Android <https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub> .
You are receiving this because you were mentioned.Message ID: ***@***.*** ***@***.***> >
don_rotations_checkout
Case #1a: Rotate VECTOR 90 deg about Z axis:
c_mat =
[[0.00,-1.00,0.00],[1.00,0.00,0.00],[0.00,0.00,1.00]]
From global vector [1,0,0], get this body vector:
[[0.00],[1.00],[0.00]]
Euler angles (radians) - RPY order:
[[0.00],[0.00],[1.57]]
Roll(phi) =0.00
Pitch (theta) =0.00
Yaw (psi) =90.00
-------------------------------------------------------
Euler angles (radians) - PYR order:
[[0.00],[1.57],[0.00]]
Pitch(theta) =0.00
Yaw (psi) =90.00
Roll (phi) =0.00
-------------------------------------------------------
Euler angles (radians) - PYR order: -- but using RotationFrame::Rotating
[[0.00],[1.57],[0.00]]
Pitch(theta) = 0.00
Yaw (psi) =90.00
Roll (phi) =0.00
-------------------------------------------------------
Case #1b: Rotate VECTOR 90 deg about - Z axis: (same as rotating frame +90 about +Z)
c_mat =
[[0.00,1.00,0.00],[-1.00,0.00,0.00],[0.00,0.00,1.00]]
From global vector [1,0,0], get this body vector:
[[0.00],[-1.00],[0.00]]
Euler angles (radians) - RPY order:
[[0.00],[0.00],[-1.57]]
Roll(phi) =0.00
Pitch (theta) =0.00
Yaw (psi) =-90.00
-------------------------------------------------------
Euler angles (radians) - PYR order:
[[0.00],[-1.57],[0.00]]
Pitch(theta) =0.00
Yaw (psi) =-90.00
Roll (phi) =0.00
-------------------------------------------------------
Euler angles (radians) - PYR order: -- but using RotationFrame::Rotating
[[0.00],[-1.57],[0.00]]
Pitch(theta) = 0.00
Yaw (psi) =-90.00
Roll (phi) =0.00
-------------------------------------------------------
Case #2: Rotate VECTOR 90 deg about X axis:
c_mat =
[[1.00,0.00,0.00],[0.00,0.00,-1.00],[0.00,1.00,0.00]]
From global vector [1,0,0], get this body vector:
[[1.00],[0.00],[0.00]]
Euler angles (radians) - RPY order:
[[1.57],[0.00],[0.00]]
Roll(phi) =90.00
Pitch (theta) =0.00
Yaw (psi) =0.00
-------------------------------------------------------
Euler angles (radians) - PYR order:
[[0.00],[0.00],[1.57]]
Pitch(theta) =0.00
Yaw (psi) =0.00
Roll (phi) =90.00
-------------------------------------------------------
Euler angles (radians) - PYR order: -- but using RotationFrame::Rotating
[[1.57],[0.00],[0.00]]
Pitch(theta) = 90.00
Yaw (psi) =0.00
Roll (phi) =0.00
-------------------------------------------------------
Case 3: Rotate VECTOR 45 deg about Z axis:
c_mat =
[[0.71,-0.71,0.00],[0.71,0.71,0.00],[0.00,0.00,1.00]]
From global vector [1,0,0], get this body vector:
[[0.71],[0.71],[0.00]]
Euler angles (radians) - RPY order:
[[0.00],[0.00],[0.79]]
Roll(phi) =0.00
Pitch (theta) =0.00
Yaw (psi) =45.00
-------------------------------------------------------
Euler angles (radians) - PYR order:
[[0.00],[0.79],[0.00]]
Pitch(theta) =0.00
Yaw (psi) =45.00
Roll (phi) =0.00
-------------------------------------------------------
Euler angles (radians) - PYR order: -- but using RotationFrame::Rotating
[[0.00],[0.79],[0.00]]
Pitch(theta) = 0.00
Yaw (psi) =45.00
Roll (phi) =0.00
-------------------------------------------------------
|
Is line 53 in Geometry.cpp supposed to say: theta = 1.0e-5 ??
I am trying to match up your code with the algorithm description in: https://www.cl.cam.ac.uk/techreports/UCAM-CL-TR-696.html
Also, you have a class called AngularVelocity, but I think the values are supposed to be gyro rates * integration time ? Otherwise, how do you account for the delta-T in order to integrate the angular rates?
The text was updated successfully, but these errors were encountered: