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 * This is a simple example of using a constraint.
26 * Here we have two independent pendulums hanging from ground pins.
27 * They can be connected by a spring or a distance constraint.
28 */
29
30 #include "SimTKsimbody.h"
31
32 #include <cmath>
33 #include <cstdio>
34 #include <exception>
35 #include <vector>
36
37 using namespace std;
38 using namespace SimTK;
39
40 static const Real Deg2Rad = (Real)SimTK_DEGREE_TO_RADIAN,
41 Rad2Deg = (Real)SimTK_RADIAN_TO_DEGREE;
42
43 static const Transform GroundFrame;
44
45 static const Real m = 1; // kg
46 static const Real g = 9.8; // meters/s^2; apply in �y direction
47 static const Real d = 0.5; // meters
48
49 class ShermsForce : public Force::Custom::Implementation {
50 public:
ShermsForce(const MobilizedBody & b1,const MobilizedBody & b2)51 ShermsForce(const MobilizedBody& b1, const MobilizedBody& b2) : body1(b1), body2(b2) { }
clone() const52 ShermsForce* clone() const {return new ShermsForce(*this);}
53
calcForce(const State & state,Vector_<SpatialVec> & bodyForces,Vector_<Vec3> & particleForces,Vector & mobilityForces) const54 void calcForce(const State& state,
55 Vector_<SpatialVec>& bodyForces,
56 Vector_<Vec3>& particleForces,
57 Vector& mobilityForces) const override
58 {
59 const Vec3& pos1 = body1.getBodyTransform(state).p();
60 const Vec3& pos2 = body2.getBodyTransform(state).p();
61 const Real d = (pos2-pos1).norm();
62 const Real k = 1000, d0 = 1;
63 const Vec3 f = k*(d-d0)*(pos2-pos1)/d;
64 body1.applyBodyForce(state, SpatialVec(Vec3(0), f), bodyForces);
65 body2.applyBodyForce(state, SpatialVec(Vec3(0), -f), bodyForces);
66 }
calcPotentialEnergy(const State & state) const67 Real calcPotentialEnergy(const State& state) const override {
68 return 0;
69 }
70 private:
71 const MobilizedBody& body1;
72 const MobilizedBody& body2;
73 };
74 //template <class E> Vector_<E>
75 //operator*(const VectorView_<E>& l, const typename CNT<E>::StdNumber& r)
76 // { return Vector_<E>(l)*=r; }
77
ff(Vector & v)78 void ff(Vector& v) {
79 v = 23.;
80 }
81
main(int argc,char ** argv)82 int main(int argc, char** argv) {
83 try { // If anything goes wrong, an exception will be thrown.
84
85 // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
86 MultibodySystem mbs;
87
88 SimbodyMatterSubsystem twoPends(mbs);
89 GeneralForceSubsystem forces(mbs);
90 DecorationSubsystem viz(mbs);
91 Force::UniformGravity gravity(forces, twoPends, Vec3(0, -g, 0));
92 gravity.setDisabledByDefault(true);
93
94 // ADD BODIES AND THEIR MOBILIZERS
95 Body::Rigid pendulumBody = Body::Rigid(MassProperties(m, Vec3(0), Inertia(1)));
96 pendulumBody.addDecoration(Transform(),
97 DecorativeBrick(Vec3(.1,.0667,.05)).setOpacity(.5));
98
99 MobilizedBody:: Ball /*Gimbal*/ /*FreeLine*/ /*LineOrientation*/ /*Free*/
100 leftPendulum(twoPends.Ground(),
101 Transform(Vec3(-1, 0, 0)),
102 pendulumBody,
103 Transform(Vec3(0, d, 0)));
104 /*
105 MobilizedBody::Ball
106 leftPendulum2(leftPendulum,
107 Transform(Vec3(0.5, 0, 0)),
108 pendulumBody,
109 Transform(Vec3(0, d, 0)));
110 */
111
112 // leftPendulum.setDefaultRadius(0.2); // for Ball artwork
113
114 Vec3 radii(1.5/2.,1/3.,1/4.); radii*=.5; //radii=Vec3(.333,.5,1);
115 MobilizedBody::Ellipsoid rightPendulum(twoPends.Ground(), pendulumBody);
116 rightPendulum.setDefaultRadii(radii)
117 .setDefaultInboardFrame(Transform(Rotation(),Vec3(1,0,0)))
118 .setDefaultOutboardFrame(Transform( Rotation( SpaceRotationSequence, Pi/2, XAxis, -Pi/2, YAxis ), Vec3(0,d-radii[1],0)));
119
120 //rightPendulum.setDefaultAngle(20*Deg2Rad);
121 // rightPendulum.setDefaultRotation( Rotation(60*Deg2Rad, Vec3(0,0,1)) );
122
123 // OPTIONALLY TIE TOGETHER WITH SPRING/DAMPER OR DISTANCE CONSTRAINT
124
125 const Real distance = /*2*/1.5; // nominal length for spring; length for constraint
126 const Real stiffness = 100; // only if spring is used
127 const Real damping = 10; // "
128
129 char c;
130 cout << "Constraint, spring, or nothing? c/s/n"; cin >> c;
131
132 ConstraintIndex cid;
133 const Vec3 leftAttachPt(.1,0.05,0);
134 if (c == 'c') {
135
136 cid =
137 //Constraint::PointInPlane(twoPends.Ground(), UnitVec3(0,1,0), -2*d,
138 // leftPendulum2, Vec3(0))
139 Constraint::Rod(leftPendulum, leftAttachPt,
140 rightPendulum, Vec3(0),
141 distance)
142 // Constraint::Ball(leftPendulum2, Vec3(.5,0,0),
143 // twoPends.Ground(), Vec3(0,-d,0))
144 .getConstraintIndex();
145
146 } else if (c == 's') {
147 Force::TwoPointLinearSpring(forces, leftPendulum, leftAttachPt,
148 rightPendulum, Vec3(0),
149 stiffness, distance);
150 Force::TwoPointLinearDamper(forces, leftPendulum, leftAttachPt,
151 rightPendulum, Vec3(0),
152 damping);
153 }
154
155 // Add visualization line for spring (Rod constraint has one automatically)
156 if (c=='s')
157 viz.addRubberBandLine(leftPendulum, leftAttachPt,
158 rightPendulum, Vec3(0),
159 DecorativeLine().setColor(Orange).setLineThickness(4));
160
161 //forces.addMobilityConstantForce(leftPendulum, 0, 20);
162 //forces.addCustomForce(ShermsForce(leftPendulum,rightPendulum));
163 //forces.addGlobalEnergyDrain(1);
164
165 mbs.setHasTimeAdvancedEvents(false);
166
167 cout << "HAS TIME ADVANCED EVENTS=" << mbs.hasTimeAdvancedEvents() << endl;
168
169 Measure::Constant meas1(twoPends, 20);
170 const Real amp = 3, freq = 100, phase = Pi/2;
171 Measure::Sinusoid sint(twoPends, amp, freq, phase);
172
173 Measure::Integrate twentyPlus10t(twoPends, Measure::Constant(twoPends, 10), meas1);
174
175 State s = mbs.realizeTopology(); // returns a reference to the the default state
176 //twoPends.setUseEulerAngles(s, true);
177 mbs.realizeModel(s); // define appropriate states for this System
178
179 gravity.enable(s);
180 twentyPlus10t.setValue(s, 20);
181
182 mbs.realize(s, Stage::Instance); // instantiate constraints
183
184 cout << "meas1=" << meas1.getValue(s) << endl;
185
186
187 if (cid.isValid()) {
188 int mp, mv, ma;
189 twoPends.getConstraint(cid).getNumConstraintEquationsInUse(s, mp, mv, ma);
190 cout << "CONSTRAINT ID " << cid << " mp,v,a=" << mp << ", " << mv << ", " << ma << endl;
191 cout << "CONSTRAINT -- " << twoPends.getConstraint(cid).getSubtree();
192 }
193
194 for (MobilizedBodyIndex i(0); i < twoPends.getNumBodies(); ++i) {
195 const MobilizedBody& mb = twoPends.getMobilizedBody(i);
196 cout << "Body " << i
197 << " base=" << mb.getBaseMobilizedBody().getMobilizedBodyIndex()
198 << endl;
199 }
200
201
202 SimbodyMatterSubtree sub(twoPends);
203 sub.addTerminalBody(leftPendulum); sub.addTerminalBody(rightPendulum);
204 sub.realizeTopology();
205 cout << "SUB -- " << sub;
206
207 SimbodyMatterSubtreeResults results;
208 sub.initializeSubtreeResults(s, results);
209 cout << "INIT RESULTS=" << results;
210
211 Visualizer display(mbs);
212 display.setBackgroundType(Visualizer::SolidColor);
213
214 // gravity.disable(s);
215 mbs.realize(s, Stage::Position);
216 display.report(s);
217 cout << "q=" << s.getQ() << endl;
218 cout << "qErr=" << s.getQErr() << endl;
219 cout << "p_MbM=" << rightPendulum.getMobilizerTransform(s).p() << endl;
220
221 Vector_<SpatialVec> bodyForces;
222 Vector_<Vec3> particleForces;
223 Vector mobilityForces;
224 gravity.calcForceContribution(s,bodyForces,particleForces,mobilityForces);
225 cout << "Gravity forces: body:" << bodyForces << endl;
226 cout << " particle:" << particleForces << endl;
227 cout << " mobility:" << mobilityForces << endl;
228 cout << " PE=" << gravity.calcPotentialEnergyContribution(s) << endl;
229
230 if (cid.isValid()) {
231 const Constraint& c = twoPends.getConstraint(cid);
232 cout << "CONSTRAINT perr=" << c.getPositionErrorsAsVector(s)
233 << endl;
234 cout << " d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
235 cout << " d(perr)/dq=" << c.calcPositionConstraintMatrixPNInv(s);
236 }
237
238 cout << "Default configuration shown. Ready? "; cin >> c;
239
240 sub.copyPositionsFromState(s, results);
241 cout << "POS RESULTS=" << results;
242
243 //leftPendulum.setAngle(s, -60*Deg2Rad);
244 //leftPendulum.setQToFitRotation(s, Rotation(-60*Deg2Rad,ZAxis));
245
246 //rightPendulum.setQToFitTranslation(s, Vec3(0,1,0));
247 leftPendulum.setQToFitRotation (s, Rotation(-.9*Pi/2,ZAxis));
248 rightPendulum.setQToFitRotation(s, Rotation(-.9*Pi/2,YAxis));
249
250
251 //TODO
252 //rightPendulum.setUToFitLinearVelocity(s, Vec3(1.1,0,1.2));
253
254 leftPendulum.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
255 rightPendulum.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
256
257
258 s.setTime(0);
259
260 mbs.realize(s, Stage::Velocity);
261 display.report(s);
262
263 cout << "q=" << s.getQ() << endl;
264 cout << "qErr=" << s.getQErr() << endl;
265 cout << "p_MbM=" << rightPendulum.getMobilizerTransform(s).p() << endl;
266 cout << "v_MbM=" << rightPendulum.getMobilizerVelocity(s)[1] << endl;
267 cout << "Unassembled configuration shown. Ready to assemble? "; cin >> c;
268
269 // These are the SimTK Simmath integrators:
270 RungeKuttaMersonIntegrator myStudy(mbs);
271 //CPodesIntegrator myStudy(mbs, CPodes::BDF, CPodes::Newton);
272 //myStudy.setOrderLimit(2); // cpodes only
273 //VerletIntegrator myStudy(mbs);
274
275
276 //myStudy.setMaximumStepSize(0.001);
277 myStudy.setAccuracy(1e-1);
278 //myStudy.setProjectEveryStep(true);
279 myStudy.setConstraintTolerance(1e-2);
280 //myStudy.setAllowInterpolation(false);
281 //myStudy.setMaximumStepSize(.1);
282
283 const Real dt = 1./60; // output intervals
284 const Real finalTime = 20;
285
286 myStudy.setFinalTime(finalTime);
287
288 // Peforms assembly if constraints are violated.
289 myStudy.initialize(s);
290
291 cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
292 cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
293 cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
294 cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
295 cout << "U WEIGHTS=" << s.getUWeights() << endl;
296 cout << "Z WEIGHTS=" << s.getZWeights() << endl;
297 cout << "1/QTOLS=" << s.getQErrWeights() << endl;
298 cout << "1/UTOLS=" << s.getUErrWeights() << endl;
299
300 {
301 const State& s = myStudy.getState();
302 display.report(s);
303 cout << "q=" << s.getQ() << endl;
304 cout << "qErr=" << s.getQErr() << endl;
305 cout << "p_MbM=" << rightPendulum.getMobilizerTransform(s).p() << endl;
306 cout << "Assembled configuration shown. Ready to simulate? "; cin >> c;
307 }
308
309 Integrator::SuccessfulStepStatus status;
310 int nextReport = 0;
311 int nextScheduledEvent = 0;
312 Real schedule[] = {1.234, 3.1415, 3.14159, 4.5, 9.090909, 100.};
313 while ((status=myStudy.stepTo(nextReport*dt,schedule[nextScheduledEvent]))
314 != Integrator::EndOfSimulation)
315 {
316 const State& s = myStudy.getState();
317 mbs.realize(s);
318 const Real leftPendulumAngle = leftPendulum.getBodyRotation(s).convertRotationToAngleAxis()[0] * Rad2Deg;
319
320 if (status == Integrator::ReachedScheduledEvent
321 || std::abs(std::floor(s.getTime()+0.5)-s.getTime())<1e-4)
322 {
323 printf("%5g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(),
324 leftPendulumAngle,
325 mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
326 myStudy.getPreviousStepSizeTaken(),
327 Integrator::getSuccessfulStepStatusString(status).c_str(),
328 myStudy.isStateInterpolated()?" (INTERP)":"");
329 printf(" qerr=%10.8g uerr=%10.8g uderr=%10.8g\n",
330 twoPends.getQErr(s).normRMS(),
331 twoPends.getUErr(s).normRMS(),
332 twoPends.getUDotErr(s).normRMS());
333
334 cout << "t=" << s.getTime() << "sint=" << sint.getValue(s) << "a*sin(wt+p)="
335 << amp*std::sin(freq*s.getTime() + phase) << endl;
336
337 cout << "20+10t=" << twentyPlus10t.getValue(s) << endl;
338
339 if (cid.isValid()) {
340 const Constraint& c = twoPends.getConstraint(cid);
341 cout << "CONSTRAINT perr=" << c.getPositionErrorsAsVector(s)
342 << " verr=" << c.getVelocityErrorsAsVector(s)
343 << " aerr=" << c.getAccelerationErrorsAsVector(s)
344 << endl;
345 //cout << " d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
346 //cout << " ~d(f)/d lambda=" << c.calcPositionConstraintMatrixPT(s);
347 //cout << " d(perr)/dq=" << c.calcPositionConstraintMatrixPQInverse(s);
348 }
349 }
350
351 Vector qdot;
352 twoPends.calcQDot(s, s.getU(), qdot);
353 // cout << "===> qdot =" << qdot << endl;
354
355 Vector qdot2;
356 twoPends.multiplyByN(s, false, s.getU(), qdot2);
357 // cout << "===> qdot2=" << qdot2 << endl;
358
359 Vector u1,u2;
360 twoPends.multiplyByNInv(s, false, qdot, u1);
361 twoPends.multiplyByNInv(s, false, qdot2, u2);
362 // cout << "===> u =" << s.getU() << endl;
363 // cout << "===> u1=" << u1 << endl;
364 // cout << "===> u2=" << u2 << endl;
365 // cout << " norm=" << (s.getU()-u2).normRMS() << endl;
366
367 //sub.copyPositionsFromState(s, results);
368 //sub.copyVelocitiesFromState(s, results);
369 // sub.copyAccelerationsFromState(s, results);
370 //cout << results;
371
372 display.report(s);
373 //if (s.getTime() >= finalTime)
374 // break;
375
376 //status = myStudy.stepTo(s.getTime() + dt);
377
378 //THIS CAN FAIL SOMETIMES
379 //if (s.getTime() >= nextReport*dt)
380 // ++nextReport;
381
382 if (status == Integrator::ReachedReportTime)
383 ++nextReport;
384
385 if (s.getTime() >= schedule[nextScheduledEvent])
386 ++nextScheduledEvent;
387 }
388
389 printf("Using Integrator %s:\n", myStudy.getMethodName());
390 printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
391 printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
392 printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());
393
394 }
395 catch (const exception& e) {
396 printf("EXCEPTION THROWN: %s\n", e.what());
397 exit(1);
398 }
399 catch (...) {
400 printf("UNKNOWN EXCEPTION THROWN\n");
401 exit(1);
402 }
403
404 }
405
406