Gimbal Lock conversion to quarternions

Need help, or want to share a macro? Post here!
Forum rules
Be nice to others! Respect the FreeCAD code of conduct!
Post Reply
doxley
Posts: 35
Joined: Tue Jun 18, 2019 6:55 pm

Gimbal Lock conversion to quarternions

Post by doxley »

I have a problem in which I am directly manipulating the rotation matrix and running into a gimbal lock issue. I'm trying to switch to directly using FreeCAD rotations to avoid the issue. I'm trying to mirror with rotations what I'm doing with the matrix manipulation, but I'm getting different results.

The Gimbal Lock attachment shows the basic problem and the occurrence of the gimbal lock. The Rotations attachment shows the "same" view when using rotations.

The code:

Code: Select all

def lift_lcs(lcs, lift_angle, rotation_angle, helix_radius, outside_height):
    """Adjust lcs by amount determined by a wafer's parameters."""
    # print(f"LIFT ANGLE: {lift_angle}, {helix_radius}, {outside_height}")
    if lift_angle == 0:
        lcs.Placement.Base = lcs.Placement.Base + FreeCAD.Vector(0, 0, outside_height)
        return

    translate_vector = FreeCAD.Vector(-helix_radius, 0, 0)
    lcs.Placement.Base = lcs.Placement.Base + translate_vector

    # Rotation based solution
    ra = np.rad2deg(rotation_angle)
    la = np.rad2deg(lift_angle)
    rot = FreeCAD.Placement(lcs.Placement.Base, FreeCAD.Rotation(FreeCAD.Vector(0, 1, 0), la))
    rot2 = FreeCAD.Placement(lcs.Placement.Base, FreeCAD.Rotation(FreeCAD.Vector(0, 0, 1), -ra))
    rot_place = rot2.multiply(rot).multiply(lcs.Placement)
    
    # Matrix manipulation solution
    pm = lcs.Placement.toMatrix()
    pm.rotateY(lift_angle)
    pm.rotateZ(-rotation_angle)
    pm_place = FreeCAD.Placement(lcs.Placement.Base, FreeCAD.Rotation(pm))

    # Determine result and select solution
    lcs.Placement = FreeCAD.Placement(pm)
    lcs.Placement = lcs.Placement.multiply(rot_place)
    print(f"TV: {lcs.Label},  {lcs.Placement}, {translate_vector}")
    lcs.Placement.Base = lcs.Placement.Base - translate_vector
    print(f"TV2: {lcs.Label}, {lcs.Placement}, {translate_vector}")

    # Verify that the produced matrices match (this passes).
    if not match_matrix(pm_place.toMatrix(), rot_place.toMatrix()):
        print(f"LL: Lift: {np.round(la, 3)}, {np.round(ra, 3)}\nAs Rotation")
        print_matrix(rot_place.toMatrix())
        print("As Matrix")
        print_matrix(pm)
        foo = 3/0   # Generate an error to stop run
        
def run_test():
    lcs2 = FreeCAD.activeDocument().addObject('PartDesign::CoordinateSystem', "testlcs")
    lcs2.Placement = FreeCAD.Placement(FreeCAD.Vector(2,3,3), FreeCAD.Rotation(0, 25, 30))
    lift = np.deg2rad(6)
    rotate = np.deg2rad(10)
    radius = 8 * 25.4
    outside = 0.75 * 25.4
    lift_lcs(lcs2, lift, rotate, radius, outside)
shows both implementations. The result is selected by commenting one solution or the other.

The basic structure shown in the images is a set of adjacent lofts where each loft is derived from two ellipses and is a slice (wafer) out of a cylinder. The the planes of the ellipses are separated by an angle (lift angle - nominally 0-8 degrees) and offset by a rotation about the center of one by an angle (rotation angle - nominally 0-+/-20 degrees). The matrix manipulation image is correct up to the offset cause by the gimbal lock. The translation vector (helix_radius - nominal 0-5 times the diameter of the loft cylinder) is determined (outside this code) as a function of the lift angle and the outside height (longest vertical edge of the loft). There is a local coordinate system attached at the center of each ellipse with consistent placement.

