Skip to content

Latest commit

 

History

History
626 lines (456 loc) · 18.7 KB

visualization.rst

File metadata and controls

626 lines (456 loc) · 18.7 KB

Three Dimensional Visualization

Note

You can download this example as a Python script: :jupyter-download:script:`visualization` or Jupyter Notebook: :jupyter-download📓`visualization`.

.. jupyter-execute::

   from scipy.integrate import solve_ivp
   import numpy as np
   import sympy as sm
   import sympy.physics.mechanics as me

In this chapter, I will give a basic introduction to creating three dimensional graphics to visualize the motion of your multibody system. There are many software tools for generating interactive three dimensional graphics from classic lower level tools like OpenGL to graphical user interfaces for drawing and animating 3D models like Blender [*]. We will use pythreejs which is a Python wrapper to the three.js Javascript library that is built on WebGL which is a low level graphics library similar to OpenGL but made to execute through your web browser. Check out the demos on three.js's website to get an idea of how powerful the tool is for 3D visualizations in the web browser.

I will again use the example in :numref:`fig-eom-double-rod-pendulum`. Here is the figure for that system:

The following dropdown has all of the code to construct the model and simulate it with the time values ts and the state trajectories xs as the final output.

Modeling and Simulation Code

.. jupyter-execute::

   m, g, kt, kl, l = sm.symbols('m, g, k_t, k_l, l')
   q1, q2, q3 = me.dynamicsymbols('q1, q2, q3')
   u1, u2, u3 = me.dynamicsymbols('u1, u2, u3')

   N = me.ReferenceFrame('N')
   A = me.ReferenceFrame('A')
   B = me.ReferenceFrame('B')

   A.orient_axis(N, q1, N.z)
   B.orient_axis(A, q2, A.x)

   A.set_ang_vel(N, u1*N.z)
   B.set_ang_vel(A, u2*A.x)

   O = me.Point('O')
   Ao = me.Point('A_O')
   Bo = me.Point('B_O')
   Q = me.Point('Q')

   Ao.set_pos(O, l/2*A.x)
   Bo.set_pos(O, l*A.x)
   Q.set_pos(Bo, q3*B.y)

   O.set_vel(N, 0)
   Ao.v2pt_theory(O, N, A)
   Bo.v2pt_theory(O, N, A)
   Q.set_vel(B, u3*B.y)
   Q.v1pt_theory(Bo, N, B)

   t = me.dynamicsymbols._t

   qdot_repl = {q1.diff(t): u1,
                q2.diff(t): u2,
                q3.diff(t): u3}

   Q.set_acc(N, Q.acc(N).xreplace(qdot_repl))

   R_Ao = m*g*N.x
   R_Bo = m*g*N.x + kl*q3*B.y
   R_Q = m/4*g*N.x - kl*q3*B.y
   T_A = -kt*q1*N.z + kt*q2*A.x
   T_B = -kt*q2*A.x

   I = m*l**2/12
   I_A_Ao = I*me.outer(A.y, A.y) + I*me.outer(A.z, A.z)
   I_B_Bo = I*me.outer(B.x, B.x) + I*me.outer(B.z, B.z)

   points = [Ao, Bo, Q]
   forces = [R_Ao, R_Bo, R_Q]
   masses = [m, m, m/4]

   frames = [A, B]
   torques = [T_A, T_B]
   inertias = [I_A_Ao, I_B_Bo]

   Fr_bar = []
   Frs_bar = []

   for ur in [u1, u2, u3]:

       Fr = 0
       Frs = 0

       for Pi, Ri, mi in zip(points, forces, masses):
           vr = Pi.vel(N).diff(ur, N)
           Fr += vr.dot(Ri)
           Rs = -mi*Pi.acc(N)
           Frs += vr.dot(Rs)

       for Bi, Ti, Ii in zip(frames, torques, inertias):
           wr = Bi.ang_vel_in(N).diff(ur, N)
           Fr += wr.dot(Ti)
           Ts = -(Bi.ang_acc_in(N).dot(Ii) +
                  me.cross(Bi.ang_vel_in(N), Ii).dot(Bi.ang_vel_in(N)))
           Frs += wr.dot(Ts)

       Fr_bar.append(Fr)
       Frs_bar.append(Frs)

   Fr = sm.Matrix(Fr_bar)
   Frs = sm.Matrix(Frs_bar)

   q = sm.Matrix([q1, q2, q3])
   u = sm.Matrix([u1, u2, u3])
   p = sm.Matrix([g, kl, kt, l, m])

   qd = q.diff(t)
   ud = u.diff(t)

   ud_zerod = {udr: 0 for udr in ud}

   Mk = -sm.eye(3)
   gk = u

   Md = Frs.jacobian(ud)
   gd = Frs.xreplace(ud_zerod) + Fr

   eval_eom = sm.lambdify((q, u, p), [Mk, gk, Md, gd])

   def eval_rhs(t, x, p):
       """Return the right hand side of the explicit ordinary differential
       equations which evaluates the time derivative of the state ``x`` at time
       ``t``.

       Parameters
       ==========
       t : float
          Time in seconds.
       x : array_like, shape(6,)
          State at time t: [q1, q2, q3, u1, u2, u3]
       p : array_like, shape(5,)
          Constant parameters: [g, kl, kt, l, m]

       Returns
       =======
       xd : ndarray, shape(6,)
           Derivative of the state with respect to time at time ``t``.

       """

       # unpack the q and u vectors from x
       q = x[:3]
       u = x[3:]

       # evaluate the equations of motion matrices with the values of q, u, p
       Mk, gk, Md, gd = eval_eom(q, u, p)

       # solve for q' and u'
       qd = np.linalg.solve(-Mk, np.squeeze(gk))
       ud = np.linalg.solve(-Md, np.squeeze(gd))

       # pack dq/dt and du/dt into a new state time derivative vector dx/dt
       xd = np.empty_like(x)
       xd[:3] = qd
       xd[3:] = ud

       return xd

   q_vals = np.array([
       np.deg2rad(25.0),  # q1, rad
       np.deg2rad(5.0),  # q2, rad
       0.1,  # q3, m
   ])

   u_vals = np.array([
       0.1,  # u1, rad/s
       2.2,  # u2, rad/s
       0.3,  # u3, m/s
   ])

   p_vals = np.array([
       9.81,  # g, m/s**2
       3.0,  # kl, N/m
       0.01,  # kt, Nm/rad
       0.6,  # l, m
       1.0,  # m, kg
   ])

   x0 = np.empty(6)
   x0[:3] = q_vals
   x0[3:] = u_vals

   fps = 50
   t0, tf = 0.0, 10.0
   ts = np.linspace(t0, tf, num=int(fps*(tf - t0)))
   result = solve_ivp(eval_rhs, (t0, tf), x0, args=(p_vals,), t_eval=ts)
   xs = result.y.T
