1#------------------------------------------------------------------------------
2# Name:        pychrono example
3# Purpose:
4#
5# Author:      Lijing Yang
6#
7# Created:     6/12/2020
8# Copyright:   (c) ProjectChrono 2019
9#------------------------------------------------------------------------------
10
11
12import pychrono.core as chrono
13import pychrono.irrlicht as chronoirr
14import math
15
16print ("Example: demonstration of using friction models")
17
18# The path to the Chrono data directory containing various assets (meshes, textures, data files)
19# is automatically set, relative to the default location of this demo.
20# If running from a different directory, you must change the path to the data directory with:
21# chrono.SetChronoDataPath('../../../../data/')
22
23
24# Helper class to define a cylindrical shape
25class MyObstacle:
26    def __init__(self, r, pos):
27        self.radius = r
28        self.center = pos
29    def GetVisualization(self):
30        level = chrono.ChAssetLevel()
31        cyl = chrono.ChCylinderShape()
32        cyl.GetCylinderGeometry().rad = self.radius
33        cyl.GetCylinderGeometry().p1 = self.center + chrono.ChVectorD(0, 0, 0)
34        cyl.GetCylinderGeometry().p2 = self.center + chrono.ChVectorD(0, 1.1, 0)
35        level.AddAsset(cyl)
36        level.AddAsset(chrono.ChColorAsset(0.6, 0.3, 0.0))
37        return level
38
39# Custom collision detection callback class
40class MyCustomCollisionDetection(chrono.CustomCollisionCallback):
41    def __init__(self, ball, ground,
42                 ball_mat, obst_mat,
43                 ball_radius, obstacle):
44        super().__init__()
45        self.m_ball = ball
46        self.m_ground = ground
47        self.m_ball_mat = ball_mat
48        self.m_obst_mat = obst_mat
49        self.m_ball_radius = ball_radius
50        self.m_obst_radius = obstacle.radius
51        self.m_obst_center = obstacle.center
52
53    def OnCustomCollision(self, sys):
54        # super().OnCustomCollision(sys)
55        r_sum = self.m_ball_radius + self.m_obst_radius
56
57        # Get current ball position and project on horizontal plane.
58        b_pos = self.m_ball.GetPos()
59        b_center = chrono.ChVectorD(b_pos.x, 0.0, b_pos.z)
60
61        # Check collision with obstacle (working in the horizontal plane).
62        o_center = chrono.ChVectorD(self.m_obst_center.x, 0.0, self.m_obst_center.z)
63        delta = o_center - b_center
64        # Get the squared euclidean norm
65        dist2 = delta.Length2()
66
67        if dist2 >= r_sum * r_sum:
68            return
69
70        # Find collision points on the ball and obstacle and the contact normal.
71        dist = math.sqrt(dist2)
72        normal = delta / dist
73        pt_ball = b_center + normal * self.m_ball_radius
74        pt_obst = o_center - normal * self.m_obst_radius
75
76        # Populate the collision info object (express all vectors in 3D).
77        # We pass null pointers to collision shapes.
78        contact = chrono.ChCollisionInfo()
79        contact.modelA = self.m_ball.GetCollisionModel()
80        contact.modelB = self.m_ground.GetCollisionModel()
81        contact.shapeA = None
82        contact.shapeB = None
83        contact.vN = chrono.ChVectorD(normal.x, 0.0, normal.z)
84        contact.vpA = chrono.ChVectorD(pt_ball.x, b_pos.y, pt_ball.z)
85        contact.vpB = chrono.ChVectorD(pt_obst.x, b_pos.y, pt_obst.z)
86        contact.distance = dist - r_sum
87
88        sys.GetContactContainer().AddContact(contact, self.m_ball_mat, self.m_obst_mat)
89
90
91# ---------------------------------------------------------------------
92#
93#  Create the simulation system and add items
94#
95
96# Change use_NSC to specify different contact method
97use_NSC = 0
98
99ball_radius = 0.5
100obst_radius = 2.0
101obst_center = chrono.ChVectorD(2.9, 0, 2.9)
102obstacle = MyObstacle(obst_radius, obst_center)
103
104# Create the system and the various contact materials
105if use_NSC:
106    sys = chrono.ChSystemNSC()
107    g_mat = chrono.ChMaterialSurfaceNSC()
108    g_mat.SetRestitution(0.9)
109    g_mat.SetFriction(0.4)
110    b_mat = chrono.ChMaterialSurfaceNSC()
111    b_mat.SetRestitution(0.9)
112    b_mat.SetFriction(0.5)
113    o_mat = chrono.ChMaterialSurfaceNSC()
114    o_mat.SetRestitution(0.9)
115    o_mat.SetFriction(0.4)
116
117    ground_mat = g_mat
118    ball_mat = b_mat
119    obst_mat = o_mat
120
121    time_step = 1e-3
122    frame_skip = 10
123
124else: # use SMC contact method
125    sys = chrono.ChSystemSMC()
126
127    g_mat = chrono.ChMaterialSurfaceSMC()
128    g_mat.SetRestitution(0.9)
129    g_mat.SetFriction(0.4)
130    b_mat = chrono.ChMaterialSurfaceSMC()
131    b_mat.SetRestitution(0.9)
132    b_mat.SetFriction(0.5)
133    o_mat = chrono.ChMaterialSurfaceSMC()
134    o_mat.SetRestitution(0.9)
135    o_mat.SetFriction(0.4)
136
137    ground_mat = g_mat
138    ball_mat = b_mat
139    obst_mat = o_mat
140
141    time_step = 1e-4
142    frame_skip = 100
143
144sys.Set_G_acc(chrono.ChVectorD(0, -9.8, 0))
145
146# Create the ground body with a plate and side walls (both collision and visualization).
147ground = chrono.ChBody()
148sys.AddBody(ground)
149ground.SetCollide(True)
150ground.SetBodyFixed(True)
151
152ground.GetCollisionModel().ClearModel()
153ground.GetCollisionModel().AddBox(ground_mat, 5.0, 1.0, 5.0, chrono.ChVectorD(0, -1, 0))
154ground.GetCollisionModel().AddBox(ground_mat, 0.1, 1.0, 5.1, chrono.ChVectorD(-5, 0, 0))
155ground.GetCollisionModel().AddBox(ground_mat, 0.1, 1.0, 5.1, chrono.ChVectorD( 5, 0, 0))
156ground.GetCollisionModel().AddBox(ground_mat, 5.1, 1.0, 0.1, chrono.ChVectorD(0, 0, -5))
157ground.GetCollisionModel().AddBox(ground_mat, 5.1, 1.0, 0.1, chrono.ChVectorD(0, 1,  5))
158ground.GetCollisionModel().BuildModel()
159
160vshape_1 = chrono.ChBoxShape()
161vshape_1.GetBoxGeometry().SetLengths(chrono.ChVectorD(10, 2, 10))
162vshape_1.GetBoxGeometry().Pos = chrono.ChVectorD(0, -1, 0)
163ground.AddAsset(vshape_1)
164
165vshape_2 = chrono.ChBoxShape()
166vshape_2.GetBoxGeometry().SetLengths(chrono.ChVectorD(0.2, 2, 10.2))
167vshape_2.GetBoxGeometry().Pos = chrono.ChVectorD(-5, 0, 0)
168ground.AddAsset(vshape_2)
169
170vshape_3 = chrono.ChBoxShape()
171vshape_3.GetBoxGeometry().SetLengths(chrono.ChVectorD(0.2, 2, 10.2))
172vshape_3.GetBoxGeometry().Pos = chrono.ChVectorD(5, 0, 0)
173ground.AddAsset(vshape_3)
174
175vshape_4 = chrono.ChBoxShape()
176vshape_4.GetBoxGeometry().SetLengths(chrono.ChVectorD(10.2, 2, 0.2))
177vshape_4.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, -5)
178ground.AddAsset(vshape_4)
179
180vshape_5 = chrono.ChBoxShape()
181vshape_5.GetBoxGeometry().SetLengths(chrono.ChVectorD(10.2, 2, 0.2))
182vshape_5.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, 5)
183ground.AddAsset(vshape_5)
184
185ground.AddAsset(chrono.ChTexture(chrono.GetChronoDataFile("textures/blue.png")))
186
187# Add obstacle visualization (in a separate level with a different color).
188ground.AddAsset(obstacle.GetVisualization())
189
190# Create the falling ball
191ball = chrono.ChBody()
192sys.AddBody(ball)
193ball.SetMass(10)
194comp = 4 * ball_radius * ball_radius
195ball.SetInertiaXX(chrono.ChVectorD(comp, comp, comp))
196ball.SetPos(chrono.ChVectorD(-3, 1.2 * ball_radius, -3))
197ball.SetPos_dt(chrono.ChVectorD(5, 0, 5))
198ball.SetCollide(True)
199
200ball.GetCollisionModel().ClearModel()
201ball.GetCollisionModel().AddSphere(ball_mat, ball_radius)
202ball.GetCollisionModel().BuildModel()
203
204vshape_s = chrono.ChSphereShape()
205vshape_s.GetSphereGeometry().rad = ball_radius
206vshape_s.GetSphereGeometry().Pos = ball.GetPos()
207ball.AddAsset(vshape_s)
208ball.AddAsset(chrono.ChTexture(chrono.GetChronoDataFile("textures/bluewhite.png")))
209
210# Create a custom collision detection callback object and register it with the system
211my_collision = MyCustomCollisionDetection(ball, ground, ball_mat, obst_mat, ball_radius, obstacle)
212sys.RegisterCustomCollisionCallback(my_collision)
213
214
215# ---------------------------------------------------------------------
216#
217#  Create an Irrlicht application to visualize the system
218#
219
220myapplication = chronoirr.ChIrrApp(sys, 'PyChrono example: Custom contact', chronoirr.dimension2du(1024,768))
221
222myapplication.AddTypicalSky()
223myapplication.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
224myapplication.AddTypicalCamera(chronoirr.vector3df(8, 8, -6))
225myapplication.AddTypicalLights()
226
227# ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items
228# in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes.
229# If you need a finer control on which item really needs a visualization proxy in
230# Irrlicht, just use application.AssetBind(myitem) on a per-item basis.
231
232myapplication.AssetBindAll()
233
234# ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets
235# that you added to the bodies into 3D shapes, they can be visualized by Irrlicht!
236
237myapplication.AssetUpdateAll()
238
239
240# ---------------------------------------------------------------------
241#
242#  Run the simulation
243#
244
245myapplication.SetTimestep(1e-4)
246myapplication.SetTryRealtime(True)
247
248frame = 0
249while(myapplication.GetDevice().run()):
250    if frame % 100 == 0:
251        myapplication.BeginScene()
252        myapplication.DrawAll()
253        myapplication.EndScene()
254    myapplication.DoStep()
255    frame += 1
256
257
258
259
260