forked from hamish2014/FreeCAD_assembly2
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathdegreesOfFreedom.py
235 lines (213 loc) · 9.98 KB
/
degreesOfFreedom.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
if __name__ == '__main__': #testomg.
import sys
sys.path.append('/usr/lib/freecad/lib/') #path to FreeCAD library on Linux
import FreeCADGui
assert not hasattr(FreeCADGui, 'addCommand')
FreeCADGui.addCommand = lambda x,y: 0
from lib3D import *
import numpy
maxStep_linearDisplacement = 10.0
class PlacementDegreeOfFreedom:
def __init__(self, parentSystem, objName, object_dof):
self.system = parentSystem
self.objName = objName
self.object_dof = object_dof
self.vM = parentSystem.variableManager
self.ind = parentSystem.variableManager.index[objName] + object_dof
if self.ind % 6 < 3:
self.directionVector = numpy.zeros(3)
self.directionVector[ self.ind % 6 ] = 1
def getValue( self):
return self.vM.X[self.ind]
def setValue( self, value):
self.vM.X[self.ind] = value
def maxStep(self):
if self.ind % 6 < 3:
return maxStep_linearDisplacement
else:
return pi/5
def rotational(self):
return self.ind % 6 > 2
def migrate_to_new_variableManager( self, new_vM):
self.vM = new_vM
self.ind = new_vM.index[self.objName] + self.object_dof
def str(self, indent=''):
return '%s<Placement DegreeOfFreedom %s-%s value:%f>' % (indent, self.objName, ['x','y','z','azimuth','elavation','rotation'][self.ind % 6], self.getValue())
def __repr__(self):
return self.str()
class LinearMotionDegreeOfFreedom:
def __init__(self, parentSystem, objName):
self.system = parentSystem
self.objName = objName
self.vM = parentSystem.variableManager
self.objInd = parentSystem.variableManager.index[objName]
def setDirection(self, directionVector):
self.directionVector = directionVector
def getValue( self ):
i = self.objInd
return dotProduct( self.directionVector, self.vM.X[i:i+3])
def setValue( self, value):
currentValue = self.getValue()
correction = (value -currentValue)*self.directionVector
i = self.objInd
self.vM.X[i:i+3] = self.vM.X[i:i+3] + correction
def maxStep(self):
return maxStep_linearDisplacement #inf
def rotational(self):
return False
def migrate_to_new_variableManager( self, new_vM):
self.vM = new_vM
self.objInd = new_vM.index[self.objName]
def str(self, indent=''):
return '%s<LinearMotion DegreeOfFreedom %s direction:%s value:%f>' % (indent, self.objName, self.directionVector, self.getValue())
def __repr__(self):
return self.str()
def prettyPrintArray( A, indent=' ', fmt='%1.1e' ):
def pad(t):
return t if t[0] == '-' else ' ' + t
for r in A:
txt = ' '.join( pad(fmt % v) for v in r)
print(indent + '[ %s ]' % txt)
class AxisRotationDegreeOfFreedom:
'''
calculate the rotation variables ( azi, ela, angle )so that
R_effective = R_about_axis * R_to_align_axis
where
R = azimuth_elevation_rotation_matrix(azi, ela, theta )
'''
def __init__(self, parentSystem, objName):
self.system = parentSystem
self.vM = parentSystem.variableManager
self.objName = objName
self.objInd = self.vM.index[objName]
def setAxis(self, axis, axis_r, check_R_to_align_axis=False):
if not ( hasattr(self, 'axis') and numpy.array_equal( self.axis, axis )): #if to avoid unnessary updates.
self.axis = axis
axis2, angle2 = rotation_required_to_rotate_a_vector_to_be_aligned_to_another_vector( axis_r, axis )
self.R_to_align_axis = axis_rotation_matrix( angle2, *axis2 )
if check_R_to_align_axis:
print('NOTE: checking AxisRotationDegreeOfFreedom self.R_to_align_axis')
if norm( dotProduct(self.R_to_align_axis, axis_r) - axis ) > 10**-12:
raise ValueError(" dotProduct(self.R_to_align_axis, axis_r) - axis ) [%e] > 10**-12" % norm( dotProduct(self.R_to_align_axis, axis_r) - axis ))
if not hasattr(self, 'x_ref_r'):
self.x_ref_r, self.y_ref_r = plane_degrees_of_freedom( axis_r )
else: #use gram_schmidt_orthonormalization ; import for case where axis close to z-axis, where numerical noise effects the azimuth angle used to generate plane DOF...
notUsed, self.x_ref_r, self.y_ref_r = gram_schmidt_orthonormalization( axis_r, self.x_ref_r, self.y_ref_r) #still getting wonky rotations :(
self.x_ref = dotProduct(self.R_to_align_axis, self.x_ref_r)
self.y_ref = dotProduct(self.R_to_align_axis, self.y_ref_r)
def determine_R_about_axis(self, R_effective, checkAnswer=True, tol=10**-12): #not used anymore
'determine R_about_axis so that R_effective = R_about_axis * R_to_align_axis'
A = self.R_to_align_axis.transpose()
X = numpy.array([
numpy.linalg.solve(A, R_effective[row,:]) for row in range(3)
])
#prettyPrintArray(X)
if checkAnswer:
print(' determine_R_about_axis: diff between R_effective and R_about_axis * R_to_align_axis (should be all close to zero):')
error = R_effective - dotProduct(X, self.R_to_align_axis)
assert norm(error) <= tol
return X
def vectorsAngleInDofsCoordinateSystem(self,v):
return numpy.arctan2(
dotProduct(self.y_ref, v),
dotProduct(self.x_ref, v),
)
def getValue( self, refApproach=True, tol=10**-7 ):
i = self.objInd
R_effective = azimuth_elevation_rotation_matrix( *self.vM.X[i+3:i+6] )
if refApproach:
v = dotProduct( R_effective, self.x_ref_r)
if tol != None and abs( dotProduct(v, self.axis) ) > tol:
raise ValueError("abs( dotProduct(v, self.axis) ) > %e [error %e]" % (tol, abs( dotProduct(v, self.axis) )))
angle = self.vectorsAngleInDofsCoordinateSystem(v)
else:
raise NotImplementedError("does not work yet")
R_effective = azimuth_elevation_rotation_matrix( *self.vM.X[i+3:i+6] )
R_about_axis = self.determine_R_about_axis(R_effective)
axis, angle = rotation_matrix_axis_and_angle( R_about_axis )
print( axis )
print( self.axis )
# does not work because axis(R_about_axis) != self.axis #which is damm weird if you ask me
return angle
def setValue( self, angle):
R_about_axis = axis_rotation_matrix( angle, *self.axis )
R = dotProduct(R_about_axis, self.R_to_align_axis)
axis, angle = rotation_matrix_axis_and_angle( R )
#todo, change to quaternions
#Q2 = quaternion2( self.value, *self.axis )
#q0,q1,q2,q3 = quaternion_multiply( Q2, self.Q1 )
#axis, angle = quaternion_to_axis_and_angle( q1, q2, q3, q0 )
azi, ela = axis_to_azimuth_and_elevation_angles(*axis)
i = self.objInd
self.vM.X[i+3:i+6] = azi, ela, angle
def maxStep(self):
return pi/5
def rotational(self):
return True
def migrate_to_new_variableManager( self, new_vM):
self.vM = new_vM
self.objInd = new_vM.index[self.objName]
def str(self, indent=''):
return '%s<AxisRotation DegreeOfFreedom %s axis:%s value:%f>' % (indent, self.objName, self.axis, self.getValue())
def __repr__(self):
return self.str()
if __name__ == '__main__':
from numpy.random import rand
import sys
from variableManager import VariableManager
print('Testing degrees-of-freedom library')
print('creating test FreeCAD document, constraining a single Cube')
import FreeCAD, Part
FreeCAD.newDocument("testDoc")
#FreeCAD.setActiveDocument("box")
#FreeCAD.ActiveDocument = FreeCAD.getDocument("box")
objName = "box"
box = FreeCAD.ActiveDocument.addObject("Part::FeaturePython", objName)
box.Shape = Part.makeBox(2,3,2)
#FreeCAD.ActiveDocument.recompute()
box.Placement.Base.x = rand()
box.Placement.Base.y = rand() + 1
box.Placement.Base.z = rand() + 2
print(box.Placement)
class FakeSystem:
def __init__(self, variableManager):
self.variableManager = variableManager
vM = VariableManager(FreeCAD.ActiveDocument)
print(vM.X)
constaintSystem = FakeSystem(vM)
print('\nTesting PlacementDegreeOfFreedom')
for object_dof in range(6):
d = PlacementDegreeOfFreedom( constaintSystem, objName, object_dof )
print(d)
for i in range(6):
value = pi*( rand() - 0.5 )
d.setValue(value)
assert d.getValue() == value
print('\nTesting LinearMotionDegreeOfFreedom')
tol = 10**-14
for i in range(3):
d = LinearMotionDegreeOfFreedom( constaintSystem, objName )
d.setDirection( normalize(rand(3) - 0.5) )
print(d)
for i in range(12):
value = 12*( rand() - 0.5 )
d.setValue(value)
returnedValue = d.getValue()
if abs(returnedValue - value) > tol :
raise ValueError("d.getValue() - value != %1.0e, [diff %e]" % (tol, returnedValue - value))
print('\nTesting AxisRotationDegreeOfFreedom')
tol = 10**-14
for i in range(3):
d = AxisRotationDegreeOfFreedom( constaintSystem, objName )
axis_r = normalize(rand(3) - 0.5) #axis in parts co-ordinate system (i.e. relative to part)
axis = normalize(rand(3) - 0.5) # desired axis in global co-ordinate system
d.setAxis( axis, axis_r )
d.setValue(0) #update azi,ela,theta to statify aligment of axis vector
print(d)
for i in range(6):
value = 2*pi*( rand() - 0.5 )
d.setValue(value)
returnedValue = d.getValue()
print(' d.getValue() %f value %f, diff %e' % (returnedValue, value, returnedValue - value))
if abs(returnedValue - value) > tol :
raise ValueError("d.getValue() - value != %1.0e, [diff %e]" % (tol, returnedValue - value))