.. jupyter-execute::

   ts.shape, xs.shape

[*]Blender was birthed in the Netherlands!

pythreejs

pythreejs allows you to use three.js via Python. The functions and objects that pythreejs makes available are found in its documentation, but since these have a 1:1 mapping to the three.js code, you'll also find more comprehensive information in the ThreeJS documentation. We will import pythreejs like so:

.. jupyter-execute::

   import pythreejs as p3js

pythreejs has many primitive geometric shapes, for example :external:py:class:`~py3js.geometries.CylinderGeometry` can be used to create cylinders and cones:

.. jupyter-execute::

   cyl_geom = p3js.CylinderGeometry(radiusTop=2.0, radiusBottom=10.0, height=50.0)
   cyl_geom

The image above is interactive; you can use your mouse or trackpad to click, hold, and move the object.

If you want to apply a material to the surface of the geometry you create a :external:py:class:`~py3js.Mesh` which associates a :external:py:class:`~py3js.Material` with the geometry. For example, you can color the above cylinder like so:

.. jupyter-execute::

   red_material = p3js.MeshStandardMaterial(color='red')

   cyl_mesh = p3js.Mesh(geometry=cyl_geom, material=red_material)

   cyl_mesh

Creating a Scene

Here I create a new orange cylinder that is displaced from the origin of the scene and that has its own coordinate axes. :external:py:class:`~py3js.AxesHelper()` creates simple X (red), Y (green), and Z (blue) affixed to the mesh. :external:py:attr:`~py3js.Mesh.position` is overridden to set the position.

