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