I suspect my issue is tangled in the possibly incorrect use of the lcs.Placement.Base in creating the rotations though I'm at a loss for what I'm doing wrong. I'm also confused as both solution produce the same result rotation matrices yet the images are different. Below is the code for the matrix check to verify that i'm not mangling something there.

Any help appreciated.
Thanks,
--Don

Code: Select all

OS: Ubuntu 22.04.1 LTS (ubuntu:GNOME/ubuntu)
Word size of FreeCAD: 64-bit
Version: 0.21.
Build type: Release
Branch: unknown
Hash: 1faf86c3da12c1712e1d5ec015721d9aa02f8672
Python 3.10.4, Qt 5.15.3, Coin 4.0.0, Vtk 9.1.0, OCC 7.5.1
Locale: English/United States (en_US)
Installed mods: 
  * Curves 0.5.2
  * Assembly4 0.12.3
  * Manipulator 1.4.9
  * fasteners 0.3.50

Code: Select all

def print_matrix(matrix):
    for row_num in range(4):
        try:
            row = matrix.row(row_num)
            items = [x for x in row]
            mod_items = "\t"
            for item in items:
                if item < 0.000001:
                    item = 0
                item = str(np.round(item, 3))
                mod_items += f"{item : >8}"
            print(f"\t{mod_items}")
        except:
            pass

def match_matrix(m1, m2):
    res = True
    for row_num in range(4):
        m1_row = m1.row(row_num)
        m2_row = m2.row(row_num)
        for col_num in range(len(m1_row)):
            try:
                if abs(m1_row[col_num] - m2_row[col_num]) > 0.01:
                    res = False
                    print(f"MISSED: {row_num}, {col_num}, {m1_row[col_num]}, {m2_row[col_num]}")
            except Exception as e:
                res = False
                print(f"FAILED WITH: {e.args}")
    return res
Attachments
Rotations.png
Rotations.png (11.68 KiB) Viewed 365 times
Gimbal Lock.png
Gimbal Lock.png (15.54 KiB) Viewed 365 times
edwilliams16
Veteran
Posts: 3112
Joined: Thu Sep 24, 2020 10:31 pm
Location: Hawaii
Contact:

Re: Gimbal Lock conversion to quarternions

Post by edwilliams16 »

If I could figure out what you were trying to achieve, I could help. But all I have is the code, which makes no sense to me

Code: Select all

    ra = np.rad2deg(rotation_angle)
    la = np.rad2deg(lift_angle)
    rot = FreeCAD.Placement(lcs.Placement.Base, FreeCAD.Rotation(FreeCAD.Vector(0, 1, 0), la))
    rot2 = FreeCAD.Placement(lcs.Placement.Base, FreeCAD.Rotation(FreeCAD.Vector(0, 0, 1), -ra))
    rot_place = rot2.multiply(rot).multiply(lcs.Placement)
rot, rot2, rot_place and lcs.Placement() are all Placements, not Rotations. (rot.Rotation would be the rotation part) As such in the last line you have applied the translation portion lcs.Placement.Base three times, in between the rotations. I can't visualize what that does - but I'm sure it wasn't your intent. The extra translations won't affect the overall rotation, but who knows where it ends up.

How did you produce your gimbal lock figure? I'm assuming it met your intent, just failing at the top?

EDIT:

So maybe what you need is

Code: Select all

    ra = np.rad2deg(rotation_angle)
    la = np.rad2deg(lift_angle)
    rot = FreeCAD.Rotation(FreeCAD.Vector(0, 1, 0), la)
    rot2 = FreeCAD.Rotation(FreeCAD.Vector(0, 0, 1), -ra)
    rot_place = rot2.multiply(rot).multiply(lcs.Placement.Rotation)
Post Reply