.. jupyter-execute::

   cyl_geom = p3js.CylinderGeometry(radiusTop=0.1, radiusBottom=0.5, height=2.0)
   cyl_material = p3js.MeshStandardMaterial(color='orange', wireframe=True)
   cyl_mesh = p3js.Mesh(geometry=cyl_geom, material=cyl_material)
   axes = p3js.AxesHelper()
   cyl_mesh.add(axes)
   cyl_mesh.position = (3.0, 3.0, 3.0)

Now we will create a :external:py:class:`~py3js.Scene` which can contain multiple meshes and other objects like lights, cameras, and axes. There is a fair amount of boiler plate code for creating the static scene. All of the objects should be added to the children= keyword argument of Scene. The last line creates a :external:py:class:`~py3js.Renderer` that links the camera view to the scene and enables :external:py:class:`~py3js.OrbitControls` to allow zooming, panning, and rotating with a mouse or trackpad.

.. jupyter-execute::

   view_width = 600
   view_height = 400

   camera = p3js.PerspectiveCamera(position=[10.0, 10.0, 10.0],
                                   aspect=view_width/view_height)
   dir_light = p3js.DirectionalLight(position=[0.0, 10.0, 10.0])
   ambient_light = p3js.AmbientLight()

   axes = p3js.AxesHelper()
   scene = p3js.Scene(children=[cyl_mesh, axes, camera, dir_light, ambient_light])
   controller = p3js.OrbitControls(controlling=camera)
   renderer = p3js.Renderer(camera=camera,
                            scene=scene,
                            controls=[controller],
                            width=view_width,
                            height=view_height)

Now display the scene by calling the renderer:

.. jupyter-execute::

   renderer

Transformation Matrices

The location and orientation of any given mesh is stored in its transformation matrix. A transformation matrix is commonly used in graphics applications because it can describe the position, orientation, scaling, and skewing of a mesh of points. A transformation matrix that only describes rotation and position takes this form:

\mathbf{T} = \begin{bmatrix}
{}^N\mathbf{C}^B & \bar{0} \\
\bar{r}^{P/O} & 1
\end{bmatrix} \quad \mathbf{T}\in \mathbb{R}^{4x4}

Here the direction cosine matrix of a mesh B with respect to the scene's global reference frame N is stored in the first three rows and columns, the position vector to a reference point P fixed in the mesh relative to the scene's origin point O is stored in the first three columns of the bottom row. If there is no rotation or translation, the transformation matrix becomes the identity matrix. This matrix is stored in the :external:py:attr:`~py3js.Mesh.matrix` attribute of the mesh:

.. jupyter-execute::

   cyl_mesh.matrix

Notice that the 4x4 matrix is stored "flattened" in a single list of 16 values.

.. jupyter-execute::

   len(cyl_mesh.matrix)

If you change this list to a NumPy array you can :external:py:meth:`~numpy.reshape` it and :external:py:meth:`~numpy.flatten` it to see the connection.

.. jupyter-execute::

   np.array(cyl_mesh.matrix).reshape(4, 4)

.. jupyter-execute::

   np.array(cyl_mesh.matrix).reshape(4, 4).flatten()

Each mesh/geometry has its own local coordinate system and origin. For the cylinder, the origin is at the geometric center and the axis of the cylinder is aligned with its local Y axis. For our body A, we need the cylinder's axis to align with our \hat{a}_x vector. To solve this, we need to create a new reference frame in which its Y unit vector is aligned with the \hat{a}_x. I introduce reference frame A_c for this purpose:

