1 /* -------------------------------------------------------------------------- *
2 * Simbody(tm) *
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) 2007-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 /**@file
25 * Throwaway main program.
26 */
27
28 #include "SimTKsimbody.h"
29
30 #include <cmath>
31 #include <cstdio>
32 #include <exception>
33 #include <vector>
34
35 using namespace std;
36 using namespace SimTK;
37
38 static const Real Deg2Rad = (Real)SimTK_DEGREE_TO_RADIAN,
39 Rad2Deg = (Real)SimTK_RADIAN_TO_DEGREE;
40
41 static const Transform GroundFrame;
42
43 static const Real m = 1; // kg
44 static const Real g = 9.8; // meters/s^2; apply in �y direction
45 static const Real d = 0.5; // meters
46
47 static const Vec3 hl(1, 0.5, 0.5); // body half lengths
48
49 class MyConstraintImplementation : public Constraint::Custom::Implementation {
50 public:
MyConstraintImplementation(MobilizedBody & mobilizer,Real speed)51 MyConstraintImplementation(MobilizedBody& mobilizer, Real speed)
52 : Implementation(mobilizer.updMatterSubsystem(), 0,1,0),
53 theMobilizer(), whichMobility(), prescribedSpeed(NaN)
54 {
55 theMobilizer = addConstrainedMobilizer(mobilizer);
56 whichMobility = MobilizerUIndex(0);
57 prescribedSpeed = speed;
58 }
clone() const59 MyConstraintImplementation* clone() const override {return new MyConstraintImplementation(*this);}
60
61 // Implementation of virtuals required for nonholonomic constraints.
62
63 // One non-holonomic (well, velocity-level) constraint equation.
64 // verr = u - s
65 // aerr = udot
66 //
calcVelocityErrors(const State & s,const Array_<SpatialVec,ConstrainedBodyIndex> & V_AB,const Array_<Real,ConstrainedUIndex> & constrainedU,Array_<Real> & verr) const67 void calcVelocityErrors
68 (const State& s,
69 const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
70 const Array_<Real, ConstrainedUIndex>& constrainedU,
71 Array_<Real>& verr) const override
72 {
73 assert(verr.size() == 1);
74 verr[0] = getOneU(s, constrainedU, theMobilizer, whichMobility)
75 - prescribedSpeed;
76 }
77
calcVelocityDotErrors(const State & s,const Array_<SpatialVec,ConstrainedBodyIndex> & A_AB,const Array_<Real,ConstrainedUIndex> & constrainedUDot,Array_<Real> & vaerr) const78 void calcVelocityDotErrors
79 (const State& s,
80 const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
81 const Array_<Real, ConstrainedUIndex>& constrainedUDot,
82 Array_<Real>& vaerr) const override
83 {
84 assert(vaerr.size() == 1);
85 vaerr[0] = getOneUDot(s, constrainedUDot,
86 theMobilizer, whichMobility);
87 }
88
89 // apply generalized force lambda to the mobility
addInVelocityConstraintForcesVirtual(const State & s,const Array_<Real> & multipliers,Array_<SpatialVec,ConstrainedBodyIndex> & bodyForcesInA,Array_<Real,ConstrainedUIndex> & mobilityForces) const90 void addInVelocityConstraintForcesVirtual
91 (const State& s,
92 const Array_<Real>& multipliers,
93 Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
94 Array_<Real,ConstrainedUIndex>& mobilityForces) const
95 {
96 assert(multipliers.size() == 1);
97 const Real lambda = multipliers[0];
98 addInOneMobilityForce(s, theMobilizer, whichMobility,
99 lambda, mobilityForces);
100 }
101
102 private:
103 ConstrainedMobilizerIndex theMobilizer;
104 MobilizerUIndex whichMobility;
105 Real prescribedSpeed;
106 };
107
108 class MyConstraint : public Constraint::Custom {
109 public:
MyConstraint(MobilizedBody & mobilizer,Real speed)110 explicit MyConstraint(MobilizedBody& mobilizer, Real speed)
111 : Custom(new MyConstraintImplementation(mobilizer, speed))
112 {
113 }
114 };
115
116
main(int argc,char ** argv)117 int main(int argc, char** argv) {
118 try { // If anything goes wrong, an exception will be thrown.
119
120 // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
121 MultibodySystem mbs;
122
123 SimbodyMatterSubsystem matter(mbs);
124 GeneralForceSubsystem forces(mbs);
125 DecorationSubsystem viz(mbs);
126 Force::UniformGravity gravity(forces, matter, Vec3(0, -g, 0));
127
128 // ADD BODIES AND THEIR MOBILIZERS
129 Body::Rigid body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::brick(hl[0],hl[1],hl[2])));
130 body.addDecoration(DecorativeBrick(hl).setOpacity(.5));
131 body.addDecoration(DecorativeLine(Vec3(0), Vec3(0,1,0)).setColor(Green));
132
133 MobilizedBody::Free mobilizedBody(matter.Ground(), Transform(), body, Transform());
134 MobilizedBody::Free mobilizedBody0(mobilizedBody, Transform(Vec3(1,2,0)), body, Transform(Vec3(0,1,0)));
135 MobilizedBody::Free mobilizedBody2(mobilizedBody0, Vec3(-5,0,0), body, Transform());
136
137 Body::Rigid gear1body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::cylinderAlongZ(.5, .1)));
138 gear1body.addDecoration(DecorativeCircle(.5).setColor(Green).setOpacity(.7));
139 gear1body.addDecoration(DecorativeLine(Vec3(0), Vec3(.5,0,0)).setColor(Black).setLineThickness(4));
140 Body::Rigid gear2body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::cylinderAlongZ(1.5, .1)));
141 gear2body.addDecoration(Transform(), DecorativeCircle(1.5).setColor(Blue).setOpacity(.7));
142 gear2body.addDecoration(Transform(), DecorativeLine(Vec3(0), Vec3(1.5,0,0)).setColor(Black).setLineThickness(4));
143
144 MobilizedBody::Pin gear1(mobilizedBody2, Vec3(-1,0,0), gear1body, Transform()); // along z
145 MobilizedBody::Pin gear2(mobilizedBody2, Vec3(1,0,0), gear2body, Transform()); // along z
146 Constraint::NoSlip1D(mobilizedBody2, Vec3(-.5,0,0), UnitVec3(0,1,0), gear1, gear2);
147
148 Constraint::ConstantSpeed(gear1, 100.);
149
150 //Constraint::Ball myc2(matter.Ground(), Vec3(-4,2,0), mobilizedBody2, Vec3(0,1,0));
151 Constraint::Weld myc(matter.Ground(), Vec3(1,2,0), mobilizedBody, Vec3(0,1,0));
152 Constraint::Ball ball1(mobilizedBody, Vec3(2,0,0), mobilizedBody0, Vec3(3,1,1));
153 Constraint::Ball ball2(mobilizedBody0, Vec3(2,0,0), mobilizedBody2, Vec3(3,0,0));
154 //Constraint::PointInPlane pip(mobilizedBody0, UnitVec3(0,-1,0), 3, mobilizedBody2, Vec3(3,0,0));
155
156 //Constraint::ConstantOrientation ori(mobilizedBody, Rotation(), mobilizedBody0, Rotation());
157 //Constraint::ConstantOrientation ori2(mobilizedBody2, Rotation(), mobilizedBody0, Rotation());
158 //Constraint::Weld weld(mobilizedBody, Transform(Rotation(Pi/4, ZAxis), Vec3(1,1,0)),
159 // mobilizedBody2, Transform(Rotation(-Pi/4, ZAxis), Vec3(-1,-1,0)));
160
161 //MyConstraint xyz(gear1, 100.);
162
163 viz.addBodyFixedDecoration(mobilizedBody, Transform(Vec3(1,2,3)), DecorativeText("hello world").setScale(.1));
164
165
166
167 /*
168 class MyHandler : public ScheduledEventHandler {
169 public:
170 MyHandler(const Constraint& cons) : c(cons) { }
171 Real getNextEventTime(const State&, bool includeCurrentTime) const {
172 return .314;
173 }
174 void handleEvent(State& s, Real acc, const Vector& ywts, const Vector& cwts, Stage& modified,
175 bool& shouldTerminate) const
176 {
177 cout << "<<<< TRIGGERED AT T=" << s.getTime() << endl;
178 c.enable(s);
179 modified = Stage::Model;
180 }
181 private:
182 const Constraint& c;
183 };
184
185 mbs.addEventHandler(new MyHandler(xyz));
186 */
187
188
189 State s = mbs.realizeTopology(); // returns a reference to the the default state
190
191 //xyz.disable(s);
192
193 //matter.setUseEulerAngles(s, true);
194 mbs.realizeModel(s); // define appropriate states for this System
195
196 //mobilizedBody0.setQ(s, .1);
197 //mobilizedBody.setQ(s, .2);
198
199 Visualizer display(mbs);
200 display.setBackgroundColor(White);
201 display.setBackgroundType(Visualizer::SolidColor);
202
203 mbs.realize(s, Stage::Velocity);
204 display.report(s);
205 cout << "q=" << s.getQ() << endl;
206 cout << "u=" << s.getU() << endl;
207 cout << "qErr=" << s.getQErr() << endl;
208 cout << "uErr=" << s.getUErr() << endl;
209
210 for (ConstraintIndex cid(0); cid < matter.getNumConstraints(); ++cid) {
211 const Constraint& c = matter.getConstraint(cid);
212 int mp,mv,ma;
213 c.getNumConstraintEquationsInUse(s, mp,mv,ma);
214
215 cout << "CONSTRAINT " << cid << (c.isDisabled(s) ? "**DISABLED** " : "")
216 << " constrained bodies=" << c.getNumConstrainedBodies();
217 if (c.getNumConstrainedBodies()) cout << " ancestor=" << c.getAncestorMobilizedBody().getMobilizedBodyIndex();
218 cout << " constrained mobilizers/nq/nu=" << c.getNumConstrainedMobilizers()
219 << "/" << c.getNumConstrainedQ(s) << "/" << c.getNumConstrainedU(s)
220 << " mp,mv,ma=" << mp << "," << mv << "," << ma
221 << endl;
222 for (ConstrainedBodyIndex cid(0); cid < c.getNumConstrainedBodies(); ++cid) {
223 cout << " constrained body: " << c.getMobilizedBodyFromConstrainedBody(cid).getMobilizedBodyIndex();
224 cout << endl;
225 }
226 for (ConstrainedMobilizerIndex cmx(0); cmx < c.getNumConstrainedMobilizers(); ++cmx) {
227 cout << " constrained mobilizer " << c.getMobilizedBodyFromConstrainedMobilizer(cmx).getMobilizedBodyIndex()
228 << ", q(" << c.getNumConstrainedQ(s, cmx) << ")=";
229 for (MobilizerQIndex i(0); i < c.getNumConstrainedQ(s, cmx); ++i)
230 cout << " " << c.getConstrainedQIndex(s, cmx, i);
231 cout << ", u(" << c.getNumConstrainedU(s, cmx) << ")=";
232 for (MobilizerUIndex i(0); i < c.getNumConstrainedU(s, cmx); ++i)
233 cout << " " << c.getConstrainedUIndex(s, cmx, i);
234 cout << endl;
235 }
236 cout << c.getSubtree();
237
238 if (mp) {
239 cout << "perr=" << c.getPositionErrorsAsVector(s) << endl;
240 cout << " d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
241 cout << " ~d(Pt lambda)/dlambda=" << ~c.calcPositionConstraintMatrixPt(s);
242 cout << " d(perr)/dq=" << c.calcPositionConstraintMatrixPNInv(s);
243
244 Matrix P = c.calcPositionConstraintMatrixP(s);
245 Matrix PQ(mp,matter.getNQ(s));
246 Vector out(matter.getNQ(s));
247 for (int i=0; i<mp; ++i) {
248 Vector in = ~P[i];
249 matter.multiplyByNInv(s, true, in, out);
250 PQ[i] = ~out;
251 }
252 cout << " calculated d(perr)/dq=" << PQ;
253 }
254
255
256 if (mv) {
257 cout << "verr=" << c.getVelocityErrorsAsVector(s) << endl;
258 //cout << " d(verrdot)/dudot=" << c.calcVelocityConstraintMatrixV(s);
259 cout << " ~d(Vt lambda)/dlambda=" << ~c.calcVelocityConstraintMatrixVt(s);
260 }
261
262 }
263 const Constraint& c = matter.getConstraint(myc.getConstraintIndex());
264
265 cout << "Default configuration shown. Ready? "; getchar();
266
267 mobilizedBody.setQToFitTransform (s, Transform(Rotation(.05,Vec3(1,1,1)),Vec3(.1,.2,.3)));
268 mobilizedBody0.setQToFitTransform (s, Transform(Rotation(.05,Vec3(1,-1,1)),Vec3(.2,.2,.3)));
269 mobilizedBody2.setQToFitTransform (s, Transform(Rotation(.05,Vec3(-1,1,1)),Vec3(.1,.2,.1)));
270 mobilizedBody.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
271 mobilizedBody0.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
272 mobilizedBody2.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
273
274 //gear1.setUToFitAngularVelocity(s, Vec3(0,0,500)); // these should be opposite directions!
275 //gear2.setUToFitAngularVelocity(s, Vec3(0,0,100));
276
277 mbs.realize(s, Stage::Velocity);
278 display.report(s);
279
280 cout << "q=" << s.getQ() << endl;
281 cout << "u=" << s.getU() << endl;
282 cout << "qErr=" << s.getQErr() << endl;
283 cout << "uErr=" << s.getUErr() << endl;
284 cout << "p_MbM=" << mobilizedBody.getMobilizerTransform(s).p() << endl;
285 cout << "v_MbM=" << mobilizedBody.getMobilizerVelocity(s)[1] << endl;
286 cout << "Unassembled configuration shown. Ready to assemble? "; getchar();
287
288 // These are the SimTK Simmath integrators:
289 RungeKuttaMersonIntegrator myStudy(mbs);
290 //CPodesIntegrator myStudy(mbs, CPodes::BDF, CPodes::/*Newton*/Functional);
291 //myStudy.setOrderLimit(2); // cpodes only
292 //VerletIntegrator myStudy(mbs);
293 // ExplicitEulerIntegrator myStudy(mbs, .0005); // fixed step
294 //ExplicitEulerIntegrator myStudy(mbs); // variable step
295
296
297 //myStudy.setMaximumStepSize(0.001);
298 myStudy.setAccuracy(1e-6); myStudy.setAccuracy(1e-1);
299 //myStudy.setProjectEveryStep(true);
300 //myStudy.setProjectInterpolatedStates(false);
301 myStudy.setConstraintTolerance(1e-7); myStudy.setConstraintTolerance(1e-2);
302 //myStudy.setAllowInterpolation(false);
303 //myStudy.setMaximumStepSize(.1);
304
305 const Real dt = .02; // output intervals
306 const Real finalTime = 2;
307
308 myStudy.setFinalTime(finalTime);
309
310 std::vector<State> saveEm;
311 saveEm.reserve(2000);
312
313 for (int i=0; i<50; ++i)
314 saveEm.push_back(s); // delay
315
316
317 // Peforms assembly if constraints are violated.
318 myStudy.initialize(s);
319
320 for (int i=0; i<50; ++i)
321 saveEm.push_back(s); // delay
322
323 cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
324 cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
325 cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
326 cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
327 cout << "U WEIGHTS=" << s.getUWeights() << endl;
328 cout << "Z WEIGHTS=" << s.getZWeights() << endl;
329 cout << "1/QTOLS=" << s.getQErrWeights() << endl;
330 cout << "1/UTOLS=" << s.getUErrWeights() << endl;
331
332 {
333 const State& s = myStudy.getState();
334 display.report(s);
335 cout << "q=" << s.getQ() << endl;
336 cout << "u=" << s.getU() << endl;
337 cout << "qErr=" << s.getQErr() << endl;
338 cout << "uErr=" << s.getUErr() << endl;
339 cout << "p_MbM=" << mobilizedBody.getMobilizerTransform(s).p() << endl;
340 cout << "PE=" << mbs.calcPotentialEnergy(s) << " KE=" << mbs.calcKineticEnergy(s) << " E=" << mbs.calcEnergy(s) << endl;
341 cout << "angle=" << std::acos(~mobilizedBody.expressVectorInGroundFrame(s, Vec3(0,1,0)) * UnitVec3(1,1,1)) << endl;
342 cout << "Assembled configuration shown. Ready to simulate? "; getchar();
343 }
344
345 Integrator::SuccessfulStepStatus status;
346 int nextReport = 0;
347
348 mbs.resetAllCountersToZero();
349
350 int stepNum = 0;
351 while ((status=myStudy.stepTo(nextReport*dt))
352 != Integrator::EndOfSimulation)
353 {
354 const State& s = myStudy.getState();
355 mbs.realize(s, Stage::Acceleration);
356
357 if ((stepNum++%10)==0) {
358 const Real angle = std::acos(~mobilizedBody.expressVectorInGroundFrame(s, Vec3(0,1,0)) * UnitVec3(1,1,1));
359 printf("%5g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(),
360 angle,
361 mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
362 myStudy.getPreviousStepSizeTaken(),
363 Integrator::getSuccessfulStepStatusString(status).c_str(),
364 myStudy.isStateInterpolated()?" (INTERP)":"");
365 printf(" qerr=%10.8g uerr=%10.8g uderr=%10.8g\n",
366 matter.getQErr(s).normRMS(),
367 matter.getUErr(s).normRMS(),
368 s.getSystemStage() >= Stage::Acceleration ? matter.getUDotErr(s).normRMS() : Real(-1));
369 #ifdef HASC
370 cout << "CONSTRAINT perr=" << c.getPositionError(s)
371 << " verr=" << c.getVelocityError(s)
372 << " aerr=" << c.getAccelerationError(s)
373 << endl;
374 #endif
375 //cout << " d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
376 //cout << " ~d(f)/d lambda=" << c.calcPositionConstraintMatrixPT(s);
377 //cout << " d(perr)/dq=" << c.calcPositionConstraintMatrixPQInverse(s);
378 cout << "Q=" << matter.getQ(s) << endl;
379 cout << "U=" << matter.getU(s) << endl;
380 cout << "Multipliers=" << matter.getMultipliers(s) << endl;
381 }
382
383 Vector qdot;
384 matter.calcQDot(s, s.getU(), qdot);
385 // cout << "===> qdot =" << qdot << endl;
386
387 Vector qdot2;
388 matter.multiplyByN(s, false, s.getU(), qdot2);
389 // cout << "===> qdot2=" << qdot2 << endl;
390
391 Vector u1,u2;
392 matter.multiplyByNInv(s, false, qdot, u1);
393 matter.multiplyByNInv(s, false, qdot2, u2);
394 // cout << "===> u =" << s.getU() << endl;
395 // cout << "===> u1=" << u1 << endl;
396 // cout << "===> u2=" << u2 << endl;
397 // cout << " norm=" << (s.getU()-u2).normRMS() << endl;
398
399 display.report(s);
400 saveEm.push_back(s);
401
402 if (status == Integrator::ReachedReportTime)
403 ++nextReport;
404 }
405
406 printf("Using Integrator %s:\n", myStudy.getMethodName());
407 printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
408 printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
409 printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());
410
411 printf("System stats: realize %dP %dV %dA, projectQ %d, projectU %d\n",
412 mbs.getNumRealizationsOfThisStage(Stage::Position),
413 mbs.getNumRealizationsOfThisStage(Stage::Velocity),
414 mbs.getNumRealizationsOfThisStage(Stage::Acceleration),
415 mbs.getNumProjectQCalls(), mbs.getNumProjectUCalls());
416
417
418 while(true) {
419 for (int i=0; i < (int)saveEm.size(); ++i) {
420 display.report(saveEm[i]);
421 //display.report(saveEm[i]); // half speed
422 }
423 getchar();
424 }
425 }
426 catch (const exception& e) {
427 printf("EXCEPTION THROWN: %s\n", e.what());
428 exit(1);
429 }
430 catch (...) {
431 printf("UNKNOWN EXCEPTION THROWN\n");
432 exit(1);
433 }
434
435 }
436
437