|
44
|
1 from PyQt4.QtGui import *
|
|
|
2 from PyQt4.QtCore import *
|
|
|
3 from PyQt4.QtOpenGL import QGLWidget
|
|
|
4 from OpenGL.GL import *
|
|
|
5 from OpenGL.GLU import gluPerspective
|
|
|
6 import sys
|
|
|
7 from random import random
|
|
|
8 from math import pi, cos, sin, fabs, sqrt
|
|
|
9 from numpy import mat, array, ones, zeros, eye
|
|
|
10 from numpy.linalg import norm
|
|
|
11 import numpy as np
|
|
|
12 import time
|
|
|
13 import scipy.integrate
|
|
|
14
|
|
|
15 """
|
|
|
16 Test script that lets a dice bounce.
|
|
|
17 Converted from 20-sim equations into python code.
|
|
|
18
|
|
|
19 20-sim website:
|
|
|
20 http://www.20sim.com
|
|
|
21
|
|
|
22 """
|
|
292
|
23
|
|
|
24 # OpenGL drawing madness:
|
|
44
|
25 def drawCube(w):
|
|
292
|
26 glBegin(GL_QUADS) # Start Drawing The Cube
|
|
|
27 glColor3f(0.0,1.0,0.0) # Set The Color To Blue
|
|
|
28 glVertex3f( w, w,-w) # Top Right Of The Quad (Top)
|
|
|
29 glVertex3f(-w, w,-w) # Top Left Of The Quad (Top)
|
|
|
30 glVertex3f(-w, w, w) # Bottom Left Of The Quad (Top)
|
|
|
31 glVertex3f( w, w, w) # Bottom Right Of The Quad (Top)
|
|
44
|
32
|
|
292
|
33 glColor3f(1.0,0.5,0.0) # Set The Color To Orange
|
|
|
34 glVertex3f( w,-w, w) # Top Right Of The Quad (Bottom)
|
|
|
35 glVertex3f(-w,-w, w) # Top Left Of The Quad (Bottom)
|
|
|
36 glVertex3f(-w,-w,-w) # Bottom Left Of The Quad (Bottom)
|
|
|
37 glVertex3f( w,-w,-w) # Bottom Right Of The Quad (Bottom)
|
|
44
|
38
|
|
292
|
39 glColor3f(1.0,0.0,0.0) # Set The Color To Red
|
|
|
40 glVertex3f( w, w, w) # Top Right Of The Quad (Front)
|
|
|
41 glVertex3f(-w, w, w) # Top Left Of The Quad (Front)
|
|
|
42 glVertex3f(-w,-w, w) # Bottom Left Of The Quad (Front)
|
|
|
43 glVertex3f( w,-w, w) # Bottom Right Of The Quad (Front)
|
|
44
|
44
|
|
292
|
45 glColor3f(1.0,1.0,0.0) # Set The Color To Yellow
|
|
|
46 glVertex3f( w,-w,-w) # Bottom Left Of The Quad (Back)
|
|
|
47 glVertex3f(-w,-w,-w) # Bottom Right Of The Quad (Back)
|
|
|
48 glVertex3f(-w, w,-w) # Top Right Of The Quad (Back)
|
|
|
49 glVertex3f( w, w,-w) # Top Left Of The Quad (Back)
|
|
44
|
50
|
|
292
|
51 glColor3f(0.0,0.0,1.0) # Set The Color To Blue
|
|
|
52 glVertex3f(-w, w, w) # Top Right Of The Quad (Left)
|
|
|
53 glVertex3f(-w, w,-w) # Top Left Of The Quad (Left)
|
|
|
54 glVertex3f(-w,-w,-w) # Bottom Left Of The Quad (Left)
|
|
|
55 glVertex3f(-w,-w, w) # Bottom Right Of The Quad (Left)
|
|
44
|
56
|
|
292
|
57 glColor3f(1.0,0.0,1.0) # Set The Color To Violet
|
|
|
58 glVertex3f( w, w,-w) # Top Right Of The Quad (Right)
|
|
|
59 glVertex3f( w, w, w) # Top Left Of The Quad (Right)
|
|
|
60 glVertex3f( w,-w, w) # Bottom Left Of The Quad (Right)
|
|
|
61 glVertex3f( w,-w,-w) # Bottom Right Of The Quad (Right)
|
|
|
62 glEnd() # Done Drawing The Quad
|
|
44
|
63
|
|
|
64 def drawFloor(w, h):
|
|
292
|
65 glBegin(GL_QUADS) # Start Drawing The Cube
|
|
|
66 glColor3f(1.0,0.5,0.0) # Set The Color To Orange
|
|
|
67 glVertex3f( w,-w,h)# Top Right Of The Quad (Bottom)
|
|
|
68 glVertex3f(-w,-w,h)# Top Left Of The Quad (Bottom)
|
|
|
69 glVertex3f(-w,w,h)# Bottom Left Of The Quad (Bottom)
|
|
|
70 glVertex3f( w,w,h)# Bottom Right Of The Quad (Bottom)
|
|
|
71 glEnd() # Done Drawing The Quad
|
|
44
|
72
|
|
|
73 def drawAxis():
|
|
292
|
74 glLineWidth(0.5)
|
|
|
75 glBegin(GL_LINES)
|
|
|
76 glColor3f(1.0, 0.0, 0.0)
|
|
|
77 glVertex3f(0,0,0)
|
|
|
78 glVertex3f(1,0,0)
|
|
|
79 glColor3f(0.0, 1.0, 0.0)
|
|
|
80 glVertex3f(0,0,0)
|
|
|
81 glVertex3f(0,1,0)
|
|
|
82 glColor3f(0.0, 0.0, 1.0)
|
|
|
83 glVertex3f(0,0,0)
|
|
|
84 glVertex3f(0,0,1)
|
|
|
85 glEnd()
|
|
44
|
86
|
|
|
87
|
|
292
|
88 # Math helper functions:
|
|
|
89
|
|
44
|
90 def cross(A, B):
|
|
292
|
91 a = A.A1
|
|
|
92 b = B.A1
|
|
|
93 return mat(np.cross(a, b)).T
|
|
44
|
94
|
|
|
95 def skew(X):
|
|
292
|
96 Y = mat(zeros( (3, 3) ))
|
|
|
97 a, b, c = X.A1
|
|
|
98 Y[0,1] = -c
|
|
|
99 Y[0,2] = b
|
|
|
100 Y[1,0] = c
|
|
|
101 Y[1,2] = -a
|
|
|
102 Y[2,0] = -b
|
|
|
103 Y[2,1] = a
|
|
|
104 return Y
|
|
44
|
105
|
|
|
106 def adjoint(T):
|
|
292
|
107 W = T[0:3, 0]
|
|
|
108 V = T[3:6, 0]
|
|
|
109 a = mat(zeros( (6, 6) ) )
|
|
|
110 a[0:3, 0:3] = skew(W)
|
|
|
111 a[3:6, 0:3] = skew(V)
|
|
|
112 a[3:6, 3:6] = skew(W)
|
|
|
113 return a
|
|
44
|
114
|
|
|
115 def Adjoint(H):
|
|
292
|
116 R = H[0:3, 0:3]
|
|
|
117 P = H[0:3, 3]
|
|
|
118 a = mat(zeros( (6, 6) ) )
|
|
|
119 a[0:3, 0:3] = R
|
|
|
120 a[3:6, 3:6] = R
|
|
|
121 a[3:6, 0:3] = skew(P) * R
|
|
|
122 return a
|
|
44
|
123
|
|
|
124 def quatToR(q):
|
|
292
|
125 x, y, z, w = q.A1
|
|
|
126 r = mat(eye(3))
|
|
|
127 r[0,0] = 1 - (2*y**2+2*z**2)
|
|
|
128 r[0,1] = 2*x*y+2*z*w
|
|
|
129 r[0,2] = 2*x*z - 2*y*w
|
|
|
130 r[1,0] = 2*x*y-2*z*w
|
|
|
131 r[1,1] = 1 - (2*x**2 + 2*z**2)
|
|
|
132 r[1,2] = 2*y*z + 2*x*w
|
|
|
133 r[2,0] = 2*x*z+2*y*w
|
|
|
134 r[2,1] = 2*y*z - 2*x*w
|
|
|
135 r[2,2] = 1 - (2*x**2+2*y**2)
|
|
|
136 return r
|
|
44
|
137
|
|
|
138 def rotateAbout(axis, angle):
|
|
292
|
139 ax, ay, az = (axis/norm(axis)).A1
|
|
|
140 qx = ax*sin(angle/2.0)
|
|
|
141 qy = ay*sin(angle/2.0)
|
|
|
142 qz = az*sin(angle/2.0)
|
|
|
143 qw = cos(angle/2.0)
|
|
|
144 q = mat(array([qx,qy,qz,qw])).T
|
|
|
145 return q
|
|
44
|
146
|
|
|
147 def normalizeQuaternion(quat):
|
|
292
|
148 x,y,z,w = quat.A1
|
|
|
149 magnitude = sqrt(x*x + y*y + z*z + w*w)
|
|
|
150 x = x / magnitude
|
|
|
151 y = y / magnitude
|
|
|
152 z = z / magnitude
|
|
|
153 w = w / magnitude
|
|
|
154 quat[0, 0] = x
|
|
|
155 quat[1, 0] = y
|
|
|
156 quat[2, 0] = z
|
|
|
157 quat[3, 0] = w
|
|
|
158 return quat
|
|
44
|
159
|
|
|
160 def VTo4x4(V):
|
|
|
161 v1, v2, v3 = V.A1
|
|
|
162 return mat(array( \
|
|
|
163 [[0.0, -v3, v2, -v1], \
|
|
|
164 [ v3, 0.0, -v1, -v2], \
|
|
|
165 [-v2, v1, 0.0, -v3], \
|
|
|
166 [v1, v2, v3, 0.0] ]))
|
|
|
167
|
|
292
|
168 def homogeneous(R, p):
|
|
|
169 """ Create a H matrix from rotation and position """
|
|
44
|
170 H = mat(eye(4))
|
|
|
171 H[0:3, 0:3] = R
|
|
|
172 H[0:3, 3] = p
|
|
|
173 return H
|
|
|
174
|
|
|
175
|
|
292
|
176 class Simulation:
|
|
|
177 def simulate(self, endtime, dt):
|
|
|
178 """ Evaluate the state of the system over a given time """
|
|
|
179 state = self.initial()
|
|
|
180 times = np.arange(0.0, endtime, dt)
|
|
|
181 y0 = state.T.A1
|
|
|
182 def fWrapper(y, t):
|
|
|
183 y = mat(y).T
|
|
|
184 dy = self.rateOfChange(y, t)
|
|
|
185 return dy.T.A1
|
|
|
186 res = scipy.integrate.odeint(fWrapper, y0, times)
|
|
|
187 states = []
|
|
|
188 res = res.T
|
|
|
189 r,c = res.shape
|
|
|
190 for ci in range(0,c):
|
|
|
191 states.append(mat(res[:,ci]).T)
|
|
|
192 self.result = states
|
|
|
193 return states
|
|
44
|
194
|
|
|
195
|
|
292
|
196 class BouncingCube(Simulation):
|
|
|
197 """ Simulation of a bouncing cube """
|
|
|
198 def __init__(self):
|
|
|
199 # Parameters:
|
|
|
200 self.gravity = mat( array([0,0,-9.81]) ).T
|
|
|
201 self.massI = mat(eye(6)) * 1.01 # Mass matrix
|
|
|
202
|
|
|
203 def initial(self):
|
|
|
204 """ Return initial pose """
|
|
|
205 PInitial = mat( zeros((6, 1)) )
|
|
|
206 posInitial = mat(array([-1.2, -1.3, 2.8])).T
|
|
|
207 quatInitial = rotateAbout(mat(array([0.2, 1.0, 0.4])).T, 0.5)
|
|
|
208
|
|
|
209 # The state vector!
|
|
|
210 state = mat(zeros( (13,1) ))
|
|
|
211 state[0:4, 0] = quatInitial
|
|
|
212 state[4:7, 0] = posInitial
|
|
|
213 state[7:13, 0] = PInitial
|
|
|
214 return state
|
|
|
215
|
|
|
216 def rateOfChange(self, states, thetime):
|
|
|
217 """ Calculates rate of change given the current states. """
|
|
|
218 quat = states[0:4, 0] # Orientation (4)
|
|
|
219 pos = states[4:7, 0] # Position (3)
|
|
|
220 P = states[7:13, 0] # Momentum (6)
|
|
|
221 massI, gravity = self.massI, self.gravity
|
|
|
222 # Rigid body parts:
|
|
|
223 # Forward Kinematic chain:
|
|
|
224 H = homogeneous(quatToR(quat), pos) # Forward kinematics
|
|
|
225
|
|
|
226 AdjX2 = mat(eye(6)) # The connectionpoint in the real world
|
|
|
227 adjAdjX2_1 = adjoint(AdjX2[0:6,0])
|
|
|
228 adjAdjX2_2 = adjoint(AdjX2[0:6,1])
|
|
|
229 adjAdjX2_3 = adjoint(AdjX2[0:6,2])
|
|
|
230 adjAdjX2_4 = adjoint(AdjX2[0:6,3])
|
|
|
231 adjAdjX2_5 = adjoint(AdjX2[0:6,4])
|
|
|
232 adjAdjX2_6 = adjoint(AdjX2[0:6,5])
|
|
|
233 AdjInv2 = Adjoint(H.I)
|
|
|
234 M2 = AdjInv2.T * (massI * AdjInv2) # Transfor mass to base frame
|
|
|
235 MassMatrix = M2
|
|
|
236
|
|
|
237 wrenchGrav2 = mat( zeros((1,6)) )
|
|
|
238 wrenchGrav2[0, 0:3] = -cross(gravity, pos).T
|
|
|
239 wrenchGrav2[0, 3:6] = gravity.T
|
|
|
240
|
|
|
241 Bk = mat( zeros( (6,6) ))
|
|
|
242 Bk[0:3, 0:3] = skew(P[0:3, 0])
|
|
|
243 Bk[3:6, 0:3] = skew(P[3:6, 0])
|
|
|
244 Bk[0:3, 3:6] = skew(P[3:6, 0])
|
|
|
245
|
|
|
246 # TODO: do this a cholesky:
|
|
|
247 v = np.linalg.solve(MassMatrix, P) # Matrix inverse like thingy !
|
|
|
248
|
|
|
249 T2_00 = v # Calculate the relative twist!
|
|
|
250 TM2 = T2_00.T * M2
|
|
|
251 twistExternal = T2_00 # Twist van het blokje
|
|
|
252 TMSum2 = TM2
|
|
44
|
253
|
|
292
|
254 PDotBodies = mat( zeros( (6,1)) )
|
|
|
255 PDotBodies[0,0] = TMSum2 * (adjAdjX2_1 * T2_00)
|
|
|
256 PDotBodies[1,0] = TMSum2 * (adjAdjX2_2 * T2_00)
|
|
|
257 PDotBodies[2,0] = TMSum2 * (adjAdjX2_3 * T2_00)
|
|
|
258 PDotBodies[3,0] = TMSum2 * (adjAdjX2_4 * T2_00)
|
|
|
259 PDotBodies[4,0] = TMSum2 * (adjAdjX2_5 * T2_00)
|
|
|
260 PDotBodies[5,0] = TMSum2 * (adjAdjX2_6 * T2_00)
|
|
|
261
|
|
|
262 PDot = -PDotBodies - Bk * v
|
|
|
263 PDot += wrenchGrav2.T
|
|
|
264
|
|
|
265 ##### Contact wrench part:
|
|
|
266 HB_W = H # Is H-matrix van het blokje
|
|
|
267 WrenchB = mat(zeros( (1,6) ))
|
|
|
268 for px in [-0.5, 0.5]:
|
|
|
269 for py in [-0.5, 0.5]:
|
|
|
270 for pz in [-0.5, 0.5]:
|
|
|
271 HB1_B = homogeneous(mat(eye(3)), mat([px,py,pz]).T)
|
|
|
272 HB1_W = HB_W * HB1_B
|
|
|
273 HW1_W = homogeneous(mat(eye(3)), HB1_W[0:3,3])
|
|
|
274 HW_W1 = HW1_W.I
|
|
|
275 HB_W1 = HW_W1 * HB_W
|
|
|
276
|
|
|
277 AdjHB_W1 = Adjoint(HB_W1)
|
|
|
278 TB_W1_W1 = AdjHB_W1 * twistExternal
|
|
|
279 z = HB1_W[2, 3]
|
|
|
280 vx, vy, vz = TB_W1_W1[3:6, 0].A1
|
|
|
281 if True:
|
|
|
282 # Contact forces:
|
|
|
283 Fx = 0 #np.exp(-5.0*vx)
|
|
|
284 Fy = 0 # np.exp(-5.0*vy)
|
|
|
285 Fz = np.exp(-z*10.0)
|
|
|
286 else:
|
|
|
287 Fx = 0.0
|
|
|
288 Fy = 0.0
|
|
|
289 Fz = 0.0
|
|
|
290 # TODO: reflect impulse
|
|
|
291 WrenchW1 = mat([0,0,0,0,0,Fz])
|
|
|
292 # Transform it back:
|
|
|
293 WrenchB += (AdjHB_W1.T * WrenchW1.T).T
|
|
|
294 ##### End of contact wrench
|
|
|
295
|
|
|
296 PDot += (WrenchB * AdjInv2).T
|
|
|
297
|
|
|
298 # Position and orientation rates:
|
|
|
299 QOmega = VTo4x4(v[0:3, 0])
|
|
|
300 quatDot = 0.5 * QOmega * quat
|
|
|
301 vel = v[3:6, 0]
|
|
|
302 posDot = skew(v[0:3]) * pos + vel
|
|
|
303 # The rate vector:
|
|
|
304 rates = mat(zeros( (13,1) ))
|
|
|
305 rates[0:4, 0] = quatDot
|
|
|
306 rates[4:7, 0] = posDot
|
|
|
307 rates[7:13, 0] = PDot
|
|
|
308 return rates
|
|
44
|
309
|
|
292
|
310 def draw(self, state):
|
|
|
311 Dicequat = state[0:4, 0]
|
|
|
312 Dicepos = state[4:7, 0]
|
|
|
313
|
|
|
314 x, y, z = Dicepos.A1
|
|
|
315 R = quatToR(Dicequat)
|
|
|
316
|
|
|
317 glTranslatef(x, y, z)
|
|
|
318 # Trick to rotate the openGL matrix:
|
|
|
319 r = R.A1
|
|
|
320 rotR = (r[0], r[3], r[6], 0.0,
|
|
|
321 r[1], r[4], r[7], 0.0,
|
|
|
322 r[2], r[5], r[8], 0.0,
|
|
|
323 0.0, 0.0, 0.0, 1.0)
|
|
|
324 glMultMatrixd(rotR)
|
|
|
325
|
|
|
326 drawCube(0.6)
|
|
|
327
|
|
|
328
|
|
|
329 class RollingPendulum(Simulation):
|
|
|
330 """ A 2D pendulum with a rolling contact. """
|
|
|
331 def __init__(self):
|
|
|
332 self.R = 0.01
|
|
|
333 self.L = 1
|
|
|
334 self.M = 1
|
|
|
335 self.g = 9.81
|
|
|
336
|
|
|
337 def initial(self):
|
|
|
338 i = mat(zeros((2,1)))
|
|
|
339 i[0, 0] =
|
|
|
340 return i
|
|
|
341
|
|
|
342 def rateOfChange(self, states, thetime):
|
|
|
343 phi, omega = states[0:2, 0]
|
|
|
344 rates = mat(zeros((2,1)))
|
|
|
345 return rates
|
|
|
346
|
|
|
347 def draw(self, state):
|
|
|
348 pass
|
|
|
349 #drawCube(0.6)
|
|
|
350
|
|
|
351
|
|
|
352 class WobbelingWheel(Simulation):
|
|
|
353 """ A wheel pushed sideways over a surface """
|
|
|
354 def __init__(self):
|
|
|
355 self.r1 = 0.5
|
|
|
356 self.r2 = 0.05
|
|
|
357
|
|
|
358 def initial(self):
|
|
|
359 i = mat(zeros((5,1)))
|
|
|
360 return i
|
|
|
361
|
|
|
362 def rateOfChange(self, states, thetime):
|
|
|
363 x, y = states[0:2, 0]
|
|
|
364 rates = mat(zeros((5,1)))
|
|
|
365 return rates
|
|
|
366
|
|
|
367 def draw(self, state):
|
|
|
368 pass
|
|
|
369 #drawCube(0.6)
|
|
|
370
|
|
44
|
371
|
|
|
372 class W(QGLWidget):
|
|
292
|
373 time = 0.0
|
|
|
374 index = 0
|
|
|
375 def __init__(self, sim):
|
|
|
376 super().__init__()
|
|
|
377 self.firstRun = True
|
|
|
378 self.savePNGS = False
|
|
|
379 self.index = 0
|
|
|
380 self.sim = sim
|
|
|
381 self.resize(500,500)
|
|
|
382 self.UP()
|
|
|
383 t = QTimer(self)
|
|
|
384 t.timeout.connect(self.UP)
|
|
|
385 t.start(40)
|
|
44
|
386
|
|
292
|
387 def UP(self):
|
|
|
388 if self.sim.result:
|
|
|
389 if self.index + 1 < len(self.sim.result):
|
|
|
390 self.index += 1
|
|
|
391 else:
|
|
|
392 self.index = 0
|
|
|
393 # Paint:
|
|
|
394 self.update()
|
|
|
395
|
|
|
396 if self.firstRun:
|
|
|
397 # Create png images for the movie:
|
|
|
398 if self.savePNGS:
|
|
|
399 pm = self.renderPixmap()
|
|
|
400 pm.save('image'+str(self.index)+'.png')
|
|
44
|
401
|
|
292
|
402 def initializeGL(self):
|
|
44
|
403 glClearColor(0.0, 0.5, 0.0, 1.0)
|
|
|
404 glEnable(GL_DEPTH_TEST)
|
|
|
405 glDepthFunc(GL_LESS)
|
|
|
406 glShadeModel(GL_SMOOTH)
|
|
292
|
407
|
|
|
408 def resizeGL(self,w,h):
|
|
44
|
409 glViewport(0, 0, w, h)
|
|
|
410 glMatrixMode(GL_PROJECTION)
|
|
|
411 glLoadIdentity()
|
|
|
412 gluPerspective(45.0, float(w)/float(h), 0.1, 100.0)
|
|
|
413 glMatrixMode(GL_MODELVIEW)
|
|
292
|
414
|
|
|
415 def paintGL(self):
|
|
44
|
416 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) # Clear buffers
|
|
|
417 glLoadIdentity() # Reset The View
|
|
|
418 glTranslatef(0.0,-2.0,-10.0) # Move Left And Into The Screen
|
|
|
419 glRotatef(-90.0, 1.0, 0.0, 0.0)
|
|
|
420 drawFloor(2.0, 0.0)
|
|
|
421 drawAxis()
|
|
|
422 self.renderText(1.0, 0.0, 0.0, 'X')
|
|
|
423 self.renderText(0.0, 1.0, 0.0, 'Y')
|
|
|
424 self.renderText(0.0, 0.0, 1.0, 'Z')
|
|
|
425 self.renderText(0.0,0.0,1.2,str(self.time))
|
|
292
|
426 if self.sim.result:
|
|
|
427 self.sim.draw(self.sim.result[self.index])
|
|
44
|
428
|
|
|
429
|
|
292
|
430 et = 1.5
|
|
44
|
431 dt = 0.04
|
|
292
|
432 # sim = BouncingCube()
|
|
|
433 sim = WobbelingWheel()
|
|
243
|
434 print('starting integration... endtime =', et, ' stepsize =', dt)
|
|
44
|
435 t0 = time.time()
|
|
292
|
436 states = sim.simulate(et, dt)
|
|
44
|
437 t1 = time.time()
|
|
292
|
438 print('That was heavy, it took me ', t1 -t0, ' seconds!')
|
|
44
|
439 app = QApplication(sys.argv)
|
|
292
|
440 w = W(sim)
|
|
44
|
441 w.show()
|
|
|
442 sys.exit(app.exec_())
|
|
|
443
|