.. jupyter-execute::

   Ac = me.ReferenceFrame('Ac')
   Ac.orient_axis(A, sm.pi/2, A.z)

Now we can create a transformation matrix for A_c and A_o. A_o aligns with the cylinder mesh's origin and A_c aligns with its coordinate system.

.. jupyter-execute::

   TA = sm.eye(4)
   TA[:3, :3] = Ac.dcm(N)
   TA[3, :3] = sm.transpose(Ao.pos_from(O).to_matrix(N))
   TA

The B rod is already correctly aligned with the cylinder geometry's local coordinate system so we do not need to introduce a new reference frame for its transformation matrix.

.. jupyter-execute::

   TB = sm.eye(4)
   TB[:3, :3] = B.dcm(N)
   TB[3, :3] = sm.transpose(Bo.pos_from(O).to_matrix(N))
   TB

Lastly, we will introduce a sphere mesh to show the location of point Q. We can choose any reference frame because a sphere looks the same from all directions, but I choose to use the B frame here since we describe the point as sliding along the rod B. This choice will play a role in making the local coordinate axes visualize a bit better in the final animations.

.. jupyter-execute::

   TQ = sm.eye(4)
   TQ[:3, :3] = B.dcm(N)
   TQ[3, :3] = sm.transpose(Q.pos_from(O).to_matrix(N))
   TQ

Now that we have symbolic transformation matrices, let's flatten them all to be in the form that three.js needs:

.. jupyter-execute::

   TA = TA.reshape(16, 1)
   TB = TB.reshape(16, 1)
   TQ = TQ.reshape(16, 1)

.. jupyter-execute::

   TA

Now create a function to numerically evaluate the transformation matrices given the generalized coordinates and constants of the system:

.. jupyter-execute::

   eval_transform = sm.lambdify((q, p), (TA, TB, TQ))
   eval_transform(q_vals, p_vals)

Finally, create a list of lists for the transformation matrices at each time in ts, as this is the form needed for the animation data below:

.. jupyter-execute::

   TAs = []
   TBs = []
   TQs = []

   for xi in xs:
       TAi, TBi, TQi = eval_transform(xi[:3], p_vals)
       TAs.append(TAi.squeeze().tolist())
       TBs.append(TBi.squeeze().tolist())
       TQs.append(TQi.squeeze().tolist())

Here are the first two numerical transformation matrices to see what we have created:

.. jupyter-execute::

   TAs[:2]

Geometry and Mesh Definitions

Create two cylinders for rods A and B and a sphere for particle Q:

.. jupyter-execute::

   rod_radius = p_vals[3]/20  # l/20
   sphere_radius = p_vals[3]/16  # l/16

   geom_A = p3js.CylinderGeometry(
       radiusTop=rod_radius,
       radiusBottom=rod_radius,
       height=p_vals[3],  # l
   )

   geom_B = p3js.CylinderGeometry(
       radiusTop=rod_radius,
       radiusBottom=rod_radius,
       height=p_vals[3],  # l
   )

   geom_Q = p3js.SphereGeometry(radius=sphere_radius)

Now create meshes for each body and add a material of a different color for each mesh. Each mesh will need a unique name so that we can associate the animation information with the correct object. After the creation of the mesh set :external:py:attr:`~py3js.Mesh.matrixAutoUpdate` to false so that we can manually specify the transformation matrix during the animation. Lastly, add local coordinate axes to each mesh and set the transformation matrix to the initial configuration.

