1 #ifndef SimTK_SIMMATH_PENDULUMSYSTEM_H_
2 #define SimTK_SIMMATH_PENDULUMSYSTEM_H_
3
4 /* -------------------------------------------------------------------------- *
5 * SimTK Core: SimTK Simmath(tm) *
6 * -------------------------------------------------------------------------- *
7 * This is part of the SimTK Core biosimulation toolkit originating from *
8 * Simbios, the NIH National Center for Physics-Based Simulation of *
9 * Biological Structures at Stanford, funded under the NIH Roadmap for *
10 * Medical Research, grant U54 GM072970. See https://simtk.org. *
11 * *
12 * Portions copyright (c) 2006-7 Stanford University and the Authors. *
13 * Authors: Michael Sherman, Peter Eastman *
14 * Contributors: *
15 * *
16 * Permission is hereby granted, free of charge, to any person obtaining a *
17 * copy of this software and associated documentation files (the "Software"), *
18 * to deal in the Software without restriction, including without limitation *
19 * the rights to use, copy, modify, merge, publish, distribute, sublicense, *
20 * and/or sell copies of the Software, and to permit persons to whom the *
21 * Software is furnished to do so, subject to the following conditions: *
22 * *
23 * The above copyright notice and this permission notice shall be included in *
24 * all copies or substantial portions of the Software. *
25 * *
26 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
27 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
28 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL *
29 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, *
30 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR *
31 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE *
32 * USE OR OTHER DEALINGS IN THE SOFTWARE. *
33 * -------------------------------------------------------------------------- */
34
35 /**
36 * This is a simple System that is used by various test cases.
37 */
38
39 #include "SimTKcommon.h"
40 #include "SimTKcommon/internal/SystemGuts.h"
41
42 using namespace SimTK;
43
44 class PendulumSystem;
45 class PendulumSystemGuts: public System::Guts {
46 friend class PendulumSystem;
47
48 // TOPOLOGY STATE
49 SubsystemIndex subsysIndex;
50
51 // TOPOLOGY CACHE
52 mutable DiscreteVariableIndex massIndex, lengthIndex, gravityIndex;
53 mutable QIndex q0;
54 mutable UIndex u0;
55 mutable QErrIndex qerr0;
56 mutable UErrIndex uerr0;
57 mutable UDotErrIndex udoterr0;
58 mutable EventTriggerByStageIndex event0;
59 mutable CacheEntryIndex mgForceIndex; // a cache entry m*g calculated at Dynamics stage
60 public:
PendulumSystemGuts()61 PendulumSystemGuts() : Guts() {
62 // Index types set themselves invalid on construction.
63 }
64
getPendulumSystem()65 const PendulumSystem& getPendulumSystem() const {
66 return reinterpret_cast<const PendulumSystem&>(getSystem());
67 }
68
getSubsysIndex()69 SubsystemIndex getSubsysIndex() const {
70 return subsysIndex;
71 }
72
cloneImpl()73 /*virtual*/PendulumSystemGuts* cloneImpl() const override {return new PendulumSystemGuts(*this);}
74
75 /////////////////////////////////////////////////////////
76 // Implementation of continuous DynamicSystem virtuals //
77 /////////////////////////////////////////////////////////
78
79 /*virtual*/int realizeTopologyImpl(State&) const override;
80 /*virtual*/int realizeModelImpl(State&) const override;
81 /*virtual*/int realizeInstanceImpl(const State&) const override;
82 /*virtual*/int realizePositionImpl(const State&) const override;
83 /*virtual*/int realizeVelocityImpl(const State&) const override;
84 /*virtual*/int realizeDynamicsImpl(const State&) const override;
85 /*virtual*/int realizeAccelerationImpl(const State&) const override;
86
87 // qdot==u here so these are just copies
multiplyByNImpl(const State & state,const Vector & u,Vector & dq)88 /*virtual*/void multiplyByNImpl(const State& state, const Vector& u,
89 Vector& dq) const override {dq=u;}
multiplyByNTransposeImpl(const State & state,const Vector & fq,Vector & fu)90 /*virtual*/void multiplyByNTransposeImpl(const State& state, const Vector& fq,
91 Vector& fu) const override {fu=fq;}
multiplyByNPInvImpl(const State & state,const Vector & dq,Vector & u)92 /*virtual*/void multiplyByNPInvImpl(const State& state, const Vector& dq,
93 Vector& u) const override {u=dq;}
multiplyByNPInvTransposeImpl(const State & state,const Vector & fu,Vector & fq)94 /*virtual*/void multiplyByNPInvTransposeImpl(const State& state, const Vector& fu,
95 Vector& fq) const override {fq=fu;}
96
97 // No prescribed motion.
prescribeQImpl(State &)98 /*virtual*/bool prescribeQImpl(State&) const override {return false;}
prescribeUImpl(State &)99 /*virtual*/bool prescribeUImpl(State&) const override {return false;}
100
101 /*virtual*/void projectQImpl(State&, Vector& qErrEst,
102 const ProjectOptions& options, ProjectResults& results) const override;
103 /*virtual*/void projectUImpl(State&, Vector& uErrEst,
104 const ProjectOptions& options, ProjectResults& results) const override;
105
106 };
107
108 class PendulumSystem: public System {
109 public:
PendulumSystem()110 PendulumSystem() : System()
111 {
112 adoptSystemGuts(new PendulumSystemGuts());
113 DefaultSystemSubsystem defsub(*this);
114 updGuts().subsysIndex = defsub.getMySubsystemIndex();
115
116 setHasTimeAdvancedEvents(false);
117 }
118
getGuts()119 const PendulumSystemGuts& getGuts() const {
120 return dynamic_cast<const PendulumSystemGuts&>(getSystemGuts());
121 }
122
updGuts()123 PendulumSystemGuts& updGuts() {
124 return dynamic_cast<PendulumSystemGuts&>(updSystemGuts());
125 }
126
127 // Instance variables are written to our defaultState.
setDefaultMass(Real mass)128 void setDefaultMass(Real mass) {
129 const PendulumSystemGuts& guts = getGuts();
130 updDefaultState().updDiscreteVariable(guts.subsysIndex, guts.massIndex) = Value<Real>(mass);
131 }
132
setDefaultLength(Real length)133 void setDefaultLength(Real length) {
134 const PendulumSystemGuts& guts = getGuts();
135 updDefaultState().updDiscreteVariable(guts.subsysIndex, guts.lengthIndex) = Value<Real>(length);
136 }
137
setDefaultGravity(Real gravity)138 void setDefaultGravity(Real gravity) {
139 const PendulumSystemGuts& guts = getGuts();
140 updDefaultState().updDiscreteVariable(guts.subsysIndex, guts.gravityIndex) = Value<Real>(gravity);
141 }
142
setDefaultTimeAndState(Real t,const Vector & q,const Vector & u)143 void setDefaultTimeAndState(Real t, const Vector& q, const Vector& u) {
144 const PendulumSystemGuts& guts = getGuts();
145 updDefaultState().updU(guts.subsysIndex) = u;
146 updDefaultState().updQ(guts.subsysIndex) = q;
147 updDefaultState().updTime() = t;
148 }
149
getMass(const State & s)150 Real getMass(const State& s) const {
151 const PendulumSystemGuts& guts = getGuts();
152 const AbstractValue& m = s.getDiscreteVariable(guts.subsysIndex, guts.massIndex);
153 return Value<Real>::downcast(m).get();
154 }
getDefaultMass()155 Real getDefaultMass() const {return getMass(getDefaultState());}
156
getLength(const State & s)157 Real getLength(const State& s) const {
158 const PendulumSystemGuts& guts = getGuts();
159 const AbstractValue& d = s.getDiscreteVariable(guts.subsysIndex, guts.lengthIndex);
160 return Value<Real>::downcast(d).get();
161 }
getDefaultLength()162 Real getDefaultLength() const {return getLength(getDefaultState());}
163
getGravity(const State & s)164 Real getGravity(const State& s) const {
165 const PendulumSystemGuts& guts = getGuts();
166 const AbstractValue& g = s.getDiscreteVariable(guts.subsysIndex, guts.gravityIndex);
167 return Value<Real>::downcast(g).get();
168 }
getDefaultGravity()169 Real getDefaultGravity() const {return getGravity(getDefaultState());}
170 };
171
172 /*
173 * This system is a 2d pendulum swinging in gravity. It is modeled as
174 * a point mass free in the plane, plus a distance constraint to model
175 * the rod.
176 *
177 * y | g O
178 * ^ v \ d
179 * | \
180 * | * m
181 * ------> x
182 *
183 * Gravity acts in the y direction, the rod is length d, mass m, pivot
184 * location is the ground origin (0,0).
185 *
186 * The DAE for a generic multibody system is:
187 * qdot = Qu
188 * M udot = f - ~A lambda
189 * A udot = b
190 * perr(t,q) = 0
191 * verr(t,q,u) = 0
192 *
193 * Let r^2 = x^2 + y^2
194 * v^2 = x'^2 + y'^2
195 * We will express the "rod length=d" constraint as
196 * (r^2 - d^2)/2 = 0 (perr)
197 * xx' + yy' = 0 (verr)
198 * xx'' + yy'' = -v^2 (aerr)
199 *
200 * So the matrix A = d perr/dq = [x y] and b = -v^2, and the
201 * equations of motion are:
202 * [ m 0 x ] [ x'' ] [ 0 ]
203 * [ 0 m y ] [ y'' ] = [ -mg ]
204 * [ x y 0 ] [ L ] [-v^2 ]
205 * where L (the Lagrange multiplier) is proportional to
206 * the rod tension. You can solve this to get
207 * L = (m*v^2 - mg*y)/(r^2)
208 * x'' = - x*L/m
209 * y'' = - y*L/m - g
210 *
211 */
realizeTopologyImpl(State & s)212 int PendulumSystemGuts::realizeTopologyImpl(State& s) const {
213 // Instance variables mass, length, gravity
214 massIndex = s.allocateDiscreteVariable(subsysIndex, Stage::Instance,
215 new Value<Real>(1));
216 lengthIndex = s.allocateDiscreteVariable(subsysIndex, Stage::Instance,
217 new Value<Real>(1));
218 gravityIndex = s.allocateDiscreteVariable(subsysIndex, Stage::Instance,
219 new Value<Real>(13.7503716373294544));
220 const Vector init(2, Real(0));
221 q0 = s.allocateQ(subsysIndex, init);
222 u0 = s.allocateU(subsysIndex, init);
223
224 mgForceIndex = s.allocateCacheEntry(subsysIndex, Stage::Dynamics,
225 new Value<Real>());
226 System::Guts::realizeTopologyImpl(s);
227 return 0;
228 }
realizeModelImpl(State & s)229 int PendulumSystemGuts::realizeModelImpl(State& s) const {
230 System::Guts::realizeModelImpl(s);
231 return 0;
232 }
realizeInstanceImpl(const State & s)233 int PendulumSystemGuts::realizeInstanceImpl(const State& s) const {
234 qerr0 = s.allocateQErr(subsysIndex, 1);
235 uerr0 = s.allocateUErr(subsysIndex, 1);
236 udoterr0 = s.allocateUDotErr(subsysIndex, 1); // and multiplier
237 // event0 = s.allocateEvent(subsysIndex, Stage::Position, 3);
238 System::Guts::realizeInstanceImpl(s);
239 return 0;
240 }
realizePositionImpl(const State & s)241 int PendulumSystemGuts::realizePositionImpl(const State& s) const {
242 const Real d = getPendulumSystem().getLength(s);
243 const Vector& q = s.getQ(subsysIndex);
244 // This is the perr() equation.
245 s.updQErr(subsysIndex)[0] = (q[0]*q[0] + q[1]*q[1] - d*d)/2;
246
247 // s.updEventsByStage(subsysIndex, Stage::Position)[0] = 100*q[0]-q[1];
248 //
249 // s.updEventsByStage(subsysIndex, Stage::Position)[1] =
250 // s.getTime() > 1.49552 && s.getTime() < 12.28937;
251 //
252 // s.updEventsByStage(subsysIndex, Stage::Position)[2] = s.getTime()-1.495508;
253 System::Guts::realizePositionImpl(s);
254 return 0;
255 }
256
realizeVelocityImpl(const State & s)257 int PendulumSystemGuts::realizeVelocityImpl(const State& s) const {
258 const Vector& q = s.getQ(subsysIndex);
259 const Vector& u = s.getU(subsysIndex);
260 Vector& qdot = s.updQDot(subsysIndex);
261
262 qdot[0] = u[0]; // qdot=u
263 qdot[1] = u[1];
264
265 // This is the verr() equation.
266 s.updUErr(subsysIndex)[0] = q[0]*u[0] + q[1]*u[1];
267 System::Guts::realizeVelocityImpl(s);
268 return 0;
269 }
270
realizeDynamicsImpl(const State & s)271 int PendulumSystemGuts::realizeDynamicsImpl(const State& s) const {
272 const Real m = getPendulumSystem().getMass(s);
273 const Real g = getPendulumSystem().getGravity(s);
274
275 Real& mg = Value<Real>::updDowncast
276 (s.updCacheEntry(subsysIndex, mgForceIndex)).upd();
277 // Calculate the force due to gravity.
278 mg = m*g;
279 System::Guts::realizeDynamicsImpl(s);
280 return 0;
281 }
282
realizeAccelerationImpl(const State & s)283 int PendulumSystemGuts::realizeAccelerationImpl(const State& s) const {
284 const Real m = getPendulumSystem().getMass(s);
285 const Real g = getPendulumSystem().getGravity(s);
286 // we're pretending we couldn't calculate this here!
287 const Real mg = Value<Real>::updDowncast
288 (s.updCacheEntry(subsysIndex, mgForceIndex)).get();
289
290 const Vector& q = s.getQ(subsysIndex);
291 const Vector& u = s.getU(subsysIndex);
292 Vector& udot = s.updUDot(subsysIndex);
293
294 const Real r2 = q[0]*q[0] + q[1]*q[1];
295 const Real v2 = u[0]*u[0] + u[1]*u[1];
296 const Real L = (m*v2 - mg*q[1])/r2;
297 udot[0] = - q[0]*L/m;
298 udot[1] = - q[1]*L/m - g;
299 s.updQDotDot() = udot;
300 s.updMultipliers(subsysIndex)[0] = L;
301 s.updUDotErr(subsysIndex)[0] = q[0]*udot[0] + q[1]*udot[1] + v2;
302 System::Guts::realizeAccelerationImpl(s);
303 return 0;
304 }
305
306 /*
307 * Here we want to remove any constraint errors from the current state,
308 * and project out any component of the integrator's error estimate
309 * perpendicular to the constraint manifold. We will do this sequentially
310 * rather than handling position and velocity simultaneously.
311 *
312 * For this system we have P = d perr/dq = V = d verr/du = [x y].
313 * Weighted, we have PW=tp*[x/wx y/wy] VW=tv*[x/wxd y/wyd].
314 * With pinv(A)=~A*(A*~A)^-1, we have:
315 *
316 * pinv(P) = ~[ x y] / ( x ^2+ y ^2)
317 * pinv(PW) = ~(1/tp)*[(wx *wy ^2)*x (wx ^2*wy) *y] / ((wy *x)^2+(wx *y)^2)
318 * pinv(VW) = ~(1/tv)*[(wxd*wyd^2)*x (wxd^2*wyd)*y] / ((wyd*x)^2+(wxd*y)^2)
319 * (the latter assuming x,y already projected on position manifold)
320 *
321 * We want to solve
322 * |perr(q0 - dq)|_TRMS <= accuracy, such that dq=min_WLS(dq)
323 * PW(q0) dq = Tp * perr(q0); q = q0-dq
324 * Then
325 * |verr(q,u0 - du)|_TRMS <= accuracy, du=min_WLS(du)
326 * VW(q) du = Tv * verr(q,u0); u = u0-du
327 *
328 *
329 * To remove the corresponding error estimates:
330 * PW(q) qperp = PW(q) qerrest; qerrest -= qperp
331 * VW(q) uperp = VW(q) uerrest; uerrest -= uperp
332 *
333 *
334 */
wrms(const Vector & y,const Vector & w)335 static Real wrms(const Vector& y, const Vector& w) {
336 Real sumsq = 0;
337 for (int i=0; i<y.size(); ++i)
338 sumsq += square(y[i]*w[i]);
339 return std::sqrt(sumsq/y.size());
340 }
341
342 //int PendulumSystemGuts::projectImpl(State& s, Real consAccuracy,
343 // const Vector& yweights, const Vector& ctols,
344 // Vector& yerrest, System::ProjectOptions opts) const // yerrest is in/out
345 //{
346 // const Vec2& wq = Vec2::getAs(&yweights[0]);
347 // const Vec2& wu = Vec2::getAs(&yweights[2]);
348 // const Real& tp = ctols[0]; // inverse tolerances 1/ti
349 // const Real& tv = ctols[1];
350 //
351 // const Vec2& q = Vec2::getAs(&s.getQ(subsysIndex)[0]); // set up aliases
352 // const Vec2& u = Vec2::getAs(&s.getU(subsysIndex)[0]);
353 // Real& ep = s.updQErr(subsysIndex)[0];
354 // Real& ev = s.updUErr(subsysIndex)[0];
355 //
356 // //cout << "BEFORE wperr=" << tp*ep << endl;
357 //
358 // Real wqchg;
359 // if (opts.hasAnyPositionOptions()) {
360 // do {
361 // // Position projection
362 // Real r2 = ~q*q; // x^2+y^2
363 // Real wqr2 = square(wq[1]*q[0]) + square(wq[0]*q[1]);
364 // Row2 P(~q), PW(tp*q[0]/wq[0], tp*q[1]/wq[1]);
365 // Vec2 Pinv(q/r2);
366 // Vec2 PWinv = Vec2(square(wq[1])*wq[0]*q[0],
367 // square(wq[0])*wq[1]*q[1]) / (tp*wqr2);
368 // Vec2 dq = Pinv*(ep); //cout << "dq=" << dq << endl;
369 // Vec2 wdq = PWinv*(tp*ep); //cout << "wdq=" << wdq << endl;
370 //
371 // wqchg = std::sqrt(wdq.normSqr()/q.size()); // wrms norm
372 //
373 // s.updQ(subsysIndex)[0] -= wdq[0]/wq[0];
374 // s.updQ(subsysIndex)[1] -= wdq[1]/wq[1];
375 // realize(s, Stage::Position); // recalc QErr (ep)
376 //
377 // //cout << "AFTER q-=wdq/W wperr=" << tp*ep << " wqchg=" << wqchg << endl;
378 // } while (std::abs(tp*ep) > consAccuracy && wqchg >= 0.01*consAccuracy);
379 // }
380 //
381 // // Do velocity projection at new values of q
382 // Real r2 = ~q*q; // x^2+y^2
383 // Real wur2 = square(wu[1]*q[0]) + square(wu[0]*q[1]);
384 // Row2 V(~q), VW(tv*q[0]/wu[0], tv*q[1]/wu[1]);
385 // Vec2 Vinv(q/r2);
386 // Vec2 VWinv = Vec2(square(wu[1])*wu[0]*q[0],
387 // square(wu[0])*wu[1]*q[1]) / (tv*wur2);
388 // realize(s, Stage::Velocity); // calculate UErr (ev)
389 //
390 // //cout << "BEFORE wverr=" << tv*ev << endl;
391 // Vec2 du = Vinv*(ev); //cout << "du=" << du << endl;
392 // Vec2 wdu = VWinv*(tv*ev); //cout << "wdu=" << wdu << endl;
393 //
394 // s.updU(subsysIndex)[0] -= wdu[0]/wu[0];
395 // s.updU(subsysIndex)[1] -= wdu[1]/wu[1];
396 //
397 // realize(s, Stage::Velocity); // recalc UErr
398 // //cout << "AFTER u-=wdu wverr=" << tv*ev << endl;
399 //
400 // // Now do error estimates.
401 //
402 //
403 // if (yerrest.size()) {
404 // Vec2& eq = Vec2::updAs(&yerrest[0]);
405 // Vec2& eu = Vec2::updAs(&yerrest[2]);
406 //
407 // // Recalc PW, PWInv:
408 // const Real wqr2 = square(wq[1]*q[0]) + square(wq[0]*q[1]);
409 // const Row2 PW = Row2(tp*q[0]/wq[0], tp*q[1]/wq[1]);
410 // const Vec2 PWinv = Vec2(wq[0]*square(wq[1])*q[0],
411 // square(wq[0])*wq[1]*q[1]) / (tp*wqr2);
412 //
413 // Vec2 qperp = PWinv*(PW*eq);
414 // Vec2 uperp = VWinv*(VW*eu);
415 //
416 // //cout << "ERREST before=" << yerrest
417 // // << " wrms=" << wrms(yerrest,yweights) << endl;
418 // //cout << "PW*eq=" << PW*eq << " VW*eu=" << VW*eu << endl;
419 // eq -= qperp; eu -= uperp;
420 //
421 // //cout << "ERREST after=" << yerrest
422 // // << " wrms=" << wrms(yerrest,yweights) << endl;
423 // //cout << "PW*eq=" << PW*eq << " VW*eu=" << VW*eu << endl;
424 // }
425 //
426 // return 0;
427 //}
428
429 // qerrest is in/out
projectQImpl(State & s,Vector & qerrest,const ProjectOptions & opts,ProjectResults & results)430 void PendulumSystemGuts::projectQImpl(State& s, Vector& qerrest,
431 const ProjectOptions& opts,
432 ProjectResults& results) const
433
434 {
435 const Real consAccuracy = opts.getRequiredAccuracy();
436 const Vector& uweights = s.getUWeights(subsysIndex);
437 const Vector& ctols = s.getQErrWeights(subsysIndex);
438 // Since qdot=u here we can use uweights directly as qweights.
439 const Vec2& wq = Vec2::getAs(&uweights[0]);
440 const Real& tp = ctols[0]; // inverse tolerances 1/ti
441
442 const Vec2& q = Vec2::getAs(&s.getQ(subsysIndex)[0]); // set up aliases
443 Real& ep = s.updQErr(subsysIndex)[0];
444
445 //cout << "BEFORE wperr=" << tp*ep << endl;
446
447 Real wqchg;
448 do {
449 // Position projection
450 Real r2 = ~q*q; // x^2+y^2
451 Real wqr2 = square(wq[1]*q[0]) + square(wq[0]*q[1]);
452 Row2 P(~q), PW(tp*q[0]/wq[0], tp*q[1]/wq[1]);
453 Vec2 Pinv(q/r2);
454 Vec2 PWinv = Vec2(square(wq[1])*wq[0]*q[0],
455 square(wq[0])*wq[1]*q[1]) / (tp*wqr2);
456 Vec2 dq = Pinv*(ep); //cout << "dq=" << dq << endl;
457 Vec2 wdq = PWinv*(tp*ep); //cout << "wdq=" << wdq << endl;
458
459 wqchg = std::sqrt(wdq.normSqr()/q.size()); // wrms norm
460
461 s.updQ(subsysIndex)[0] -= wdq[0]/wq[0];
462 s.updQ(subsysIndex)[1] -= wdq[1]/wq[1];
463 realize(s, Stage::Position); // recalc QErr (ep)
464
465 //cout << "AFTER q-=wdq/W wperr=" << tp*ep << " wqchg=" << wqchg << endl;
466 } while (std::abs(tp*ep) > consAccuracy && wqchg >= 0.01*consAccuracy);
467
468 // Now do error estimates.
469
470 if (qerrest.size()) {
471 Vec2& eq = Vec2::updAs(&qerrest[0]);
472
473 // Recalc PW, PWInv:
474 const Real wqr2 = square(wq[1]*q[0]) + square(wq[0]*q[1]);
475 const Row2 PW = Row2(tp*q[0]/wq[0], tp*q[1]/wq[1]);
476 const Vec2 PWinv = Vec2(wq[0]*square(wq[1])*q[0],
477 square(wq[0])*wq[1]*q[1]) / (tp*wqr2);
478
479 Vec2 qperp = PWinv*(PW*eq);
480
481 //cout << "ERREST before=" << yerrest
482 // << " wrms=" << wrms(qerrest,qweights) << endl;
483 //cout << "PW*eq=" << PW*eq << endl;
484 eq -= qperp;
485
486 //cout << "ERREST after=" << yerrest
487 // << " wrms=" << wrms(qerrest,qweights) << endl;
488 //cout << "PW*eq=" << PW*eq << endl;
489 }
490
491 results.setExitStatus(ProjectResults::Succeeded);
492 }
493
projectUImpl(State & s,Vector & uerrest,const ProjectOptions & opts,ProjectResults & results)494 void PendulumSystemGuts::projectUImpl(State& s, Vector& uerrest,
495 const ProjectOptions& opts, ProjectResults& results) const
496 {
497 const Real consAccuracy = opts.getRequiredAccuracy();
498 const Vector& uweights = s.getUWeights(subsysIndex);
499 const Vector& ctols = s.getUErrWeights(subsysIndex);
500
501 const Vec2& wu = Vec2::getAs(&uweights[0]);
502 const Real& tv = ctols[0];
503
504 const Vec2& q = Vec2::getAs(&s.getQ(subsysIndex)[0]); // set up aliases
505 const Vec2& u = Vec2::getAs(&s.getU(subsysIndex)[0]);
506 Real& ev = s.updUErr(subsysIndex)[0];
507
508 //cout << "BEFORE wperr=" << tp*ep << endl;
509
510 // Do velocity projection at current values of q, which should have
511 // been projected already.
512 Real r2 = ~q*q; // x^2+y^2
513 Real wur2 = square(wu[1]*q[0]) + square(wu[0]*q[1]);
514 Row2 V(~q), VW(tv*q[0]/wu[0], tv*q[1]/wu[1]);
515 Vec2 Vinv(q/r2);
516 Vec2 VWinv = Vec2(square(wu[1])*wu[0]*q[0],
517 square(wu[0])*wu[1]*q[1]) / (tv*wur2);
518 realize(s, Stage::Velocity); // calculate UErr (ev)
519
520 //cout << "BEFORE wverr=" << tv*ev << endl;
521 Vec2 du = Vinv*(ev); //cout << "du=" << du << endl;
522 Vec2 wdu = VWinv*(tv*ev); //cout << "wdu=" << wdu << endl;
523
524 s.updU(subsysIndex)[0] -= wdu[0]/wu[0];
525 s.updU(subsysIndex)[1] -= wdu[1]/wu[1];
526
527 realize(s, Stage::Velocity); // recalc UErr
528 //cout << "AFTER u-=wdu wverr=" << tv*ev << endl;
529
530 // Now do error estimates.
531
532
533 if (uerrest.size()) {
534 Vec2& eu = Vec2::updAs(&uerrest[0]);
535 Vec2 uperp = VWinv*(VW*eu);
536
537 //cout << "ERREST before=" << uerrest
538 // << " wrms=" << wrms(uerrest,uweights) << endl;
539 //cout << " VW*eu=" << VW*eu << endl;
540 eu -= uperp;
541
542 //cout << "ERREST after=" << yerrest
543 // << " wrms=" << wrms(uerrest,uweights) << endl;
544 //cout << " VW*eu=" << VW*eu << endl;
545 }
546
547 results.setExitStatus(ProjectResults::Succeeded);
548 }
549
550 #endif /*SimTK_SIMMATH_PENDULUMSYSTEM_H_*/
551