1 /* -------------------------------------------------------------------------- *
2 * Simbody(tm) Example: Rattleback *
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) 2011-12 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 example is for experimenting with ellipsoid contact which was
25 introduced in Simbody 2.2.
26 */
27
28 #include "Simbody.h"
29
30 #include <cstdio>
31 #include <exception>
32 #include <algorithm>
33 #include <iostream>
34 #include <fstream>
35 using std::cout; using std::endl;
36
37 using namespace SimTK;
38
39 Array_<State> saveEm;
40
41
42 const Real Cm2m = 1e-2;
43 const Real CmSq2mSq = Cm2m*Cm2m; // conversion from sq cm to sq m
44 const Real Deg2Rad = (Real)SimTK_DEGREE_TO_RADIAN;
45 const Real Rad2Deg = (Real)SimTK_RADIAN_TO_DEGREE;
46
47 static const Real TimeScale = 1;
48 static const Real FrameRate = 30;
49 static const Real ReportInterval = TimeScale/FrameRate;
50 static const Real ForceScale = .25;
51 static const Real MomentScale = .5;
52
53
54 class ForceArrowGenerator : public DecorationGenerator {
55 public:
ForceArrowGenerator(const MultibodySystem & system,const CompliantContactSubsystem & complCont)56 ForceArrowGenerator(const MultibodySystem& system,
57 const CompliantContactSubsystem& complCont)
58 : m_system(system), m_compliant(complCont) {}
59
generateDecorations(const State & state,Array_<DecorativeGeometry> & geometry)60 virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) override {
61 const Vec3 frcColors[] = {Red,Orange,Cyan};
62 const Vec3 momColors[] = {Blue,Green,Purple};
63 m_system.realize(state, Stage::Velocity);
64
65 const int ncont = m_compliant.getNumContactForces(state);
66 for (int i=0; i < ncont; ++i) {
67 const ContactForce& force = m_compliant.getContactForce(state,i);
68 const ContactId id = force.getContactId();
69 const Vec3& frc = force.getForceOnSurface2()[1];
70 const Vec3& mom = force.getForceOnSurface2()[0];
71 Real frcMag = frc.norm(), momMag=mom.norm();
72 int frcThickness = 1, momThickness = 1;
73 Real frcScale = ForceScale, momScale = ForceScale;
74 while (frcMag > 10)
75 frcThickness++, frcScale /= 10, frcMag /= 10;
76 while (momMag > 10)
77 momThickness++, momScale /= 10, momMag /= 10;
78 DecorativeLine frcLine(force.getContactPoint(),
79 force.getContactPoint() + frcScale*frc);
80 DecorativeLine momLine(force.getContactPoint(),
81 force.getContactPoint() + momScale*mom);
82 frcLine.setColor(frcColors[id%3]);
83 momLine.setColor(momColors[id%3]);
84 frcLine.setLineThickness(2*frcThickness);
85 momLine.setLineThickness(2*momThickness);
86 geometry.push_back(frcLine);
87 geometry.push_back(momLine);
88
89 ContactPatch patch;
90 const bool found = m_compliant.calcContactPatchDetailsById(state,id,patch);
91 //cout << "patch for id" << id << " found=" << found << endl;
92 //cout << "resultant=" << patch.getContactForce() << endl;
93 //cout << "num details=" << patch.getNumDetails() << endl;
94 for (int i=0; i < patch.getNumDetails(); ++i) {
95 const ContactDetail& detail = patch.getContactDetail(i);
96 const Real peakPressure = detail.getPeakPressure();
97 // Make a black line from the element's contact point in the normal
98 // direction, with length proportional to log(peak pressure)
99 // on that element.
100 DecorativeLine normal(detail.getContactPoint(),
101 detail.getContactPoint()+ std::log10(peakPressure)
102 * detail.getContactNormal());
103 normal.setColor(Black);
104 geometry.push_back(normal);
105 // Make a red line that extends from the contact
106 // point in the direction of the slip velocity, of length 3*slipvel.
107 DecorativeLine slip(detail.getContactPoint(),
108 detail.getContactPoint()+3*detail.getSlipVelocity());
109 slip.setColor(Red);
110 geometry.push_back(slip);
111 }
112 }
113 }
114 private:
115 const MultibodySystem& m_system;
116 const CompliantContactSubsystem& m_compliant;
117 };
118
119 class MyReporter : public PeriodicEventReporter {
120 public:
MyReporter(const MultibodySystem & system,const CompliantContactSubsystem & complCont,Real reportInterval)121 MyReporter(const MultibodySystem& system,
122 const CompliantContactSubsystem& complCont,
123 Real reportInterval)
124 : PeriodicEventReporter(reportInterval), m_system(system),
125 m_compliant(complCont)
126 {}
127
~MyReporter()128 ~MyReporter() {}
129
handleEvent(const State & state) const130 void handleEvent(const State& state) const override {
131 m_system.realize(state, Stage::Dynamics);
132 cout << state.getTime() << ": E = " << m_system.calcEnergy(state)
133 << " Ediss=" << m_compliant.getDissipatedEnergy(state)
134 << " E+Ediss=" << m_system.calcEnergy(state)
135 +m_compliant.getDissipatedEnergy(state)
136 << endl;
137 cout << " q0(Deg): " << state.getQ()[0]*Rad2Deg << endl;
138 const int ncont = m_compliant.getNumContactForces(state);
139 cout << "Num contacts: " << m_compliant.getNumContactForces(state) << endl;
140 for (int i=0; i < ncont; ++i) {
141 const ContactForce& force = m_compliant.getContactForce(state,i);
142 //cout << force;
143 }
144 saveEm.push_back(state);
145 }
146 private:
147 const MultibodySystem& m_system;
148 const CompliantContactSubsystem& m_compliant;
149 };
150
151 // These are the item numbers for the entries on the Run menu.
152 static const int RunMenuId = 3, HelpMenuId = 7;
153 static const int GoItem = 1, ReplayItem=2, QuitItem=3;
154
155 // This is a periodic event handler that interrupts the simulation on a regular
156 // basis to poll the InputSilo for user input. If there has been some, process it.
157 // This one does nothing but look for the Run->Quit selection.
158 class UserInputHandler : public PeriodicEventHandler {
159 public:
UserInputHandler(Visualizer::InputSilo & silo,Real interval)160 UserInputHandler(Visualizer::InputSilo& silo, Real interval)
161 : PeriodicEventHandler(interval), m_silo(silo) {}
162
handleEvent(State & state,Real accuracy,bool & shouldTerminate) const163 virtual void handleEvent(State& state, Real accuracy,
164 bool& shouldTerminate) const override
165 {
166 int menuId, item;
167 if (m_silo.takeMenuPick(menuId, item) && menuId==RunMenuId && item==QuitItem)
168 shouldTerminate = true;
169 }
170
171 private:
172 Visualizer::InputSilo& m_silo;
173 };
174
main()175 int main() {
176 try
177 { // Create the system.
178
179 MultibodySystem system;
180 SimbodyMatterSubsystem matter(system);
181 GeneralForceSubsystem forces(system);
182 Force::Gravity gravity(forces, matter, UnitVec3(-1,0,0), 9.81);
183
184 ContactTrackerSubsystem tracker(system);
185 CompliantContactSubsystem contactForces(system, tracker);
186 contactForces.setTrackDissipatedEnergy(true);
187 contactForces.setTransitionVelocity(1e-2); // m/s
188
189 // Ground's normal is +x for this model
190 system.setUpDirection(+XAxis);
191
192 // Uncomment this if you want a more elegant movie.
193 //matter.setShowDefaultGeometry(false);
194
195 const Real ud = .3; // dynamic
196 const Real us = .6; // static
197 const Real uv = 0; // viscous (force/velocity)
198 const Real k = 1e8; // pascals
199 const Real c = 0.01; // dissipation (1/v)
200
201
202 // Halfspace default is +x, this one occupies -x instead, so flip.
203 const Rotation R_xdown(Pi,ZAxis);
204
205 matter.Ground().updBody().addContactSurface(
206 Transform(R_xdown, Vec3(0,0,0)),
207 ContactSurface(ContactGeometry::HalfSpace(),
208 ContactMaterial(k,c,us,ud,uv)));
209
210
211 const Real ellipsoidMass = 1; // kg
212 const Vec3 halfDims(2*Cm2m, 20*Cm2m, 3*Cm2m); // m (read in cm)
213 const Vec3 comLoc(-1*Cm2m, 0, 0);
214 const Inertia centralInertia(Vec3(17,2,16)*CmSq2mSq, Vec3(0,0,.2)*CmSq2mSq); // now kg-m^2
215 const Inertia inertia(centralInertia.shiftFromMassCenter(-comLoc, ellipsoidMass)); // in S
216 Body::Rigid ellipsoidBody(MassProperties(ellipsoidMass, comLoc, inertia));
217
218 ellipsoidBody.addDecoration(Transform(),
219 DecorativeEllipsoid(halfDims).setColor(Cyan)
220 //.setOpacity(.5)
221 .setResolution(3));
222 ellipsoidBody.addContactSurface(Transform(),
223 ContactSurface(ContactGeometry::Ellipsoid(halfDims),
224 ContactMaterial(k,c,us,ud,uv))
225 );
226 MobilizedBody::Free ellipsoid(matter.Ground(), Transform(Vec3(0,0,0)),
227 ellipsoidBody, Transform(Vec3(0)));
228
229
230 Visualizer viz(system);
231 viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
232 viz.setMode(Visualizer::RealTime);
233 viz.setDesiredFrameRate(FrameRate);
234 viz.setCameraClippingPlanes(0.1, 10);
235
236 Visualizer::InputSilo* silo = new Visualizer::InputSilo();
237 viz.addInputListener(silo);
238 Array_<std::pair<String,int> > runMenuItems;
239 runMenuItems.push_back(std::make_pair("Go", GoItem));
240 runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
241 runMenuItems.push_back(std::make_pair("Quit", QuitItem));
242 viz.addMenu("Run", RunMenuId, runMenuItems);
243
244 Array_<std::pair<String,int> > helpMenuItems;
245 helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
246 viz.addMenu("Help", HelpMenuId, helpMenuItems);
247
248 system.addEventReporter(new MyReporter(system,contactForces,ReportInterval));
249 system.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
250
251 // Check for a Run->Quit menu pick every 1/4 second.
252 system.addEventHandler(new UserInputHandler(*silo, .25));
253
254 // Initialize the system and state.
255
256 system.realizeTopology();
257 State state = system.getDefaultState();
258 matter.setUseEulerAngles(state, true);
259 system.realizeModel(state);
260
261 ellipsoid.setQToFitTransform(state, Transform(
262 Rotation(BodyRotationSequence, 0 *Deg2Rad, XAxis,
263 0.5*Deg2Rad, YAxis,
264 -0.5*Deg2Rad, ZAxis),
265 Vec3(2.1*Cm2m, 0, 0)));
266
267 ellipsoid.setUToFitAngularVelocity(state, 2*Vec3(5,0,0)); // rad/s
268
269 viz.report(state);
270 printf("Default state\n");
271
272 cout << "\nChoose 'Go' from Run menu to simulate:\n";
273 int menuId, item;
274 do { silo->waitForMenuPick(menuId, item);
275 if (menuId != RunMenuId || item != GoItem)
276 cout << "\aDude ... follow instructions!\n";
277 } while (menuId != RunMenuId || item != GoItem);
278
279
280
281 // Simulate it.
282
283 //ExplicitEulerIntegrator integ(system);
284 //CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
285 //RungeKuttaFeldbergIntegrator integ(system);
286 RungeKuttaMersonIntegrator integ(system);
287 //RungeKutta3Integrator integ(system);
288 //VerletIntegrator integ(system);
289 //integ.setMaximumStepSize(1e-0001);
290 integ.setAccuracy(1e-4); // minimum for CPodes
291 //integ.setAccuracy(.01);
292 TimeStepper ts(system, integ);
293
294
295 ts.initialize(state);
296 double cpuStart = cpuTime();
297 double realStart = realTime();
298
299 ts.stepTo(10.0);
300
301 const double timeInSec = realTime() - realStart;
302 const int evals = integ.getNumRealizations();
303 cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
304 timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step="
305 << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
306 << (1000*ts.getTime())/evals << "ms/eval\n";
307 cout << " CPU time was " << cpuTime() - cpuStart << "s\n";
308
309 printf("Using Integrator %s at accuracy %g:\n",
310 integ.getMethodName(), integ.getAccuracyInUse());
311 printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
312 printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
313 printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
314
315 viz.dumpStats(std::cout);
316
317 // Add as slider to control playback speed.
318 viz.addSlider("Speed", 1, 0, 4, 1);
319 viz.setMode(Visualizer::PassThrough);
320
321 silo->clear(); // forget earlier input
322 double speed = 1; // will change if slider moves
323 while(true) {
324 cout << "Choose Run/Replay to see that again ...\n";
325
326 int menuId, item;
327 silo->waitForMenuPick(menuId, item);
328
329
330 if (menuId != RunMenuId) {
331 cout << "\aUse the Run menu!\n";
332 continue;
333 }
334
335 if (item == QuitItem)
336 break;
337 if (item != ReplayItem) {
338 cout << "\aHuh? Try again.\n";
339 continue;
340 }
341
342 for (double i=0; i < (int)saveEm.size(); i += speed ) {
343 int slider; Real newValue;
344 if (silo->takeSliderMove(slider,newValue)) {
345 speed = newValue;
346 }
347 viz.report(saveEm[(int)i]);
348 }
349 }
350
351 } catch (const std::exception& e) {
352 std::printf("EXCEPTION THROWN: %s\n", e.what());
353 exit(1);
354
355 } catch (...) {
356 std::printf("UNKNOWN EXCEPTION THROWN\n");
357 exit(1);
358 }
359
360 return 0;
361 }
362