.. jupyter-execute::

   arrow_length = 0.2

   mesh_A = p3js.Mesh(
       geometry=geom_A,
       material=p3js.MeshStandardMaterial(color='red'),
       name='mesh_A',
   )
   mesh_A.matrixAutoUpdate = False
   mesh_A.add(p3js.AxesHelper(arrow_length))
   mesh_A.matrix = TAs[0]

   mesh_B = p3js.Mesh(
       geometry=geom_B,
       material=p3js.MeshStandardMaterial(color='blue'),
       name='mesh_B',
   )
   mesh_B.matrixAutoUpdate = False
   mesh_B.add(p3js.AxesHelper(arrow_length))
   mesh_B.matrix = TBs[0]

   mesh_Q = p3js.Mesh(
       geometry=geom_Q,
       material=p3js.MeshStandardMaterial(color='green'),
       name='mesh_Q',
   )
   mesh_Q.matrixAutoUpdate = False
   mesh_Q.add(p3js.AxesHelper(arrow_length))
   mesh_Q.matrix = TQs[0]

Scene Setup

Now create a scene and renderer similar to as we did earlier. Include the camera, lighting, coordinate axes, and all of the meshes.

.. jupyter-execute::

   view_width = 600
   view_height = 400

   camera = p3js.PerspectiveCamera(position=[1.5, 0.6, 1],
                                   up=[-1.0, 0.0, 0.0],
                                   aspect=view_width/view_height)

   key_light = p3js.DirectionalLight(position=[0, 10, 10])
   ambient_light = p3js.AmbientLight()

   axes = p3js.AxesHelper()

   children = [mesh_A, mesh_B, mesh_Q, axes, camera, key_light, ambient_light]

   scene = p3js.Scene(children=children)

   controller = p3js.OrbitControls(controlling=camera)
   renderer = p3js.Renderer(camera=camera, scene=scene, controls=[controller],
                            width=view_width, height=view_height)

Animation Setup

three.js uses the concept of a "track" to track the data that changes over time for an animation. A :external:py:class:`~py3js.VectorKeyframeTrack` can be used to associate time varying transformation matrices with a specific mesh. Create a track for each mesh. Make sure that the name keyword argument matches the name of the mesh with this syntax: scene/<mesh name>.matrix. The times and values keyword arguments hold the simulation time values and the list of transformation matrices at each time, respectively.

.. jupyter-execute::

   track_A = p3js.VectorKeyframeTrack(
       name="scene/mesh_A.matrix",
       times=ts,
       values=TAs
   )

   track_B = p3js.VectorKeyframeTrack(
       name="scene/mesh_B.matrix",
       times=ts,
       values=TBs
   )

   track_Q = p3js.VectorKeyframeTrack(
       name="scene/mesh_Q.matrix",
       times=ts,
       values=TQs
   )

Now create an :external:py:class:`~pythreejs.AnimationAction` that links the tracks to a play/pause button and associates this with the scene.

.. jupyter-execute::

   tracks = [track_B, track_A, track_Q]
   duration = ts[-1] - ts[0]
   clip = p3js.AnimationClip(tracks=tracks, duration=duration)
   action = p3js.AnimationAction(p3js.AnimationMixer(scene), clip, scene)

You can find more about setting up animations with pythreejs in their documentation:

https://pythreejs.readthedocs.io/en/stable/examples/Animation.html

Animated Interactive 3D Visualization

With the scene and animation now defined the renderer and the animation controls can be displayed with:

.. jupyter-execute::

   renderer

.. jupyter-execute::

   action

The axes attached to the inertial reference frame and each mesh are the local coordinate system for that object. The X axis is red, the Y axis is green, the Z axis is blue.

The animation can be used to confirm realistic motion of the multibody system and to visually explore the various motions that can occur.

.. todo:: Create a function that takes the simulation parameters and outputs
   the animation to show how to quickly iterate on changes to initial
   conditions and parameters.

.. todo::

   Show how to import more complex shapes.
   https://github.com/KhronosGroup/glTF-Sample-Models/raw/master/2.0/Suzanne/glTF/Suzanne.gltf
   https://upload.wikimedia.org/wikipedia/commons/e/e3/Suzanne.stl
   https://commons.wikimedia.org/wiki/File:Suzanne.stl