1 /* -------------------------------------------------------------------------- *
2 * Simbody(tm) Adhoc test: Contact Brick Test *
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) 2014 Stanford University and the Authors. *
10 * Authors: 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 /* This adhoc test is for playing with Brick contact elements.
25 */
26
27 #include "Simbody.h"
28
29 #include <cstdio>
30 #include <exception>
31 #include <algorithm>
32 #include <iostream>
33 #include <fstream>
34 using std::cout; using std::endl;
35
36 using namespace SimTK;
37
38 Array_<State> saveEm;
39
40 static const Real TimeScale = 1;
41 static const Real FrameRate = 60;
42 static const Real ReportInterval = TimeScale/FrameRate;
43 static const Real ForceScale = .25;
44 static const Real MomentScale = .5;
45
46
47 class ForceArrowGenerator : public DecorationGenerator {
48 public:
ForceArrowGenerator(const MultibodySystem & system,const CompliantContactSubsystem & complCont)49 ForceArrowGenerator(const MultibodySystem& system,
50 const CompliantContactSubsystem& complCont)
51 : m_mbs(system), m_compliant(complCont) {}
52
generateDecorations(const State & state,Array_<DecorativeGeometry> & geometry)53 virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) override {
54 const Vec3 frcColors[] = {Red,Orange,Cyan};
55 const Vec3 momColors[] = {Blue,Green,Purple};
56 m_mbs.realize(state, Stage::Velocity);
57 const SimbodyMatterSubsystem& matter = m_mbs.getMatterSubsystem();
58 const Real TextScale = m_mbs.getDefaultLengthScale()/10; // was .1
59 m_mbs.realize(state, Stage::Dynamics);
60 const Real KE=m_mbs.calcKineticEnergy(state), E=m_mbs.calcEnergy(state);
61 const Real diss=m_compliant.getDissipatedEnergy(state);
62 DecorativeText txt; txt.setIsScreenText(true);
63 txt.setText("KE/Diss/E: " + String(KE, "%.6f") + String(diss, "/%.6f")
64 + String(E+diss, "/%.6f") );
65 geometry.push_back(txt);
66
67 int nContPts = 0;
68 const int ncont = m_compliant.getNumContactForces(state);
69 for (int i=0; i < ncont; ++i) {
70 const ContactForce& force = m_compliant.getContactForce(state,i);
71 const ContactId id = force.getContactId();
72 const Vec3& pt = force.getContactPoint();
73 const Vec3& frc = force.getForceOnSurface2()[1];
74 const Vec3& mom = force.getForceOnSurface2()[0];
75 Real frcMag = frc.norm(), momMag=mom.norm();
76 const UnitVec3 frcDir(frc/frcMag, true);
77 const UnitVec3 momDir(mom/momMag, true);
78 const Vec3 offs = .1*frcDir; // shift up to clear ground
79 int frcThickness = 2, momThickness = 2;
80 Real frcScale = ForceScale, momScale = ForceScale;
81 while (frcMag > /*10*/1000000)
82 frcThickness++, frcScale /= 10, frcMag /= 10;
83 while (momMag > /*10*/1000000)
84 momThickness++, momScale /= 10, momMag /= 10;
85 geometry.push_back(DecorativePoint(pt)
86 .setScale(5).setColor(Yellow));
87 DecorativeLine frcLine(pt, pt + std::log10(frcMag)*frcDir);
88 DecorativeLine momLine(pt+offs, pt+offs + std::log10(momMag)*momDir);
89 frcLine.setColor(Black);
90 momLine.setColor(Purple);
91 frcLine.setLineThickness(frcThickness);
92 momLine.setLineThickness(momThickness);
93 geometry.push_back(frcLine);
94 geometry.push_back(momLine);
95
96 ContactPatch patch;
97 const bool found = m_compliant.calcContactPatchDetailsById(state,id,patch);
98 //cout << "patch for id" << id << " found=" << found << endl;
99 //cout << "resultant=" << patch.getContactForce() << endl;
100 //cout << "num details=" << patch.getNumDetails() << endl;
101 for (int i=0; i < patch.getNumDetails(); ++i) {
102 ++nContPts;
103 const ContactDetail& detail = patch.getContactDetail(i);
104 const Vec3& pt = detail.getContactPoint();
105 geometry.push_back(DecorativePoint(pt).setColor(Purple));
106
107 const Vec3& force = detail.getForceOnSurface2();
108 const Real forceMag = force.norm();
109 const UnitVec3 forceDir(force/forceMag, true);
110 DecorativeLine frcLine(pt, pt+std::log10(forceMag)*forceDir);
111 frcLine.setColor(Black);
112 geometry.push_back(frcLine);
113
114 // Make a red line that extends from the contact
115 // point in the direction of the slip velocity, of length 3*slipvel.
116 DecorativeLine slip(pt,pt+3.*detail.getSlipVelocity());
117 slip.setColor(Red);
118 geometry.push_back(slip);
119 }
120 }
121 txt.setText(String("Num contact points: ") + String(nContPts));
122 geometry.push_back(txt);
123
124 }
125 private:
126 const MultibodySystem& m_mbs;
127 const CompliantContactSubsystem& m_compliant;
128 };
129
130 class MyReporter : public PeriodicEventReporter {
131 public:
MyReporter(const MultibodySystem & system,const CompliantContactSubsystem & complCont,Real reportInterval)132 MyReporter(const MultibodySystem& system,
133 const CompliantContactSubsystem& complCont,
134 Real reportInterval)
135 : PeriodicEventReporter(reportInterval), m_system(system),
136 m_compliant(complCont)
137 {}
138
~MyReporter()139 ~MyReporter() {}
140
handleEvent(const State & state) const141 void handleEvent(const State& state) const override {
142 saveEm.push_back(state);
143 }
144 private:
145 const MultibodySystem& m_system;
146 const CompliantContactSubsystem& m_compliant;
147 };
148
149 // These are the item numbers for the entries on the Run menu.
150 static const int RunMenuId = 3, HelpMenuId = 7;
151 static const int GoItem = 1, ReplayItem=2, QuitItem=3;
152
153 // This is a periodic event handler that interrupts the simulation on a regular
154 // basis to poll the InputSilo for user input. If there has been some, process it.
155 // This one does nothing but look for the Run->Quit selection.
156 class UserInputHandler : public PeriodicEventHandler {
157 public:
UserInputHandler(Visualizer::InputSilo & silo,Real interval)158 UserInputHandler(Visualizer::InputSilo& silo, Real interval)
159 : PeriodicEventHandler(interval), m_silo(silo) {}
160
handleEvent(State & state,Real accuracy,bool & shouldTerminate) const161 virtual void handleEvent(State& state, Real accuracy,
162 bool& shouldTerminate) const override
163 {
164 int menuId, item;
165 if (m_silo.takeMenuPick(menuId, item) && menuId==RunMenuId && item==QuitItem)
166 shouldTerminate = true;
167 }
168
169 private:
170 Visualizer::InputSilo& m_silo;
171 };
172
main()173 int main() {
174 try
175 { // Create the system.
176
177 MultibodySystem system;
178 SimbodyMatterSubsystem matter(system);
179 GeneralForceSubsystem forces(system);
180 Force::Gravity gravity(forces, matter, UnitVec3(.1,-1,0), 9.81);
181
182 ContactTrackerSubsystem tracker(system);
183 CompliantContactSubsystem contactForces(system, tracker);
184 contactForces.setTrackDissipatedEnergy(true);
185 contactForces.setTransitionVelocity(1e-3);
186
187 const Vec3 hdim(1,2,3);
188
189 const Real fFac =.15; // to turn off friction
190 const Real fDis = .5; // to turn off dissipation
191 const Real fVis = .1; // to turn off viscous friction
192 const Real fK = .1*1e6; // pascals
193
194 // Halfspace floor
195 const Rotation R_xdown(-Pi/2,ZAxis);
196 matter.Ground().updBody().addDecoration(
197 Transform(Vec3(0,-.5,0)),
198 DecorativeBrick(Vec3(10,.5,20)).setColor(Green).setOpacity(.1));
199 matter.Ground().updBody().addContactSurface(
200 Transform(R_xdown, Vec3(0,0,0)),
201 ContactSurface(ContactGeometry::HalfSpace(),
202 ContactMaterial(fK*.1,fDis*.9,
203 fFac*.8,fFac*.7,fVis*10)));
204
205 const Real brickMass = 10;
206 Body::Rigid brickBody(MassProperties(brickMass, Vec3(0),
207 UnitInertia::brick(hdim)));
208 brickBody.addDecoration(Transform(),
209 DecorativeBrick(hdim).setColor(Cyan).setOpacity(.3));
210 const int surfx = brickBody.addContactSurface(Transform(),
211 ContactSurface(ContactGeometry::Brick(hdim),
212 ContactMaterial(fK,fDis,
213 fFac*.8,fFac*.7,fVis))
214 );
215 //brickBody.addContactSurface(Transform(),
216 // ContactSurface(ContactGeometry::Ellipsoid(hdim),
217 // ContactMaterial(fK*.1,fDis*.9,
218 // .1*fFac*.8,.1*fFac*.7,fVis*1))
219 // );
220 const ContactSurface& surf = brickBody.getContactSurface(surfx);
221 const ContactGeometry& cg = surf.getShape();
222 const ContactGeometry::Brick& cgbrick = ContactGeometry::Brick::getAs(cg);
223 cout << "cgbrick.hdim=" << cgbrick.getHalfLengths() << endl;
224 const Geo::Box& box = cgbrick.getGeoBox();
225 cout << "box.hdim=" << box.getHalfLengths() << endl;
226
227 // Vertices
228 for (int i=0; i<8; ++i) {
229 const Vec3 vpos = box.getVertexPos(i);
230 const UnitVec3 vn = box.getVertexNormal(i);
231 brickBody.addDecoration
232 (DecorativePoint(vpos).setColor(Orange));
233 brickBody.addDecoration
234 (DecorativeText(String(i)).setTransform(vpos).setColor(White)
235 .setScale(.5));
236 brickBody.addDecoration
237 (DecorativeLine(vpos, vpos + 0.5*vn).setColor(Orange));
238
239 printf("vertex %d:\n", i);
240 int e[3],ew[3],f[3],fw[3];
241 box.getVertexEdges(i,e,ew);
242 box.getVertexFaces(i,f,fw);
243 for (int ex=0; ex<3; ++ex) {
244 int ev[2]; box.getEdgeVertices(e[ex], ev);
245 printf(" e%2d(%d) ev=%d\n", e[ex], ew[ex], ev[ew[ex]]);
246 }
247 for (int fx=0; fx<3; ++fx) {
248 int fv[4]; box.getFaceVertices(f[fx], fv);
249 printf(" f%2d(%d) fv=%d\n", f[fx], fw[fx], fv[fw[fx]]);
250 }
251 }
252
253 // Edges
254 for (int i=0; i<12; ++i) {
255 const UnitVec3 n = box.getEdgeNormal(i);
256 const UnitVec3 d = box.getEdgeDirection(i);
257 const Vec3 ctr = box.getEdgeCenter(i);
258 const Real len = .75;
259 brickBody.addDecoration
260 (DecorativePoint(ctr).setColor(Green).setScale(2));
261 brickBody.addDecoration
262 (DecorativeText(String(i)).setTransform(ctr+len*n)
263 .setColor(Green).setScale(.3));
264 brickBody.addDecoration
265 (DecorativeLine(ctr, ctr + len*n).setColor(Green));
266 brickBody.addDecoration
267 (DecorativeLine(ctr, ctr + len*d).setColor(Green));
268
269 printf("edge %d:\n", i);
270 int f[2],fw[2];
271 box.getEdgeFaces(i,f,fw);
272 for (int fx=0; fx<2; ++fx) {
273 int fe[4]; box.getFaceEdges(f[fx], fe);
274 printf(" f%2d(%d) fe=%d\n", f[fx], fw[fx], fe[fw[fx]]);
275 }
276 }
277
278 // Faces
279 for (int i=0; i<6; ++i) {
280 int vertices[4]; box.getFaceVertices(i,vertices);
281 const UnitVec3 n = box.getFaceNormal(i);
282 const Vec3 ctr = box.getFaceCenter(i);
283 brickBody.addDecoration
284 (DecorativePoint(ctr).setColor(Magenta).setScale(3));
285 brickBody.addDecoration
286 (Transform(Rotation(n,ZAxis,Vec3(0,1,0),YAxis),ctr),
287 DecorativeText(String(i)).setColor(Magenta)
288 .setScale(.75).setFaceCamera(false));
289 brickBody.addDecoration
290 (DecorativeLine(ctr, ctr + 1.*n).setColor(Magenta));
291 }
292
293 MobilizedBody::Free brick(matter.Ground(), Transform(Vec3(0,3,0)),
294 brickBody, Transform(Vec3(0)));
295
296 Visualizer viz(system);
297 viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
298 viz.setShowShadows(true);
299 viz.setShowSimTime(true);
300 viz.setDesiredFrameRate(FrameRate);
301 viz.setShowFrameRate(true);
302 viz.setBackgroundType(Visualizer::SolidColor);
303 viz.setBackgroundColor(White*.9);
304
305 Visualizer::InputSilo* silo = new Visualizer::InputSilo();
306 viz.addInputListener(silo);
307 Array_<std::pair<String,int> > runMenuItems;
308 runMenuItems.push_back(std::make_pair("Go", GoItem));
309 runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
310 runMenuItems.push_back(std::make_pair("Quit", QuitItem));
311 viz.addMenu("Run", RunMenuId, runMenuItems);
312
313 Array_<std::pair<String,int> > helpMenuItems;
314 helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
315 viz.addMenu("Help", HelpMenuId, helpMenuItems);
316
317 system.addEventReporter(new MyReporter(system,contactForces,ReportInterval));
318 system.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
319
320 // Check for a Run->Quit menu pick every 1/4 second.
321 system.addEventHandler(new UserInputHandler(*silo, .25));
322
323 // Initialize the system and state.
324
325 system.realizeTopology();
326 State state = system.getDefaultState();
327
328 brick.setQToFitRotation(state, Rotation(SpaceRotationSequence,
329 .1, ZAxis, .05, XAxis));
330 brick.setUToFitLinearVelocity(state, Vec3(2,0,0));
331
332 saveEm.reserve(10000);
333
334 viz.report(state);
335 printf("Default state\n");
336 cout << "t=" << state.getTime()
337 << " q=" << brick.getQAsVector(state)
338 << " u=" << brick.getUAsVector(state)
339 << endl;
340
341 cout << "\nChoose 'Go' from Run menu to simulate:\n";
342 int menuId, item;
343 do { silo->waitForMenuPick(menuId, item);
344 if (menuId != RunMenuId || item != GoItem)
345 cout << "\aDude ... follow instructions!\n";
346 } while (menuId != RunMenuId || item != GoItem);
347
348
349 // Simulate it.
350
351 // The system as parameterized is very stiff (mostly due to friction)
352 // and thus runs best with CPodes which is extremely stable for
353 // stiff problems. To get reasonable performance out of the explicit
354 // integrators (like the RKs) you'll have to run at a very loose
355 // accuracy like 0.1, or reduce the friction coefficients and
356 // maybe the stiffnesses.
357
358 //SemiExplicitEuler2Integrator integ(system);
359 //CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
360 RungeKuttaMersonIntegrator integ(system);
361 //RungeKutta3Integrator integ(system);
362 //VerletIntegrator integ(system);
363 //integ.setMaximumStepSize(1e-0001);
364 //integ.setAccuracy(1e-3); // minimum for CPodes
365 integ.setAccuracy(1e-5);
366 //integ.setAccuracy(.01);
367 TimeStepper ts(system, integ);
368
369
370 ts.initialize(state);
371 double cpuStart = cpuTime();
372 double realStart = realTime();
373
374 ts.stepTo(20.0);
375
376 const double timeInSec = realTime() - realStart;
377 const int evals = integ.getNumRealizations();
378 cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
379 timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step="
380 << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
381 << (1000*ts.getTime())/evals << "ms/eval\n";
382 cout << " CPU time was " << cpuTime() - cpuStart << "s\n";
383
384 printf("Using Integrator %s at accuracy %g:\n",
385 integ.getMethodName(), integ.getAccuracyInUse());
386 printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
387 printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
388 printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
389
390 viz.dumpStats(std::cout);
391
392 // Add as slider to control playback speed.
393 viz.addSlider("Speed", 1, 0, 4, 1);
394 viz.setMode(Visualizer::PassThrough);
395
396 silo->clear(); // forget earlier input
397 double speed = 1; // will change if slider moves
398 while(true) {
399 cout << "Choose Run/Replay to see that again ...\n";
400
401 int menuId, item;
402 silo->waitForMenuPick(menuId, item);
403
404
405 if (menuId != RunMenuId) {
406 cout << "\aUse the Run menu!\n";
407 continue;
408 }
409
410 if (item == QuitItem)
411 break;
412 if (item != ReplayItem) {
413 cout << "\aHuh? Try again.\n";
414 continue;
415 }
416
417 for (double i=0; i < (int)saveEm.size(); i += speed ) {
418 int slider; Real newValue;
419 if (silo->takeSliderMove(slider,newValue)) {
420 speed = newValue;
421 }
422 viz.report(saveEm[(int)i]);
423 }
424 }
425
426 } catch (const std::exception& e) {
427 std::printf("EXCEPTION THROWN: %s\n", e.what());
428 exit(1);
429
430 } catch (...) {
431 std::printf("UNKNOWN EXCEPTION THROWN\n");
432 exit(1);
433 }
434
435 return 0;
436 }
437
438