1#------------------------------------------------------------------------------
2# Name:        pychrono example
3# Purpose:
4#
5# Author:      Alessandro Tasora
6#
7# Created:     1/01/2019
8# Copyright:   (c) ProjectChrono 2019
9#------------------------------------------------------------------------------
10
11
12import pychrono.core as chrono
13import pychrono.irrlicht as chronoirr
14import matplotlib.pyplot as plt
15import numpy as np
16
17print ("Example: create a slider crank and plot results");
18
19# The path to the Chrono data directory containing various assets (meshes, textures, data files)
20# is automatically set, relative to the default location of this demo.
21# If running from a different directory, you must change the path to the data directory with:
22#chrono.SetChronoDataPath('path/to/data')
23
24# ---------------------------------------------------------------------
25#
26#  Create the simulation system and add items
27#
28
29mysystem      = chrono.ChSystemNSC()
30
31# Some data shared in the following
32crank_center = chrono.ChVectorD(-1,0.5,0)
33crank_rad    = 0.4
34crank_thick  = 0.1
35rod_length   = 1.5
36
37
38# Create four rigid bodies: the truss, the crank, the rod, the piston.
39
40# Create the floor truss
41mfloor = chrono.ChBodyEasyBox(3, 1, 3, 1000)
42mfloor.SetPos(chrono.ChVectorD(0,-0.5,0))
43mfloor.SetBodyFixed(True)
44mysystem.Add(mfloor)
45
46# Create the flywheel crank
47mcrank = chrono.ChBodyEasyCylinder(crank_rad, crank_thick, 1000)
48mcrank.SetPos(crank_center + chrono.ChVectorD(0, 0, -0.1))
49# Since ChBodyEasyCylinder creates a vertical (y up) cylinder, here rotate it:
50mcrank.SetRot(chrono.Q_ROTATE_Y_TO_Z)
51mysystem.Add(mcrank)
52
53# Create a stylized rod
54mrod = chrono.ChBodyEasyBox(rod_length, 0.1, 0.1, 1000)
55mrod.SetPos(crank_center + chrono.ChVectorD(crank_rad+rod_length/2 , 0, 0))
56mysystem.Add(mrod)
57
58# Create a stylized piston
59mpiston = chrono.ChBodyEasyCylinder(0.2, 0.3, 1000)
60mpiston.SetPos(crank_center + chrono.ChVectorD(crank_rad+rod_length, 0, 0))
61mpiston.SetRot(chrono.Q_ROTATE_Y_TO_X)
62mysystem.Add(mpiston)
63
64
65# Now create constraints and motors between the bodies.
66
67# Create crank-truss joint: a motor that spins the crank flywheel
68my_motor = chrono.ChLinkMotorRotationSpeed()
69my_motor.Initialize(mcrank,   # the first connected body
70                    mfloor,   # the second connected body
71                    chrono.ChFrameD(crank_center)) # where to create the motor in abs.space
72my_angularspeed = chrono.ChFunction_Const(chrono.CH_C_PI) # ang.speed: 180°/s
73my_motor.SetMotorFunction(my_angularspeed)
74mysystem.Add(my_motor)
75
76# Create crank-rod joint
77mjointA = chrono.ChLinkLockRevolute()
78mjointA.Initialize(mrod,
79                   mcrank,
80                   chrono.ChCoordsysD( crank_center + chrono.ChVectorD(crank_rad,0,0) ))
81mysystem.Add(mjointA)
82
83# Create rod-piston joint
84mjointB = chrono.ChLinkLockRevolute()
85mjointB.Initialize(mpiston,
86                   mrod,
87                   chrono.ChCoordsysD( crank_center + chrono.ChVectorD(crank_rad+rod_length,0,0) ))
88mysystem.Add(mjointB)
89
90# Create piston-truss joint
91mjointC = chrono.ChLinkLockPrismatic()
92mjointC.Initialize(mpiston,
93                   mfloor,
94                   chrono.ChCoordsysD(
95                               crank_center + chrono.ChVectorD(crank_rad+rod_length,0,0),
96                               chrono.Q_ROTATE_Z_TO_X)
97                  )
98mysystem.Add(mjointC)
99
100
101
102# ---------------------------------------------------------------------
103#
104#  Create an Irrlicht application to visualize the system
105#
106
107myapplication = chronoirr.ChIrrApp(mysystem, 'PyChrono example', chronoirr.dimension2du(1024,768))
108
109myapplication.AddTypicalSky()
110myapplication.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
111myapplication.AddTypicalCamera(chronoirr.vector3df(1,1,3), chronoirr.vector3df(0,1,0))
112myapplication.AddTypicalLights()
113
114            # ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items
115            # in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes.
116            # If you need a finer control on which item really needs a visualization proxy in
117            # Irrlicht, just use application.AssetBind(myitem); on a per-item basis.
118
119myapplication.AssetBindAll();
120
121            # ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets
122            # that you added to the bodies into 3D shapes, they can be visualized by Irrlicht!
123
124myapplication.AssetUpdateAll();
125
126
127# ---------------------------------------------------------------------
128#
129#  Run the simulation
130#
131
132# Initialize these lists to store values to plot.
133array_time   = []
134array_angle  = []
135array_pos    = []
136array_speed  = []
137
138myapplication.SetTimestep(0.005)
139myapplication.SetTryRealtime(True)
140
141# Run the interactive simulation loop
142while(myapplication.GetDevice().run()):
143
144    # for plotting, append instantaneous values:
145    array_time.append(mysystem.GetChTime())
146    array_angle.append(my_motor.GetMotorRot())
147    array_pos.append(mpiston.GetPos().x)
148    array_speed.append(mpiston.GetPos_dt().x)
149
150    # here happens the visualization and step time integration
151    myapplication.BeginScene()
152    myapplication.DrawAll()
153    myapplication.DoStep()
154    myapplication.EndScene()
155
156    # stop simulation after 2 seconds
157    if mysystem.GetChTime() > 20:
158          myapplication.GetDevice().closeDevice()
159
160
161# Use matplotlib to make two plots when simulation ended:
162fig, (ax1, ax2) = plt.subplots(2, sharex = True)
163
164ax1.plot(array_angle, array_pos)
165ax1.set(ylabel='position [m]')
166ax1.grid()
167
168ax2.plot(array_angle, array_speed, 'r--')
169ax2.set(ylabel='speed [m]',xlabel='angle [rad]')
170ax2.grid()
171
172# trick to plot \pi on x axis of plots instead of 1 2 3 4 etc.
173plt.xticks(np.linspace(0, 2*np.pi, 5),['0','$\pi/2$','$\pi$','$3\pi/2$','$2\pi$'])
174
175
176
177
178
179