1 /* -------------------------------------------------------------------------- *
2 * Simbody(tm) Example: Contact Playground *
3 * -------------------------------------------------------------------------- *
4 * This is part of the SimTK biosimulation toolkit originating from *
5 * Simbios, the NIH National Center for Physics-Based Simulation of *
6 * Biological Structures at Stanford, funded under the NIH Roadmap for *
7 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
8 * *
9 * Portions copyright (c) 2010-13 Stanford University and the Authors. *
10 * Authors: Kevin He, Michael Sherman *
11 * Contributors: *
12 * *
13 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
14 * not use this file except in compliance with the License. You may obtain a *
15 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
16 * *
17 * Unless required by applicable law or agreed to in writing, software *
18 * distributed under the License is distributed on an "AS IS" BASIS, *
19 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
20 * See the License for the specific language governing permissions and *
21 * limitations under the License. *
22 * -------------------------------------------------------------------------- */
23
24 /* Kevin He at Roblox created this example starting with
25 ExampleContactPlayground. It basically demonstrates why an Elastic Foundation
26 (EF) contact model is not a good way to handle coarsely-meshed simple objects,
27 like a box. EF uses the centroid of each face to generate an area-weighted
28 force, which can be a good physical representation with a dense mesh but
29 since the vertices and edges don't participate there is a lot of visible
30 penetration for a coarse mesh like the ones here.
31 */
32
33 #include "Simbody.h"
34
35 #include <cstdio>
36 #include <exception>
37 #include <algorithm>
38 #include <iostream>
39 #include <fstream>
40 using std::cout; using std::endl;
41
42 using namespace SimTK;
43
44 Array_<State> saveEm;
45
46 static const Real TimeScale = 1;
47 static const Real FrameRate = 30;
48 static const Real ReportInterval = TimeScale/FrameRate;
49
50 // These are the item numbers for the entries on the Run menu.
51 static const int RunMenuId = 3, HelpMenuId = 7;
52 static const int GoItem = 1, ReplayItem=2, QuitItem=3;
53
54 // This is a periodic event handler that interrupts the simulation on a regular
55 // basis to poll the InputSilo for user input. If there has been some, process it.
56 // This one does nothing but look for the Run->Quit selection.
57 class UserInputHandler : public PeriodicEventHandler {
58 public:
UserInputHandler(Visualizer::InputSilo & silo,Real interval)59 UserInputHandler(Visualizer::InputSilo& silo, Real interval)
60 : PeriodicEventHandler(interval), m_silo(silo) {}
61
handleEvent(State & state,Real accuracy,bool & shouldTerminate) const62 virtual void handleEvent(State& state, Real accuracy,
63 bool& shouldTerminate) const override
64 {
65 int menuId, item;
66 if (m_silo.takeMenuPick(menuId, item) && menuId==RunMenuId && item==QuitItem)
67 shouldTerminate = true;
68 }
69
70 private:
71 Visualizer::InputSilo& m_silo;
72 };
73
74
75 static void makeCube(Real h, PolygonalMesh& cube);
76 static void makeTetrahedron(Real r, PolygonalMesh& tet);
77 static void makePyramid(Real baseSideLength, PolygonalMesh& pyramid);
78 static void makeOctahedron(Real radius, PolygonalMesh& pyramid);
79
80
main()81 int main() {
82 try
83 { // Create the system.
84
85 MultibodySystem system;
86 SimbodyMatterSubsystem matter(system);
87 GeneralForceSubsystem forces(system);
88 Force::Gravity gravity(forces, matter, UnitVec3(2,-10,0), 1);
89
90 ContactTrackerSubsystem tracker(system);
91 CompliantContactSubsystem contactForces(system, tracker);
92 contactForces.setTrackDissipatedEnergy(true);
93 contactForces.setTransitionVelocity(1e-3);
94
95 Vec3 halfSize(3,4,5);
96 Vec3 halfSize2(200, 4, 200);
97 ContactGeometry::TriangleMesh box(PolygonalMesh::createBrickMesh(halfSize));
98 ContactGeometry::TriangleMesh box2(PolygonalMesh::createBrickMesh(halfSize2, 20));
99 DecorativeMesh showBox(box.createPolygonalMesh());
100 DecorativeMesh showBox2(box2.createPolygonalMesh());
101
102 const Real boxMass = halfSize[0] * halfSize[1] * halfSize[2] * 8;
103 const Real boxMass2 = halfSize2[0] * halfSize2[1] * halfSize2[2] * 8;
104 Body::Rigid boxBody(MassProperties(boxMass, Vec3(0),
105 boxMass * UnitInertia::brick(halfSize)));
106 Body::Rigid boxBody2(MassProperties(boxMass2, Vec3(0),
107 boxMass2 * UnitInertia::brick(halfSize2)));
108 boxBody.addDecoration(Transform(),
109 showBox.setColor(Red).setOpacity(1));
110 boxBody.addDecoration(Transform(),
111 showBox.setColor(Gray).setRepresentation(DecorativeGeometry::DrawWireframe));
112 boxBody2.addDecoration(Transform(),
113 showBox2.setColor(Cyan).setOpacity(.6));
114 boxBody2.addDecoration(Transform(),
115 showBox2.setColor(Gray).setRepresentation(DecorativeGeometry::DrawWireframe));
116 // boxBody.addDecoration(Transform(),
117 // DecorativeSphere(1).setColor(Gray).setOpacity(.1).setResolution(10));
118
119 const Real fFac = 0.3; // to turn off friction
120 const Real fDis = 0.1; // to turn off dissipation
121 const Real fVis = 0.01; // to turn off viscous friction
122 const Real fK = 1e+8; // pascals
123
124 boxBody.addContactSurface(Transform(),
125 ContactSurface(box,
126 ContactMaterial(fK, fDis, fFac, fFac, fVis),
127 .5 /*thickness*/)
128 );
129 boxBody2.addContactSurface(Transform(),
130 ContactSurface(box2,
131 ContactMaterial(fK, fDis, fFac, fFac, fVis),
132 .5 /*thickness*/)
133 );
134 MobilizedBody::Free boxMBody(matter.Ground(), Transform(Vec3(0)), boxBody, Transform(Vec3(0)));
135
136 MobilizedBody::Weld boxMBody2(matter.Ground(), Transform(Vec3(0)), boxBody2, Transform(Vec3(0)));
137
138 Visualizer viz(system);
139 // viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
140 viz.setMode(Visualizer::RealTime);
141 viz.setDesiredBufferLengthInSec(1);
142 viz.setDesiredFrameRate(FrameRate);
143 viz.setGroundHeight(0);
144 viz.setShowShadows(true);
145
146 Visualizer::InputSilo* silo = new Visualizer::InputSilo();
147 viz.addInputListener(silo);
148 Array_<std::pair<String,int> > runMenuItems;
149 runMenuItems.push_back(std::make_pair("Go", GoItem));
150 runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
151 runMenuItems.push_back(std::make_pair("Quit", QuitItem));
152 viz.addMenu("Run", RunMenuId, runMenuItems);
153
154 Array_<std::pair<String,int> > helpMenuItems;
155 helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
156 viz.addMenu("Help", HelpMenuId, helpMenuItems);
157
158 // system.addEventReporter(new MyReporter(system,contactForces,ReportInterval));
159 system.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
160
161 // Check for a Run->Quit menu pick every 1/4 second.
162 system.addEventHandler(new UserInputHandler(*silo, .25));
163
164 // Initialize the system and state.
165
166 system.realizeTopology();
167 State state = system.getDefaultState();
168 //ball.setQToFitTransform(state, Transform(Rotation(Pi/2,XAxis),
169 // Vec3(0,-1.8,0)));
170 boxMBody.setQToFitTransform(state, Transform(Vec3(0, 10, 0)));
171 boxMBody2.setQToFitTransform(state, Transform(Vec3(0, 0, 0)));
172
173 viz.report(state);
174
175 cout << "\nChoose 'Go' from Run menu to simulate:\n";
176 int menuId, item;
177 do { silo->waitForMenuPick(menuId, item);
178 if (menuId != RunMenuId || item != GoItem)
179 cout << "\aDude ... follow instructions!\n";
180 } while (menuId != RunMenuId || item != GoItem);
181
182 //ball.setOneU(state, 2, -20);
183
184 // ball.setOneU(state, 0, .05); // to break symmetry
185
186 CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
187 integ.setAccuracy(1e-3); // minimum for CPodes
188 TimeStepper ts(system, integ);
189
190 ts.initialize(state);
191
192 double cpuStart = cpuTime();
193 double realStart = realTime();
194
195 ts.stepTo(100.0);
196
197 const double timeInSec = realTime() - realStart;
198 const int evals = integ.getNumRealizations();
199 cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
200 timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step="
201 << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
202 << (1000*ts.getTime())/evals << "ms/eval\n";
203 cout << " CPU time was " << cpuTime() - cpuStart << "s\n";
204
205 printf("Using Integrator %s at accuracy %g:\n",
206 integ.getMethodName(), integ.getAccuracyInUse());
207 printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
208 printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
209 printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
210
211 viz.dumpStats(std::cout);
212
213 // Add as slider to control playback speed.
214 viz.addSlider("Speed", 1, 0, 4, 1);
215 viz.setMode(Visualizer::PassThrough);
216
217 silo->clear(); // forget earlier input
218 double speed = 1; // will change if slider moves
219 while(true) {
220 cout << "Choose Run/Replay to see that again ...\n";
221
222 int menuId, item;
223 silo->waitForMenuPick(menuId, item);
224
225
226 if (menuId != RunMenuId) {
227 cout << "\aUse the Run menu!\n";
228 continue;
229 }
230
231 if (item == QuitItem)
232 break;
233 if (item != ReplayItem) {
234 cout << "\aHuh? Try again.\n";
235 continue;
236 }
237
238 for (double i=0; i < (int)saveEm.size(); i += speed ) {
239 int slider; Real newValue;
240 if (silo->takeSliderMove(slider,newValue)) {
241 speed = newValue;
242 }
243 viz.report(saveEm[(int)i]);
244 }
245 }
246
247 } catch (const std::exception& e) {
248 std::printf("EXCEPTION THROWN: %s\n", e.what());
249 exit(1);
250
251 } catch (...) {
252 std::printf("UNKNOWN EXCEPTION THROWN\n");
253 exit(1);
254 }
255
256 return 0;
257 }
258
259
260 // Create a triangle mesh in the shape of a pyramid, with the
261 // square base in the x-z plane centered at 0,0,0 of given side length s.
262 // The base is split into two triangles. The apex will be at (0,s,0).
makePyramid(Real s,PolygonalMesh & pyramidMesh)263 static void makePyramid(Real s, PolygonalMesh& pyramidMesh) {
264 const Real h = s/2;
265 Array_<Vec3> vertices;
266 vertices.push_back(Vec3(-h, 0, -h)); // base
267 vertices.push_back(Vec3( h, 0, -h));
268 vertices.push_back(Vec3( h, 0, h));
269 vertices.push_back(Vec3(-h, 0, h));
270 vertices.push_back(Vec3( 0, s, 0)); // apex
271 Array_<int> faceIndices;
272 int faces[6][3] = {{0, 1, 2}, {0, 2, 3}, {1, 0, 4},
273 {2, 1, 4}, {3, 2, 4}, {0, 3, 4}};
274 for (int i = 0; i < 6; i++)
275 for (int j = 0; j < 3; j++)
276 faceIndices.push_back(faces[i][j]);
277
278 for (unsigned i=0; i < vertices.size(); ++i)
279 pyramidMesh.addVertex(vertices[i]);
280 for (unsigned i=0; i < faceIndices.size(); i += 3) {
281 const Array_<int> verts(&faceIndices[i], &faceIndices[i]+3);
282 pyramidMesh.addFace(verts);
283 }
284 }
285
286
287 // Create a triangle mesh in the shape of a tetrahedron with the
288 // points in the corners of a cube inscribed in a sphere of radius r.
makeTetrahedron(Real r,PolygonalMesh & tet)289 static void makeTetrahedron(Real r, PolygonalMesh& tet) {
290 const Real h = r/std::sqrt(Real(3)); // half-dim of cube
291 Array_<Vec3> vertices;
292 vertices.push_back(Vec3( h, h, h));
293 vertices.push_back(Vec3(-h,-h, h));
294 vertices.push_back(Vec3(-h, h, -h));
295 vertices.push_back(Vec3( h,-h, -h));
296 Array_<int> faceIndices;
297 int faces[4][3] = {{0, 2, 1}, {1, 3, 0}, {0, 3, 2}, {2, 3, 1}};
298 for (int i = 0; i < 4; i++)
299 for (int j = 0; j < 3; j++)
300 faceIndices.push_back(faces[i][j]);
301
302 for (unsigned i=0; i < vertices.size(); ++i)
303 tet.addVertex(vertices[i]);
304 for (unsigned i=0; i < faceIndices.size(); i += 3) {
305 const Array_<int> verts(&faceIndices[i], &faceIndices[i]+3);
306 tet.addFace(verts);
307 }
308 }
309
makeOctahedralMesh(const Vec3 & r,Array_<Vec3> & vertices,Array_<int> & faceIndices)310 static void makeOctahedralMesh(const Vec3& r, Array_<Vec3>& vertices,
311 Array_<int>& faceIndices) {
312 vertices.push_back(Vec3( r[0], 0, 0)); //0
313 vertices.push_back(Vec3(-r[0], 0, 0)); //1
314 vertices.push_back(Vec3( 0, r[1], 0)); //2
315 vertices.push_back(Vec3( 0, -r[1], 0)); //3
316 vertices.push_back(Vec3( 0, 0, r[2])); //4
317 vertices.push_back(Vec3( 0, 0, -r[2])); //5
318 int faces[8][3] = {{0, 2, 4}, {4, 2, 1}, {1, 2, 5}, {5, 2, 0},
319 {4, 3, 0}, {1, 3, 4}, {5, 3, 1}, {0, 3, 5}};
320 for (int i = 0; i < 8; i++)
321 for (int j = 0; j < 3; j++)
322 faceIndices.push_back(faces[i][j]);
323 }
324
325 // Create a triangle mesh in the shape of an octahedron (like two
326 // pyramids stacked base-to-base, with the square base in the x-z plane
327 // centered at 0,0,0 of given "radius" r.
328 // The apexes will be at (0,+/-r,0).
makeOctahedron(Real r,PolygonalMesh & mesh)329 static void makeOctahedron(Real r, PolygonalMesh& mesh) {
330 Array_<Vec3> vertices;
331 Array_<int> faceIndices;
332 makeOctahedralMesh(Vec3(r), vertices, faceIndices);
333
334 for (unsigned i=0; i < vertices.size(); ++i)
335 mesh.addVertex(vertices[i]);
336 for (unsigned i=0; i < faceIndices.size(); i += 3) {
337 const Array_<int> verts(&faceIndices[i], &faceIndices[i]+3);
338 mesh.addFace(verts);
339 }
340 }
341
makeCube(Real h,PolygonalMesh & cube)342 static void makeCube(Real h, PolygonalMesh& cube) {
343 Array_<Vec3> vertices;
344 vertices.push_back(Vec3( h, h, h));
345 vertices.push_back(Vec3( h, h, -h));
346 vertices.push_back(Vec3( h,-h, h));
347 vertices.push_back(Vec3( h,-h, -h));
348 vertices.push_back(Vec3(-h, h, h));
349 vertices.push_back(Vec3(-h, h, -h));
350 vertices.push_back(Vec3(-h,-h, h));
351 vertices.push_back(Vec3(-h,-h, -h));
352
353 Array_<int> faceIndices;
354 int faces[6][4] = {{0,2,3,1},{1,5,4,0},{0,4,6,2},
355 {2,6,7,3},{3,7,5,1},{4,5,7,6}};
356 for (int i = 0; i < 6; i++)
357 for (int j = 0; j < 4; j++)
358 faceIndices.push_back(faces[i][j]);
359
360 for (unsigned i=0; i < vertices.size(); ++i)
361 cube.addVertex(vertices[i]);
362 for (unsigned i=0; i < faceIndices.size(); i += 4) {
363 const Array_<int> verts(&faceIndices[i], &faceIndices[i]+4);
364 cube.addFace(verts);
365 }
366 }
367
368
369