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)
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