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