1# ============================================================================= 2# PROJECT CHRONO - http:#projectchrono.org 3# 4# Copyright (c) 2014 projectchrono.org 5# All rights reserved. 6# 7# Use of this source code is governed by a BSD-style license that can be found 8# in the LICENSE file at the top level of the distribution and at 9# http:#projectchrono.org/license-chrono.txt. 10# 11# ============================================================================= 12# Authors: Simone Benatti 13# ============================================================================= 14# 15# Chrono demonstration of using contact callbacks for non-smooth contacts 16# (complementarity-based). 17# 18# The global reference frame has Y up. 19# 20# ============================================================================= 21 22import pychrono as chrono 23import pychrono.irrlicht as chronoirr 24 25# ----------------------------------------------------------------------------- 26# Callback class for contact reporting 27# ----------------------------------------------------------------------------- 28class ContactReporter (chrono.ReportContactCallback): 29 def __init__(self, box) : 30 self.m_box = box 31 super().__init__() 32 33 def OnReportContact( self, 34 pA, 35 pB, 36 plane_coord, 37 distance, 38 eff_radius, 39 cforce, 40 ctorque, 41 modA, 42 modB): 43 bodyA = chrono.CastToChBody(modA) 44 bodyB = chrono.CastToChBody(modB) 45 if (bodyA == self.m_box) : 46 print(" ", pA.x, pA.y, pA.z) 47 elif (bodyB == self.m_box) : 48 print(" ", pB.x, pB.y, pB.z) 49 50 return True 51 52 53# ----------------------------------------------------------------------------- 54# Callback class for modifying composite material 55# ----------------------------------------------------------------------------- 56class ContactMaterial(chrono.AddContactCallback): 57 def __init__(self): 58 super().__init__() 59 def OnAddContact( self, 60 contactinfo, 61 material): 62 # Downcast to appropriate composite material type 63 mat = chrono.CastToChMaterialCompositeNSC(material) 64 65 # Set different friction for left/right halfs 66 if (contactinfo.vpA.z > 0) : 67 friction = 0.3 68 else: 69 friction = 0.8 70 mat.static_friction = friction 71 mat.sliding_friction = friction 72 73 74print( "Copyright (c) 2017 projectchrono.org") 75 76# ---------------- 77# Parameters 78# ---------------- 79 80friction = 0.6 81collision_envelope = .001 82 83# ----------------- 84# Create the system 85# ----------------- 86 87system = chrono.ChSystemNSC() 88system.Set_G_acc(chrono.ChVectorD(0, -10, 0)) 89 90# Set solver settings 91system.SetSolverMaxIterations(100) 92system.SetMaxPenetrationRecoverySpeed(1e8) 93system.SetSolverForceTolerance(0) 94 95# -------------------------------------------------- 96# Create a contact material, shared among all bodies 97# -------------------------------------------------- 98 99material = chrono.ChMaterialSurfaceNSC() 100material.SetFriction(friction) 101 102# ---------- 103# Add bodies 104# ---------- 105 106container = chrono.ChBody() 107system.Add(container) 108container.SetPos(chrono.ChVectorD(0, 0, 0)) 109container.SetBodyFixed(True) 110container.SetIdentifier(-1) 111 112container.SetCollide(True) 113container.GetCollisionModel().SetEnvelope(collision_envelope) 114container.GetCollisionModel().ClearModel() 115chrono.AddBoxGeometry(container, material, chrono.ChVectorD(4, 0.5, 4), chrono.ChVectorD(0, -0.5, 0)) 116container.GetCollisionModel().BuildModel() 117 118container.AddAsset(chrono.ChColorAsset(chrono.ChColor(0.4, 0.4, 0.4))) 119 120box1 = chrono.ChBody() 121box1.SetMass(10) 122box1.SetInertiaXX(chrono.ChVectorD(1, 1, 1)) 123box1.SetPos(chrono.ChVectorD(-1, 0.21, -1)) 124box1.SetPos_dt(chrono.ChVectorD(5, 0, 0)) 125 126box1.SetCollide(True) 127box1.GetCollisionModel().SetEnvelope(collision_envelope) 128box1.GetCollisionModel().ClearModel() 129chrono.AddBoxGeometry(box1, material, chrono.ChVectorD(0.4, 0.2, 0.1)) 130box1.GetCollisionModel().BuildModel() 131 132box1.AddAsset(chrono.ChColorAsset(chrono.ChColor(0.1, 0.1, 0.4))) 133 134system.AddBody(box1) 135 136box2 = chrono.ChBody(system.NewBody()) 137box2.SetMass(10) 138box2.SetInertiaXX(chrono.ChVectorD(1, 1, 1)) 139box2.SetPos(chrono.ChVectorD(-1, 0.21, +1)) 140box2.SetPos_dt(chrono.ChVectorD(5, 0, 0)) 141 142box2.SetCollide(True) 143box2.GetCollisionModel().SetEnvelope(collision_envelope) 144box2.GetCollisionModel().ClearModel() 145chrono.AddBoxGeometry(box2, material, chrono.ChVectorD(0.4, 0.2, 0.1)) 146box2.GetCollisionModel().BuildModel() 147 148box2.AddAsset(chrono.ChColorAsset(chrono.ChColor(0.4, 0.1, 0.1))) 149 150system.AddBody(box2) 151 152# ------------------------------- 153# Create the visualization window 154# ------------------------------- 155 156application = chronoirr.ChIrrApp(system, "NSC callbacks", chronoirr.dimension2du(800, 600)) 157application.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png')) 158application.AddTypicalSky() 159application.AddTypicalLights() 160application.AddTypicalCamera(chronoirr.vector3df(4, 4, -6)) 161 162application.AssetBindAll() 163application.AssetUpdateAll() 164 165# --------------- 166# Simulate system 167# --------------- 168 169creporter = ContactReporter(box1) 170 171cmaterial = ContactMaterial() 172system.GetContactContainer().RegisterAddContactCallback(cmaterial) 173 174application.SetTimestep(1e-3) 175 176while (application.GetDevice().run()) : 177 application.BeginScene(True, True, chronoirr.SColor(255, 140, 161, 192)) 178 application.DrawAll() 179 chronoirr.drawGrid(application.GetVideoDriver(), 0.5, 0.5, 12, 12, 180 chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngX(chrono.CH_C_PI_2))) 181 application.DoStep() 182 application.EndScene() 183 184 # Process contacts 185 print(str(system.GetChTime() ) + " " + str(system.GetNcontacts()) ) 186 system.GetContactContainer().ReportAllContacts(creporter) 187 188 189