1
2 /**
3 This is a stripped down version of the collision playground example to illustrate the
4 behavior of the collision detection system and a few geometry dependent odities.
5 I have removed the ground plane and collision cliques for the purposes of this
6 example.
7
8 try the following:
9 1)all spheres: collisions are detected forces are drawn but not applied. No collision interaction;
10 2)all cubes(size 0.4-0.5): Collisions are detected but notice that the moving cube "cuts" into the stationary one
11 before collision is detected. No forces are generated at this stage. Collisions seem to occur
12 on a spherical boundary within the cube. This is similar to the contact playground example.
13
14 */
15 #include "SimTKsimbody.h"
16
17 #include <cstdio>
18 #include <exception>
19 #include <algorithm>
20 #include <iostream>
21 #include <fstream>
22
23 #include <string.h>
24 using std::cout; using std::endl;
25
26 using namespace SimTK;
27
28 Array_<State> saveEm;
29
30 static const Real TimeScale = 0.21;
31 static const Real FrameRate = 30;
32 static const Real ReportInterval = TimeScale/FrameRate;
33 static const Real ForceScale = .25;
34 static const Real MomentScale = .5;
35
36
37 class ForceArrowGenerator : public DecorationGenerator {
38 public:
ForceArrowGenerator(const MultibodySystem & system,const CompliantContactSubsystem & complCont)39 ForceArrowGenerator(const MultibodySystem& system,
40 const CompliantContactSubsystem& complCont)
41 : m_system(system), m_compliant(complCont) {}
42
generateDecorations(const State & state,Array_<DecorativeGeometry> & geometry)43 virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) override {
44 const Vec3 frcColors[] = {Red,Orange,Cyan};
45 const Vec3 momColors[] = {Blue,Green,Purple};
46 m_system.realize(state, Stage::Velocity);
47
48 const int ncont = m_compliant.getNumContactForces(state);
49 for (int i=0; i < ncont; ++i) {
50 const ContactForce& force = m_compliant.getContactForce(state,i);
51 const ContactId id = force.getContactId();
52 const Vec3& frc = force.getForceOnSurface2()[1];
53
54 const Vec3& mom = force.getForceOnSurface2()[0];
55 Real frcMag = frc.norm(), momMag=mom.norm();
56 int frcThickness = 1, momThickness = 1;
57 Real frcScale = ForceScale, momScale = ForceScale;
58 while (frcMag > 10)
59 frcThickness++, frcScale /= 10, frcMag /= 10;
60 while (momMag > 10)
61 momThickness++, momScale /= 10, momMag /= 10;
62 DecorativeLine frcLine(force.getContactPoint(),
63 force.getContactPoint() + frcScale*frc);
64 DecorativeLine momLine(force.getContactPoint(),
65 force.getContactPoint() + momScale*mom);
66 frcLine.setColor(frcColors[id%3]);
67 momLine.setColor(momColors[id%3]);
68 frcLine.setLineThickness(2*frcThickness);
69 momLine.setLineThickness(2*momThickness);
70 geometry.push_back(frcLine);
71 geometry.push_back(momLine);
72
73 ContactPatch patch;
74 const bool found = m_compliant.calcContactPatchDetailsById(state,id,patch);
75 //cout << "patch for id" << id << " found=" << found << endl;
76 //cout << "resultant=" << patch.getContactForce() << endl;
77 //cout << "num details=" << patch.getNumDetails() << endl;
78 for (int i=0; i < patch.getNumDetails(); ++i) {
79 const ContactDetail& detail = patch.getContactDetail(i);
80 const Real peakPressure = detail.getPeakPressure();
81 // Make a black line from the element's contact point in the normal
82 // direction, with length proportional to log(peak pressure)
83 // on that element.
84
85 DecorativeLine normal(detail.getContactPoint(),
86 detail.getContactPoint()+ std::log10(peakPressure)
87 * detail.getContactNormal());
88 normal.setColor(Black);
89 geometry.push_back(normal);
90
91 // Make a red line that extends from the contact
92 // point in the direction of the slip velocity, of length 3*slipvel.
93 DecorativeLine slip(detail.getContactPoint(),
94 detail.getContactPoint()+3*detail.getSlipVelocity());
95 slip.setColor(Red);
96 geometry.push_back(slip);
97 }
98 }
99 }
100 private:
101 const MultibodySystem& m_system;
102 const CompliantContactSubsystem& m_compliant;
103 };
104
105 class MyReporter : public PeriodicEventReporter {
106 public:
MyReporter(const MultibodySystem & system,const CompliantContactSubsystem & complCont,Real reportInterval)107 MyReporter(const MultibodySystem& system,
108 const CompliantContactSubsystem& complCont,
109 Real reportInterval)
110 : PeriodicEventReporter(reportInterval), m_system(system),
111 m_compliant(complCont)
112 {}
113
~MyReporter()114 ~MyReporter() {}
115
handleEvent(const State & state) const116 void handleEvent(const State& state) const override {
117 m_system.realize(state, Stage::Dynamics);
118 /* cout << state.getTime() << ": E = " << m_system.calcEnergy(state)
119 << " Ediss=" << m_compliant.getDissipatedEnergy(state)
120 << " E+Ediss=" << m_system.calcEnergy(state)
121 +m_compliant.getDissipatedEnergy(state)
122 << endl;*/
123 const int ncont = m_compliant.getNumContactForces(state);
124 if(ncont>0)
125 {
126 cout << "Num contacts: " << m_compliant.getNumContactForces(state) << endl;
127 for (int i=0; i < ncont; ++i) {
128 const ContactForce& force = m_compliant.getContactForce(state,i);
129 cout << force;
130 }
131 }
132 saveEm.push_back(state);
133 }
134 private:
135 const MultibodySystem& m_system;
136 const CompliantContactSubsystem& m_compliant;
137 };
138
139 // These are the item numbers for the entries on the Run menu.
140 static const int RunMenuId = 3, HelpMenuId = 7;
141 static const int GoItem = 1, ReplayItem=2, QuitItem=3;
142
143 // This is a periodic event handler that interrupts the simulation on a regular
144 // basis to poll the InputSilo for user input. If there has been some, process it.
145 // This one does nothing but look for the Run->Quit selection.
146 class UserInputHandler : public PeriodicEventHandler {
147 public:
UserInputHandler(Visualizer::InputSilo & silo,Real interval)148 UserInputHandler(Visualizer::InputSilo& silo, Real interval)
149 : PeriodicEventHandler(interval), m_silo(silo) {}
150
handleEvent(State & state,Real accuracy,bool & shouldTerminate) const151 virtual void handleEvent(State& state, Real accuracy,
152 bool& shouldTerminate) const override
153 {
154 int menuId, item;
155 if (m_silo.takeMenuPick(menuId, item) && menuId==RunMenuId && item==QuitItem)
156 shouldTerminate = true;
157 }
158
159 private:
160 Visualizer::InputSilo& m_silo;
161 };
162
main()163 int main() {
164 try
165 { // Create the system.
166
167 MultibodySystem system;
168 SimbodyMatterSubsystem matter(system);
169 GeneralForceSubsystem forces(system);
170
171 /// uncoment gravity to get some sort of collision interaction
172 /// for cylinder mesh
173 // Force::UniformGravity gravity(forces, matter,Vec3(0,0.001,0), 2);
174
175 ContactTrackerSubsystem tracker(system);
176 //GeneralContactSubsystem contactsys(system);
177 CompliantContactSubsystem contactForces(system, tracker);
178 contactForces.setTrackDissipatedEnergy(true);
179
180 for(SubsystemIndex i(0); i<system.getNumSubsystems(); ++i)
181 {
182 fprintf(stderr,"subsytem name %d %s\n", (int)i,
183 system.getSubsystem((SubsystemIndex)i).getName().c_str());
184 }
185
186 const Real rad = .4;
187 PolygonalMesh pyramidMesh1,pyramidMesh2;
188
189 /// load cylinder forces drawn, but interaction depends on gravity???
190
191
192 const Real fFac =1; // to turn off friction
193 const Real fDis = .5*0.2; // to turn off dissipation
194 const Real fVis = .1*.1; // to turn off viscous friction
195 const Real fK = 100*1e6; // pascals
196
197 Body::Rigid pendulumBody3(MassProperties(100.0, Vec3(0), 100*Inertia(1)));
198 PolygonalMesh body3contact = PolygonalMesh::createSphereMesh(rad, 2);
199 ContactGeometry::TriangleMesh geo3(body3contact);
200
201 const DecorativeMesh mesh3(geo3.createPolygonalMesh());
202 pendulumBody3.addDecoration(Transform(),
203 DecorativeMesh(mesh3).setOpacity(.2));
204 pendulumBody3.addDecoration(Transform(),
205 DecorativeMesh(mesh3).setColor(Gray)
206 .setRepresentation(DecorativeGeometry::DrawWireframe)
207 .setOpacity(.1));
208 ContactSurface s1(geo3,
209 ContactMaterial(fK*.1,fDis*.9,fFac*.8,fFac*.7,fVis*10));
210 s1.setThickness(1);
211 s1.setShape(geo3);
212 //ContactGeometry::Sphere geo3(rad);
213 pendulumBody3.addContactSurface(Transform(),s1);
214 /*
215 std::ifstream meshFile1,meshFile2;
216 meshFile1.open("cyl3.obj");
217 pyramidMesh1.loadObjFile(meshFile1); meshFile1.close();
218 */
219 pyramidMesh1 = PolygonalMesh::createSphereMesh(rad, 2);
220 ContactGeometry::TriangleMesh pyramid1(pyramidMesh1);
221
222 DecorativeMesh showPyramid1(pyramid1.createPolygonalMesh());
223 const Real ballMass = 200;
224 Body::Rigid ballBody(MassProperties(ballMass, Vec3(0),
225 ballMass*UnitInertia::sphere(1)));
226
227 ballBody.addDecoration(Transform(),
228 showPyramid1.setColor(Cyan).setOpacity(.2));
229 ballBody.addDecoration(Transform(),
230 showPyramid1.setColor(Gray)
231 .setRepresentation(DecorativeGeometry::DrawWireframe));
232
233 ContactSurface s2(pyramid1,
234 ContactMaterial(fK*.1,fDis*.9,
235 .1*fFac*.8,.1*fFac*.7,fVis*1));
236 s2.setThickness(1);
237 s2.setShape(pyramid1);
238 ballBody.addContactSurface(Transform(),/*ContactSurface(ContactGeometry::Sphere(rad),ContactMaterial(fK*.1,fDis*.9,
239 .1*fFac*.8,.1*fFac*.7,fVis*1))*/ s2/*.joinClique(clique1)*/);
240
241 /* Body::Rigid d(MassProperties(1.0, Vec3(0),Inertia(1)));
242
243 MobilizedBody::Pin dud(matter.Ground(),Transform(),d,Transform());
244 */
245 MobilizedBody::Free ball(matter.Ground(), Transform(Vec3(-2,-2,0)),
246 ballBody, Transform(Vec3(0)));
247
248
249
250 MobilizedBody::Free ball1(matter.Ground(), Transform(Vec3(0,0,0)),
251 ballBody, Transform(Vec3(0)));
252 /*
253 MobilizedBody::Free ball2(matter.Ground(), Transform(Vec3(-4,0,0)),
254 ballBody, Transform(Vec3(0)));
255 */
256
257 MobilizedBody::Free ball3(matter.Ground(), Transform(Vec3(-1,-2,0)),
258 ballBody, Transform(Vec3(0)));
259
260
261 MobilizedBody::Pin pendulum3(matter.Ground(), Transform(Vec3(-2,0,0)),
262 pendulumBody3, Transform(Vec3(0, 2, 0)));
263
264 ball.updBody();
265 ball1.updBody();
266
267 Visualizer viz(system);
268 viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
269 viz.setMode(Visualizer::RealTime);
270 viz.setDesiredBufferLengthInSec(1);
271 viz.setDesiredFrameRate(FrameRate);
272 viz.setGroundHeight(-3);
273 viz.setShowShadows(true);
274 viz.setBackgroundType(Visualizer::SolidColor);
275 Visualizer::InputSilo* silo = new Visualizer::InputSilo();
276 viz.addInputListener(silo);
277 Array_<std::pair<String,int> > runMenuItems;
278 runMenuItems.push_back(std::make_pair("Go", GoItem));
279 runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
280 runMenuItems.push_back(std::make_pair("Quit", QuitItem));
281 viz.addMenu("Run", RunMenuId, runMenuItems);
282
283 Array_<std::pair<String,int> > helpMenuItems;
284 helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
285 viz.addMenu("Help", HelpMenuId, helpMenuItems);
286
287 system.addEventReporter(new MyReporter(system,contactForces,ReportInterval));
288 system.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
289
290 // Check for a Run->Quit menu pick every 1/4 second.
291 system.addEventHandler(new UserInputHandler(*silo, .25));
292 // system.addEventHandler(new TriggeredEventHandler(Stage::Model));
293 // Initialize the system and state.
294
295 system.realizeTopology();
296
297 State state = system.getDefaultState();
298 /*
299 ball.setQToFitTransform(state, Transform(Rotation(Pi/2,XAxis),
300 Vec3(0,-1.8,0)));
301 */
302 //pendulum.setOneQ(state, 0, -Pi/12);
303 pendulum3.setOneQ(state, 0, -Pi/2);
304 pendulum3.setOneU(state, 0, Pi/4);
305 // ball.setOneU(state, 1, 0.1);
306 viz.report(state);
307 matter.updAllParticleVelocities(state);
308 printf("Default state\n");
309 /* cout << "t=" << state.getTime()
310 << " q=" << pendulum.getQAsVector(state) << pendulum2.getQAsVector(state)
311 << " u=" << pendulum.getUAsVector(state) << pendulum2.getUAsVector(state)
312 << endl;
313 */
314 cout << "\nChoose 'Go' from Run menu to simulate:\n";
315 int menuId, item;
316 do { silo->waitForMenuPick(menuId, item);
317 if (menuId != RunMenuId || item != GoItem)
318 cout << "\aDude ... follow instructions!\n";
319 } while (menuId != RunMenuId || item != GoItem);
320
321 // Simulate it.
322
323 // The system as parameterized is very stiff (mostly due to friction)
324 // and thus runs best with CPodes which is extremely stable for
325 // stiff problems. To get reasonable performance out of the explicit
326 // integrators (like the RKs) you'll have to run at a very loose
327 // accuracy like 0.1, or reduce the friction coefficients and
328 // maybe the stiffnesses.
329
330 //ExplicitEulerIntegrator integ(system);
331 CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
332 //RungeKuttaFeldbergIntegrator integ(system);
333 //RungeKuttaMersonIntegrator integ(system);
334 //RungeKutta3Integrator integ(system);
335 //VerletIntegrator integ(system);
336 //integ.setMaximumStepSize(1e-1);
337 //integ.setAllowInterpolation(false);
338 integ.setAccuracy(1e-3); // minimum for CPodes
339 //integ.setAccuracy(.1);
340 TimeStepper ts(system, integ);
341
342
343 ts.initialize(state);
344 double cpuStart = cpuTime();
345 double realStart = realTime();
346
347 ts.stepTo(2000.0);
348
349 const double timeInSec = realTime() - realStart;
350 const int evals = integ.getNumRealizations();
351 /* cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
352 timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step="
353 << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
354 << (1000*ts.getTime())/evals << "ms/eval\n";
355 cout << " CPU time was " << cpuTime() - cpuStart << "s\n";
356
357 printf("Using Integrator %s at accuracy %g:\n",
358 integ.getMethodName(), integ.getAccuracyInUse());
359 printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
360 printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
361 printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
362 */
363 viz.dumpStats(std::cout);
364
365 // Add as slider to control playback speed.
366 viz.addSlider("Speed", 1, 0, 4, 1);
367 viz.setMode(Visualizer::PassThrough);
368
369 silo->clear(); // forget earlier input
370 double speed = 1; // will change if slider moves
371 while(true) {
372 cout << "Choose Run/Replay to see that again ...\n";
373
374 int menuId, item;
375 silo->waitForMenuPick(menuId, item);
376
377
378 if (menuId != RunMenuId) {
379 cout << "\aUse the Run menu!\n";
380 continue;
381 }
382
383 if (item == QuitItem)
384 break;
385 if (item != ReplayItem) {
386 cout << "\aHuh? Try again.\n";
387 continue;
388 }
389
390 for (double i=0; i < (int)saveEm.size(); i += speed ) {
391 int slider; Real newValue;
392 if (silo->takeSliderMove(slider,newValue)) {
393 speed = newValue;
394 }
395 viz.report(saveEm[(int)i]);
396 }
397 }
398
399 } catch (const std::exception& e) {
400 std::printf("EXCEPTION THROWN: %s\n", e.what());
401 exit(1);
402
403 } catch (...) {
404 std::printf("UNKNOWN EXCEPTION THROWN\n");
405 exit(1);
406 }
407
408 return 0;
409 }
410
411
412