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 *
26 * Implementation of SimbodyMatterSubtree and SimbodyMatterSubtreeResults.
27 */
28
29 #include "SimTKcommon.h"
30 #include "simbody/internal/SimbodyMatterSubtree.h"
31
32 #include "MobilizedBodyImpl.h"
33 #include "SimbodyMatterSubsystemRep.h"
34 class RigidBodyNode;
35
36 #include <string>
37 #include <iostream>
38 using std::cout;
39 using std::endl;
40
41 namespace SimTK {
42
43 /////////////////
44 // SUBTREE REP //
45 /////////////////
46
47 class SimbodyMatterSubtree::SubtreeRep {
48 public:
SubtreeRep(const SimbodyMatterSubtree & handle,const SimbodyMatterSubsystem & sms)49 SubtreeRep(const SimbodyMatterSubtree& handle, const SimbodyMatterSubsystem& sms)
50 : myHandle(handle), matter(&sms), stage(Stage::Empty)
51 {
52 }
53
54 // Here we don't know the matter subsystem yet.
SubtreeRep(const SimbodyMatterSubtree & handle)55 explicit SubtreeRep(const SimbodyMatterSubtree& handle)
56 : myHandle(handle), matter(0), stage(Stage::Empty)
57 {
58 }
59
setSimbodyMatterSubsystem(const SimbodyMatterSubsystem & sms)60 void setSimbodyMatterSubsystem(const SimbodyMatterSubsystem& sms) {
61 clear();
62 matter = &sms; // just a reference
63 }
64
getSimbodyMatterSubsystem() const65 const SimbodyMatterSubsystem& getSimbodyMatterSubsystem() const {
66 assert(matter != 0);
67 return *matter;
68 }
69
invalidate(Stage g)70 void invalidate(Stage g) {
71 if (stage >= g)
72 stage = g.prev();
73 }
74
75 // Note that this retains the current handle and matter subsystem (if any).
clear()76 void clear() {
77 invalidate(Stage::Topology);
78 terminalBodies.clear();
79 ancestor = InvalidMobilizedBodyIndex;
80 }
81
82
setTerminalBodies(const Array_<MobilizedBodyIndex> & bids)83 void setTerminalBodies(const Array_<MobilizedBodyIndex>& bids) {
84 clear();
85 for (int i=0; i < (int)bids.size(); ++i)
86 addTerminalBody(bids[i]);
87 }
88
addTerminalBody(MobilizedBodyIndex bid)89 void addTerminalBody(MobilizedBodyIndex bid) {
90 assert(!isTerminalBody(bid)); // can only appear once
91 invalidate(Stage::Topology);
92 terminalBodies.push_back(bid);
93 }
94
getAncestorMobilizedBodyIndex() const95 MobilizedBodyIndex getAncestorMobilizedBodyIndex() const {
96 assert(stage >= Stage::Topology);
97 return ancestor;
98 }
99
getSubtreeBodyMobilizedBodyIndex(SubtreeBodyIndex b) const100 MobilizedBodyIndex getSubtreeBodyMobilizedBodyIndex(SubtreeBodyIndex b) const {
101 assert(stage >= Stage::Topology);
102 return allBodies[b];
103 }
104
getParentSubtreeBodyIndex(SubtreeBodyIndex b) const105 SubtreeBodyIndex getParentSubtreeBodyIndex(SubtreeBodyIndex b) const {
106 assert(b >= 1); // ancestor has no subtree parent
107 assert(stage >= Stage::Topology);
108 return parentSubtreeBodies[b];
109 }
110
111 // State must be realized to at least Stage::Model for this call to work.
112 // The supplied SubtreeResults object is allocated and properly initialized to
113 // be able to hold computation results from this Subtree.
114 void initializeSubtreeResults(const State&, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
115
116 // This can be used as a sanity check that initializeSubtreeResults() was already called
117 // in this Subtree to produce these SubtreeResults. It is by no means exhaustive but
118 // will catch egregious errors.
119 bool isCompatibleSubtreeResults(const SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
120
121 // POSITION STAGE
122
123 // State must be realized to at least Stage::Position for this to work. SubtreeResults
124 // must have already been initialized to work with this Subtree. SubtreeResults stage
125 // will be Stage::Position after this call. All body transforms will be the same as
126 // the corresponding ones in the state, except they will be measured from the ancestor
127 // frame instead of ground. Subtree q's will be identical to corresponding State q's.
128 void copyPositionsFromState(const State&, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
129
130 // State must be realized to Stage::Instance. subQ must be the right length for this
131 // Subtree, and SubtreeResults must have been properly initialized. SubtreeResults
132 // stage will be Stage::Position after this call.
133 void calcPositionsFromSubtreeQ(const State&, const Vector& subQ, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
134
135 // Calculates a perturbed position result starting with the subQ's and position results
136 // which must already be in SubtreeResults.
137 void perturbPositions(const State&, SubtreeQIndex subQIndex, Real perturbation, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
138
139
140 // VELOCITY STAGE
141
142 // State must be realized to at least Stage::Velocity for this to work. SubtreeResults
143 // must already be at Stage::Position. SubtreeResults stage
144 // will be Stage::Velocity after this call. All subtree body spatial velocities will be
145 // the same as in the State, except measured relative to A and expressed in A. Subtree u's
146 // will be identical to corresponding State u's.
147 void copyVelocitiesFromState(const State&, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
148
149 // State must be realized to Stage::Instance. subU must be the right length for this
150 // Subtree, and SubtreeResults must already be at Stage::Position. SubtreeResults
151 // stage will be Stage::Velocity after this call.
152 void calcVelocitiesFromSubtreeU(const State&, const Vector& subU, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
153
154 // State must be realized to Stage::Instance and SubtreeResults must already be at
155 // Stage::Position. SubtreeResults stage will be Stage::Velocity after this call, but
156 // all Subtree u's and body velocities will be zero.
157 void calcVelocitiesFromZeroU(const State&, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
158
159 // Calculates a perturbed velocity result starting with the subU's and velocity results
160 // which must already be in SubtreeResults.
161 void perturbVelocities(const State&, SubtreeUIndex subUIndex, Real perturbation, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
162
163
164 // ACCELERATION STAGE
165
166 // State must be realized to at least Stage::Acceleration for this to work. SubtreeResults
167 // must already be at Stage::Velocity. SubtreeResults stage
168 // will be Stage::Acceleration after this call. All subtree body spatial accelerations will be
169 // the same as in the State, except measured relative to A and expressed in A. Subtree udots
170 // will be identical to corresponding State udots.
171 void copyAccelerationsFromState(const State&, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
172
173 // State must be realized to Stage::Instance. subUDot must be the right length for this
174 // Subtree, and SubtreeResults must already be at Stage::Velocity. SubtreeResults
175 // stage will be Stage::Acceleration after this call.
176 void calcAccelerationsFromSubtreeUDot(const State&, const Vector& subUDot, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
177
178 // State must be realized to Stage::Instance and SubtreeResults must already be at
179 // Stage::Velocity. SubtreeResults stage will be Stage::Acceleration after this call.
180 // All Subtree udots's will be zero, body accelerations will have only their bias values
181 // (coriolis accelerations from nonzero u's).
182 void calcAccelerationsFromZeroUDot(const State&, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
183
184 // Calculates a perturbed velocity result starting with the subUDot's and acceleration results
185 // which must already be in SubtreeResults.
186 void perturbAccelerations(const State&, SubtreeUIndex subUDotIndex, Real perturbation, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
187
getParentMobilizedBodyIndex(MobilizedBodyIndex childIx) const188 MobilizedBodyIndex getParentMobilizedBodyIndex(MobilizedBodyIndex childIx) const {
189 return getSimbodyMatterSubsystem().getMobilizedBody(childIx)
190 .getParentMobilizedBody().getMobilizedBodyIndex();
191 }
192
getLevel(MobilizedBodyIndex mbid) const193 int getLevel(MobilizedBodyIndex mbid) const {
194 return getSimbodyMatterSubsystem().getMobilizedBody(mbid).getLevelInMultibodyTree();
195 }
196
hasPathToAncestor(MobilizedBodyIndex bid) const197 bool hasPathToAncestor(MobilizedBodyIndex bid) const {
198 assert(ancestor.isValid());
199 while (bid!=ancestor && bid!=GroundIndex)
200 bid = getParentMobilizedBodyIndex(bid);
201 return bid == ancestor; // works if ancestor is Ground also
202 }
203
isTerminalBody(MobilizedBodyIndex bid) const204 bool isTerminalBody(MobilizedBodyIndex bid) const {
205 for (int i=0; i < (int)terminalBodies.size(); ++i)
206 if (bid == terminalBodies[i])
207 return true;
208 return false;
209 }
210
211
realizeTopology()212 void realizeTopology() {
213 if (stage >= Stage::Topology)
214 return;
215 allBodies.clear(); parentSubtreeBodies.clear(); childSubtreeBodies.clear();
216
217 if (terminalBodies.empty()) {
218 stage = Stage::Topology; // this is the "empty subtree"
219 return;
220 }
221
222 ancestor = findAncestorBody();
223
224 // We'll collect all the Subtree bodies in a MobilizedBodyIndex->SubtreeBodyIndex
225 // map. We'll do this in two passes through the map -- the first to eliminate
226 // duplicates and put the bodies in MobilizedBodyIndex order, the second to assign
227 // SubtreeBodyIndexs which are just the resulting ordinals.
228 // Complexity of this first pass is O(N log N) where N is the number
229 // of unique bodies in the Subtree.
230 typedef std::map<MobilizedBodyIndex, SubtreeBodyIndex> MapType;
231 typedef MapType::value_type MapEntry;
232 MapType subtreeBodyIndexMap;
233 // Pre-load the map with the ancestor body and its subtree body id 0.
234 subtreeBodyIndexMap.insert(MapEntry(ancestor, SubtreeBodyIndex(0)));
235 for (int i=0; i < (int)terminalBodies.size(); ++i) {
236 // Run down this branch adding any new bodies we encounter
237 // until we hit one that is already in the map. If we get to
238 // Ground without hitting the ancestor (OK if Ground *is* the
239 // ancestor) then we have been given a bad terminal body which
240 // should have been caught earlier.
241 MobilizedBodyIndex mbid = terminalBodies[i];
242 // ".second" will be true if the entry was actually inserted; otherwise
243 // it was already there.
244 while (subtreeBodyIndexMap.insert(MapEntry(mbid,SubtreeBodyIndex())).second) {
245 assert(mbid != GroundIndex);
246 mbid = getParentMobilizedBodyIndex(mbid);
247 }
248 }
249
250 // Now assign the SubtreeBodyIndexs in order of MobilizedBodyIndex, and fill
251 // in allBodies which serves as the SubtreeBodyIndex->MobilizedBodyIndex map,
252 // and parentSubtreeBodies which maps a subtree body to its unique subtree
253 // parent, and childSubtreeBodies which goes from a subtree body to the
254 // list of all bodies for which it is the parent.
255 // This pass is also O(N log N) because we have to look up the parent
256 // mobilized body id in the map to get its assigned subtree body id.
257
258 allBodies.resize((unsigned)subtreeBodyIndexMap.size());
259 parentSubtreeBodies.resize((unsigned)subtreeBodyIndexMap.size());
260 childSubtreeBodies.resize((unsigned)subtreeBodyIndexMap.size());
261 allBodies[0] = ancestor;
262 parentSubtreeBodies[0] = InvalidSubtreeBodyIndex;
263
264 SubtreeBodyIndex nextFree(1); // ancestor was already assigned 0
265 MapType::iterator body = subtreeBodyIndexMap.begin();
266 assert(body->first == ancestor && body->second == 0);
267 ++body; // skip the ancestor
268 for (; body != subtreeBodyIndexMap.end(); ++body)
269 {
270 const MobilizedBodyIndex mbid = body->first;
271 const SubtreeBodyIndex sbid = nextFree++;
272 body->second = sbid;
273 allBodies[sbid] = mbid;
274
275 // Look up the parent, which *must* be (a) present, and (b)
276 // one of the earlier Subtree bodies.
277 const MobilizedBodyIndex pmbid = getParentMobilizedBodyIndex(mbid);
278 MapType::const_iterator parent = subtreeBodyIndexMap.find(pmbid);
279 assert(parent != subtreeBodyIndexMap.end());
280
281 const SubtreeBodyIndex psbid = parent->second;
282 assert(psbid < sbid && allBodies[psbid]==pmbid);
283
284 parentSubtreeBodies[sbid] = psbid;
285 childSubtreeBodies[psbid].push_back(sbid);
286 }
287
288 stage = Stage::Topology;
289 }
290
getNumSubtreeBodies() const291 int getNumSubtreeBodies() const {
292 assert(stage >= Stage::Topology);
293 return (int)allBodies.size();
294 }
295
realizeModel(const State & s)296 void realizeModel(const State& s) {
297 assert(getSimbodyMatterSubsystem().getStage(s) >= Stage::Model);
298 assert(stage >= Stage::Topology);
299 if (stage >= Stage::Model)
300 return;
301
302 stage = Stage::Model;
303 }
304
realizePosition(const State & s,const Vector & subQ)305 void realizePosition(const State& s, const Vector& subQ) {
306 assert(getSimbodyMatterSubsystem().getStage(s) >= Stage::Instance);
307 assert(stage >= Stage::Model);
308
309 stage = Stage::Position;
310 }
311
realizeVelocity(const State & s,const Vector & subU)312 void realizeVelocity(const State& s, const Vector& subU) {
313 assert(getSimbodyMatterSubsystem().getStage(s) >= Stage::Position);
314 assert(stage >= Stage::Model);
315
316 stage = Stage::Velocity;
317 }
318
realizeAcceleration(const State & s,const Vector & subUDot)319 void realizeAcceleration(const State& s, const Vector& subUDot) {
320 assert(getSimbodyMatterSubsystem().getStage(s) >= Stage::Velocity);
321 assert(stage >= Stage::Model);
322
323 stage = Stage::Acceleration;
324 }
325
getMySubtreeOwnerHandle() const326 const SimbodyMatterSubtree& getMySubtreeOwnerHandle() const {return myHandle;}
327
328 private:
329 friend class SimbodyMatterSubtree;
330 const SimbodyMatterSubtree& myHandle; // owner handle
331 const SimbodyMatterSubsystem* matter; // a reference to the full tree of which this is a subset
332
333 Stage stage; // initially invalid
334
335 // TOPOLOGY STATE VARIABLES
336
337 Array_<MobilizedBodyIndex> terminalBodies;
338
339 // TOPOLOGY CACHE VARIABLES
340
341 // Maps SubtreeBodyIndex to MobilizedBodyIndex
342 MobilizedBodyIndex ancestor;
343 Array_<MobilizedBodyIndex> allBodies; // ancestor body is 0; ids are in increasing order
344
345 // Maps each subtree body (by SubtreeBodyIndex) to its unique parent within the subtree
346 // the base body (SubtreeBodyIndex==SubtreeBaseBodyIndex==0) returns an InvalidSubtreeBodyIndex
347 // as its parent.
348 Array_<SubtreeBodyIndex> parentSubtreeBodies;
349
350 // Maps each subtree body to its children within the subtree. Note that a subtree terminal
351 // body may have children in the full matter subsystem, but which are not included in
352 // the subtree.
353 Array_< Array_<SubtreeBodyIndex> > childSubtreeBodies;
354
355 private:
356
357 // This routine finds the terminal body closest to Ground in the
358 // MobilizedBody tree's graph, then moves down all the other branches
359 // to find a body in each branch at that same lowest level. That is,
360 // we "trim" all the branches to be the same height. Then we move
361 // all the branches in sync one level closer to ground until they
362 // all hit the same body. That's the outmost common ancestor.
findAncestorBody()363 MobilizedBodyIndex findAncestorBody() {
364 assert(terminalBodies.size());
365
366 // Copy the terminal bodies, which are the current branch tips.
367 Array_<MobilizedBodyIndex> tips(&terminalBodies[0], (&terminalBodies[0])+terminalBodies.size());
368
369 // Find the level of the lowest-level tip.
370 int minTip = 0;
371 int minLevel = getLevel(tips[minTip]);
372 for (int i=1; i < (int)tips.size(); ++i)
373 if (getLevel(tips[i]) < minLevel)
374 {minTip = i; minLevel = getLevel(tips[minTip]);}
375
376 // Trim all the other branches back to the lowest level.
377 for (int i=0; i < (int)tips.size(); ++i)
378 while (getLevel(tips[i]) > minLevel)
379 tips[i] = getParentMobilizedBodyIndex(tips[i]);
380
381 // All tips are at the same level now. March them in lockstep down
382 // to their common ancestor or Ground.
383 while (!allElementsMatch(tips))
384 pruneTipsOneLevel(tips);
385
386 return tips[0]; // all branches led here
387 }
388
allElementsMatch(const Array_<MobilizedBodyIndex> & ids)389 static bool allElementsMatch(const Array_<MobilizedBodyIndex>& ids) {
390 for (int i=1; i < (int)ids.size(); ++i)
391 if (ids[i] != ids[0]) return false;
392 return true;
393 }
394
pruneTipsOneLevel(Array_<MobilizedBodyIndex> & tips) const395 void pruneTipsOneLevel(Array_<MobilizedBodyIndex>& tips) const {
396 for (int i=0; i < (int)tips.size(); ++i) {
397 assert(tips[i] != GroundIndex); // can't happen: they should have matched!
398 tips[i] = getParentMobilizedBodyIndex(tips[i]);
399 }
400 }
401 };
402
403 /////////////////////////
404 // SUBTREE RESULTS REP //
405 /////////////////////////
406
407 class SimbodyMatterSubtreeResults::SubtreeResultsRep {
408 public:
SubtreeResultsRep(const SimbodyMatterSubtreeResults & handle)409 explicit SubtreeResultsRep(const SimbodyMatterSubtreeResults& handle)
410 : myHandle(&handle), stage(Stage::Empty)
411 {
412 clear();
413 }
414
clear()415 void clear() {
416 qSubset.clear(); uSubset.clear();
417 qSeg.clear(); uSeg.clear();
418 subQ.clear(); subU.clear(); subUDot.clear();
419 bodyTransforms.clear(); perturbedBodyTransforms.clear();
420 bodyVelocities.clear(); perturbedBodyVelocities.clear();
421 bodyAccelerations.clear(); perturbedBodyAccelerations.clear();
422 perturbedQ = InvalidSubtreeQIndex;
423 perturbedU = perturbedUDot = InvalidSubtreeUIndex;
424 stage = Stage::Empty;
425 }
426
427 // Set or change the number of Subtree bodies to be accommodated here. A
428 // "change" is cheap if the number of bodies hasn't actually changed.
reallocateBodies(int nb)429 void reallocateBodies(int nb) {
430 assert(nb >= 1); // must be at least the ancestor body
431 qSeg.resize(nb); uSeg.resize(nb);
432 bodyTransforms.resize(nb); perturbedBodyTransforms.resize(nb);
433 bodyVelocities.resize(nb); perturbedBodyVelocities.resize(nb);
434 bodyAccelerations.resize(nb); perturbedBodyAccelerations.resize(nb);
435
436 // Set the unchanging results for the ancestor body, which is treated as Ground.
437 qSeg[0] = pair<SubtreeQIndex,int>(SubtreeQIndex(0), 0); // no q's
438 uSeg[0] = pair<SubtreeUIndex,int>(SubtreeUIndex(0), 0); // no u's
439
440 bodyTransforms[0] = perturbedBodyTransforms[0] = Transform();
441 bodyVelocities[0] = perturbedBodyVelocities[0] = SpatialVec(Vec3(0), Vec3(0));
442 bodyAccelerations[0] = perturbedBodyAccelerations[0] = SpatialVec(Vec3(0), Vec3(0));
443
444 // Clear q and u mapping information -- that can't be known until Stage::Model.
445 qSubset.clear();
446 uSubset.clear();
447
448 perturbedQ = InvalidSubtreeQIndex;
449 perturbedU = perturbedUDot = InvalidSubtreeUIndex;
450 stage = Stage::Topology;
451 }
452
453 // Assign the next available Subtree q and u slots to the indicated body, and
454 // remember them. We are given the MatterSubsystem q's and u's associated with
455 // the corresponding mobilized body so we can keep a mapping.
addMobilities(SubtreeBodyIndex sb,QIndex qStart,int nq,UIndex uStart,int nu)456 void addMobilities(SubtreeBodyIndex sb, QIndex qStart, int nq, UIndex uStart, int nu) {
457 assert(stage >= Stage::Topology);
458 assert(nq >= 0 && nu >= 0 && nq >= nu);
459 assert(1 <= sb && sb < getNumSubtreeBodies());
460 stage = Stage::Topology; // back up if necessary
461
462 qSeg[sb] = pair<SubtreeQIndex,int>(SubtreeQIndex(qSubset.size()), nq);
463 uSeg[sb] = pair<SubtreeUIndex,int>(SubtreeUIndex(uSubset.size()), nu);
464
465 for (int i=0; i<nq; ++i)
466 qSubset.push_back(QIndex(qStart+i));
467 for (int i=0; i<nu; ++i)
468 uSubset.push_back(UIndex(uStart+i));
469 }
470
packStateQIntoSubtreeQ(const Vector & stateQ,Vector & subtreeQ) const471 void packStateQIntoSubtreeQ(const Vector& stateQ, Vector& subtreeQ) const {
472 assert(stage >= Stage::Model);
473 assert(stateQ.size() >= getNumSubtreeQs());
474
475 subtreeQ.resize(getNumSubtreeQs());
476 for (SubtreeQIndex i(0); i<getNumSubtreeQs(); ++i)
477 subtreeQ[i] = stateQ[qSubset[i]];
478 }
479
packStateUIntoSubtreeU(const Vector & stateU,Vector & subtreeU) const480 void packStateUIntoSubtreeU(const Vector& stateU, Vector& subtreeU) const {
481 assert(stage >= Stage::Model);
482 assert(stateU.size() >= getNumSubtreeUs());
483
484 subtreeU.resize(getNumSubtreeUs());
485 for (SubtreeUIndex i(0); i<getNumSubtreeUs(); ++i)
486 subtreeU[i] = stateU[uSubset[i]];
487 }
488
489
unpackSubtreeQIntoStateQ(const Vector & subtreeQ,Vector & stateQ) const490 void unpackSubtreeQIntoStateQ(const Vector& subtreeQ, Vector& stateQ) const {
491 assert(stage >= Stage::Model);
492 assert(subtreeQ.size() == getNumSubtreeQs());
493
494 for (SubtreeQIndex i(0); i<getNumSubtreeQs(); ++i)
495 stateQ[qSubset[i]] = subtreeQ[i];
496 }
497
498 // Call this when done adding mobilities.
realizeModel(const Vector & allQ,const Vector & allU)499 void realizeModel(const Vector& allQ, const Vector& allU) {
500 stage = Stage::Model; // enable routines used below
501 assert(allQ.size() >= getNumSubtreeQs() && allU.size() >= getNumSubtreeUs());
502
503 subQ.resize(getNumSubtreeQs());
504 subU.resize(getNumSubtreeUs());
505 subUDot.resize(getNumSubtreeUs());
506
507 packStateQIntoSubtreeQ(allQ,subQ); // set initial values
508 packStateUIntoSubtreeU(allU,subU);
509
510 subUDot = NaN;
511 }
512
mapSubtreeQToSubsystemQ(SubtreeQIndex sq) const513 QIndex mapSubtreeQToSubsystemQ(SubtreeQIndex sq) const {
514 assert(stage >= Stage::Model);
515 assert(0 <= sq && sq < (int)qSubset.size());
516 return qSubset[sq]; // range checked indexing
517 }
mapSubtreeUToSubsystemU(SubtreeUIndex su) const518 UIndex mapSubtreeUToSubsystemU(SubtreeUIndex su) const {
519 assert(stage >= Stage::Model);
520 assert(0 <= su && su < (int)uSubset.size());
521 return uSubset[su];
522 }
523
getNumSubtreeBodies() const524 int getNumSubtreeBodies() const {
525 assert(stage >= Stage::Topology);
526 return (int)bodyTransforms.size();
527 }
528
getNumSubtreeQs() const529 int getNumSubtreeQs() const {
530 assert(stage >= Stage::Model);
531 return (int)qSubset.size();
532 }
533
getNumSubtreeUs() const534 int getNumSubtreeUs() const {
535 assert(stage >= Stage::Model);
536 return (int)uSubset.size();
537 }
538
setMySubtreeResultsOwnerHandle(const SimbodyMatterSubtreeResults & owner)539 void setMySubtreeResultsOwnerHandle(const SimbodyMatterSubtreeResults& owner) {
540 myHandle = &owner;
541 }
542
getMySubtreeResultsOwnerHandle() const543 const SimbodyMatterSubtreeResults& getMySubtreeResultsOwnerHandle() const {
544 assert(myHandle);
545 return *myHandle;
546 }
547
getStage() const548 Stage getStage() const {return stage;}
setStage(Stage g)549 void setStage(Stage g) {stage=g;}
550
getSubQ() const551 const Vector& getSubQ() const {
552 assert(stage >= Stage::Position);
553 return subQ;
554 }
updSubQ()555 Vector& updSubQ() {
556 assert(stage >= Stage::Model);
557 if (stage >= Stage::Position) stage=Stage::Model;
558 return subQ;
559 }
getSubU() const560 const Vector& getSubU() const {
561 assert(stage >= Stage::Velocity);
562 return subU;
563 }
updSubU()564 Vector& updSubU() {
565 assert(stage >= Stage::Model);
566 if (stage >= Stage::Velocity) stage=Stage::Position;
567 return subU;
568 }
getSubUDot() const569 const Vector& getSubUDot() const {
570 assert(stage >= Stage::Acceleration);
571 return subUDot;
572 }
updSubUDot()573 Vector& updSubUDot() {
574 assert(stage >= Stage::Model);
575 if (stage >= Stage::Acceleration) stage=Stage::Velocity;
576 return subUDot;
577 }
578
getSubtreeBodyTransform(SubtreeBodyIndex sb)579 const Transform& getSubtreeBodyTransform(SubtreeBodyIndex sb) { // X_AB
580 assert(stage >= Stage::Position);
581 assert(0 <= sb && sb < (int)bodyTransforms.size());
582 return bodyTransforms[sb];
583 }
584
getSubtreeBodyVelocity(SubtreeBodyIndex sb)585 const SpatialVec& getSubtreeBodyVelocity(SubtreeBodyIndex sb) {
586 assert(stage >= Stage::Velocity);
587 assert(0 <= sb && sb < (int)bodyVelocities.size());
588 return bodyVelocities[sb];
589 }
590
getSubtreeBodyAcceleration(SubtreeBodyIndex sb)591 const SpatialVec& getSubtreeBodyAcceleration(SubtreeBodyIndex sb) {
592 assert(stage >= Stage::Acceleration);
593 assert(0 <= sb && sb < (int)bodyAccelerations.size());
594 return bodyAccelerations[sb];
595 }
596
setSubtreeBodyTransform(SubtreeBodyIndex sb,const Transform & X_AB)597 void setSubtreeBodyTransform(SubtreeBodyIndex sb, const Transform& X_AB) {
598 assert(stage >= Stage::Model);
599 assert(1 <= sb && sb < getNumSubtreeBodies()); // can't set Ancestor transform
600 bodyTransforms[sb] = X_AB;
601 }
602
setSubtreeBodyVelocity(SubtreeBodyIndex sb,const SpatialVec & V_AB)603 void setSubtreeBodyVelocity(SubtreeBodyIndex sb, const SpatialVec& V_AB) {
604 assert(stage >= Stage::Position);
605 assert(1 <= sb && sb < getNumSubtreeBodies()); // can't set Ancestor velocity
606 bodyVelocities[sb] = V_AB;
607 }
608
setSubtreeBodyAcceleration(SubtreeBodyIndex sb,const SpatialVec & A_AB)609 void setSubtreeBodyAcceleration(SubtreeBodyIndex sb, const SpatialVec& A_AB) {
610 assert(stage >= Stage::Velocity);
611 assert(1 <= sb && sb < getNumSubtreeBodies()); // can't set Ancestor velocity
612 bodyAccelerations[sb] = A_AB;
613 }
614
findSubtreeBodyQ(SubtreeBodyIndex sb,SubtreeQIndex & q,int & nq) const615 void findSubtreeBodyQ(SubtreeBodyIndex sb, SubtreeQIndex& q, int& nq) const {
616 assert(stage >= Stage::Model);
617 q = qSeg[sb].first;
618 nq = qSeg[sb].second;
619 }
620
findSubtreeBodyU(SubtreeBodyIndex sb,SubtreeUIndex & u,int & nu) const621 void findSubtreeBodyU(SubtreeBodyIndex sb, SubtreeUIndex& u, int& nu) const {
622 assert(stage >= Stage::Model);
623 u = uSeg[sb].first;
624 nu = uSeg[sb].second;
625 }
626 private:
627 friend class SimbodyMatterSubtreeResults;
628 const SimbodyMatterSubtreeResults* myHandle; // owner handle
629
630 Stage stage; // initially invalid
631
632 // Model stage information
633 Array_< QIndex > qSubset; // map from SubtreeQIndex to MatterSubsystem q
634 Array_< UIndex > uSubset; // map from SubtreeUIndex to MatterSubsystem u (also udot)
635
636 // These identify which mobilities go with which bodies.
637 Array_< pair<SubtreeQIndex,int> > qSeg; // map from SubtreeBodyIndex to qSubset offset, length
638 Array_< pair<SubtreeUIndex,int> > uSeg; // map from SubtreeBodyIndex to uSubset offset, length
639
640 //TODO: make PIMPL
641 Vector subQ; // generalized coords for Subtree bodies
642 Array_<Transform> bodyTransforms; // X_AB, index by SubtreeBodyIndex (unperturbed)
643
644 SubtreeQIndex perturbedQ; // which Q was perturbed? InvalidSubtreeQIndex if none
645 Array_<Transform> perturbedBodyTransforms; // X_AB, after perturbation
646
647 Vector subU; // generalized speeds for Subtree bodies
648 Vector_<SpatialVec> bodyVelocities; // V_AB, index by SubtreeBodyIndex
649
650 SubtreeUIndex perturbedU; // which u was perturbed? InvalidSubtreeUIndex if none
651 Vector_<SpatialVec> perturbedBodyVelocities; // V_AB, after perturbation
652
653 Vector subUDot; // generalized accelerations for Subtree bodies
654 Vector_<SpatialVec> bodyAccelerations; // A_AB, index by SubtreeBodyIndex
655
656 SubtreeUIndex perturbedUDot; // which udot was perturbed? InvalidSubtreeUIndex if none
657 Vector_<SpatialVec> perturbedBodyAccelerations; // A_AB, after perturbation
658 };
659
660
661 ////////////////////////////
662 // SIMBODY MATTER SUBTREE //
663 ////////////////////////////
664
665 // Default constructor -- we don't know the SimbodyMatterSubsystem yet.
SimbodyMatterSubtree()666 SimbodyMatterSubtree::SimbodyMatterSubtree()
667 : rep(0)
668 {
669 rep = new SubtreeRep(*this);
670 }
671
SimbodyMatterSubtree(const SimbodyMatterSubsystem & matter)672 SimbodyMatterSubtree::SimbodyMatterSubtree(const SimbodyMatterSubsystem& matter)
673 : rep(0)
674 {
675 rep = new SubtreeRep(*this, matter);
676 }
677
678
SimbodyMatterSubtree(const SimbodyMatterSubsystem & matter,const Array_<MobilizedBodyIndex> & terminalBodies)679 SimbodyMatterSubtree::SimbodyMatterSubtree(const SimbodyMatterSubsystem& matter,
680 const Array_<MobilizedBodyIndex>& terminalBodies)
681 : rep(0)
682 {
683 rep = new SubtreeRep(*this, matter);
684 rep->setTerminalBodies(terminalBodies);
685 }
686
687 // Copy constructor
SimbodyMatterSubtree(const SimbodyMatterSubtree & src)688 SimbodyMatterSubtree::SimbodyMatterSubtree(const SimbodyMatterSubtree& src)
689 : rep(0)
690 {
691 if (src.rep)
692 rep = new SubtreeRep(*src.rep);
693 }
694
695 // Copy assignment
696 SimbodyMatterSubtree&
operator =(const SimbodyMatterSubtree & src)697 SimbodyMatterSubtree::operator=(const SimbodyMatterSubtree& src)
698 {
699 if (&src != this) {
700 if (rep && (this == &rep->getMySubtreeOwnerHandle()))
701 delete rep;
702 rep = 0;
703 if (src.rep)
704 rep = new SubtreeRep(*src.rep);
705 }
706 return *this;
707 }
708
709 // Destructor
~SimbodyMatterSubtree()710 SimbodyMatterSubtree::~SimbodyMatterSubtree() {
711 if (rep && (this == &rep->getMySubtreeOwnerHandle()))
712 delete rep;
713 rep=0;
714 }
715
716 const SimbodyMatterSubsystem&
getSimbodyMatterSubsystem() const717 SimbodyMatterSubtree::getSimbodyMatterSubsystem() const {
718 return getRep().getSimbodyMatterSubsystem();
719 }
720
721
setSimbodyMatterSubsystem(const SimbodyMatterSubsystem & matter)722 void SimbodyMatterSubtree::setSimbodyMatterSubsystem(const SimbodyMatterSubsystem& matter) {
723 return updRep().setSimbodyMatterSubsystem(matter);
724 }
725
clear()726 void SimbodyMatterSubtree::clear() {
727 return updRep().clear();
728 }
729
730
731 SimbodyMatterSubtree&
addTerminalBody(MobilizedBodyIndex i)732 SimbodyMatterSubtree::addTerminalBody(MobilizedBodyIndex i) {
733 updRep().addTerminalBody(i);
734 return *this;
735 }
736
realizeTopology()737 void SimbodyMatterSubtree::realizeTopology() {
738 updRep().realizeTopology();
739 }
740
getAncestorMobilizedBodyIndex() const741 MobilizedBodyIndex SimbodyMatterSubtree::getAncestorMobilizedBodyIndex() const {
742 return getRep().ancestor;
743 }
744
745 const Array_<MobilizedBodyIndex>&
getTerminalBodies() const746 SimbodyMatterSubtree::getTerminalBodies() const {
747 return getRep().terminalBodies;
748 }
749
getNumSubtreeBodies() const750 int SimbodyMatterSubtree::getNumSubtreeBodies() const {
751 return (int)getRep().allBodies.size();
752 }
753
754 const Array_<MobilizedBodyIndex>&
getAllBodies() const755 SimbodyMatterSubtree::getAllBodies() const {
756 assert(getRep().stage >= Stage::Topology);
757 return getRep().allBodies;
758 }
759
760 SubtreeBodyIndex
getParentSubtreeBodyIndex(SubtreeBodyIndex sbid) const761 SimbodyMatterSubtree::getParentSubtreeBodyIndex(SubtreeBodyIndex sbid) const {
762 assert(getRep().stage >= Stage::Topology);
763 return getRep().parentSubtreeBodies[sbid];
764 }
765 const Array_<SubtreeBodyIndex>&
getChildSubtreeBodyIndices(SubtreeBodyIndex sbid) const766 SimbodyMatterSubtree::getChildSubtreeBodyIndices(SubtreeBodyIndex sbid) const {
767 assert(getRep().stage >= Stage::Topology);
768 return getRep().childSubtreeBodies[sbid];
769 }
770
771 bool SimbodyMatterSubtree::
isCompatibleSubtreeResults(const SimbodyMatterSubtreeResults & sr) const772 isCompatibleSubtreeResults(const SimbodyMatterSubtreeResults& sr) const {
773 return getRep().isCompatibleSubtreeResults(sr.getRep());
774 }
775
initializeSubtreeResults(const State & s,SimbodyMatterSubtreeResults & sr) const776 void SimbodyMatterSubtree::initializeSubtreeResults(const State& s, SimbodyMatterSubtreeResults& sr) const {
777 getRep().initializeSubtreeResults(s,sr.updRep());
778 }
779
780
781 void SimbodyMatterSubtree::
copyPositionsFromState(const State & s,SimbodyMatterSubtreeResults & sr) const782 copyPositionsFromState(const State& s, SimbodyMatterSubtreeResults& sr) const {
783 getRep().copyPositionsFromState(s,sr.updRep());
784 }
785
786 void SimbodyMatterSubtree::
calcPositionsFromSubtreeQ(const State & s,const Vector & subQ,SimbodyMatterSubtreeResults & sr) const787 calcPositionsFromSubtreeQ(const State& s, const Vector& subQ, SimbodyMatterSubtreeResults& sr) const {
788 getRep().calcPositionsFromSubtreeQ(s,subQ,sr.updRep());
789 }
790
791 void SimbodyMatterSubtree::
perturbPositions(const State & s,SubtreeQIndex subQIndex,Real perturbation,SimbodyMatterSubtreeResults & sr) const792 perturbPositions(const State& s, SubtreeQIndex subQIndex, Real perturbation, SimbodyMatterSubtreeResults& sr) const {
793 getRep().perturbPositions(s,subQIndex,perturbation,sr.updRep());
794 }
795
796 void SimbodyMatterSubtree::
copyVelocitiesFromState(const State & s,SimbodyMatterSubtreeResults & sr) const797 copyVelocitiesFromState(const State& s, SimbodyMatterSubtreeResults& sr) const {
798 getRep().copyVelocitiesFromState(s,sr.updRep());
799 }
800
801 void SimbodyMatterSubtree::
calcVelocitiesFromSubtreeU(const State & s,const Vector & subU,SimbodyMatterSubtreeResults & sr) const802 calcVelocitiesFromSubtreeU(const State& s, const Vector& subU, SimbodyMatterSubtreeResults& sr) const {
803 getRep().calcVelocitiesFromSubtreeU(s,subU,sr.updRep());
804 }
805
806 void SimbodyMatterSubtree::
calcVelocitiesFromZeroU(const State & s,SimbodyMatterSubtreeResults & sr) const807 calcVelocitiesFromZeroU(const State& s, SimbodyMatterSubtreeResults& sr) const {
808 getRep().calcVelocitiesFromZeroU(s,sr.updRep());
809 }
810
811 void SimbodyMatterSubtree::
perturbVelocities(const State & s,SubtreeUIndex subUIndex,Real perturbation,SimbodyMatterSubtreeResults & sr) const812 perturbVelocities(const State& s, SubtreeUIndex subUIndex, Real perturbation, SimbodyMatterSubtreeResults& sr) const {
813 getRep().perturbVelocities(s,subUIndex,perturbation,sr.updRep());
814 }
815
816 void SimbodyMatterSubtree::
copyAccelerationsFromState(const State & s,SimbodyMatterSubtreeResults & sr) const817 copyAccelerationsFromState(const State& s, SimbodyMatterSubtreeResults& sr) const {
818 getRep().copyAccelerationsFromState(s,sr.updRep());
819 }
820
821 void SimbodyMatterSubtree::
calcAccelerationsFromSubtreeUDot(const State & s,const Vector & subUDot,SimbodyMatterSubtreeResults & sr) const822 calcAccelerationsFromSubtreeUDot(const State& s, const Vector& subUDot, SimbodyMatterSubtreeResults& sr) const {
823 getRep().calcAccelerationsFromSubtreeUDot(s,subUDot,sr.updRep());
824 }
825
826 void SimbodyMatterSubtree::
calcAccelerationsFromZeroUDot(const State & s,SimbodyMatterSubtreeResults & sr) const827 calcAccelerationsFromZeroUDot(const State& s, SimbodyMatterSubtreeResults& sr) const {
828 getRep().calcAccelerationsFromZeroUDot(s,sr.updRep());
829 }
830
831 void SimbodyMatterSubtree::
perturbAccelerations(const State & s,SubtreeUIndex subUDotIndex,Real perturbation,SimbodyMatterSubtreeResults & sr) const832 perturbAccelerations(const State& s, SubtreeUIndex subUDotIndex, Real perturbation, SimbodyMatterSubtreeResults& sr) const {
833 getRep().perturbAccelerations(s,subUDotIndex,perturbation,sr.updRep());
834 }
835
836
837
838 ////////////////////////////////////
839 // SIMBODY MATTER SUBTREE RESULTS //
840 ////////////////////////////////////
841
SimbodyMatterSubtreeResults()842 SimbodyMatterSubtreeResults::SimbodyMatterSubtreeResults() : rep(0) {
843 rep = new SubtreeResultsRep(*this);
844 }
845
~SimbodyMatterSubtreeResults()846 SimbodyMatterSubtreeResults::~SimbodyMatterSubtreeResults() {
847 if (rep && this == rep->myHandle)
848 delete rep;
849 rep = 0;
850 }
851
SimbodyMatterSubtreeResults(const SimbodyMatterSubtreeResults & src)852 SimbodyMatterSubtreeResults::SimbodyMatterSubtreeResults(const SimbodyMatterSubtreeResults& src) : rep(0) {
853 if (src.rep) {
854 rep = new SubtreeResultsRep(*src.rep);
855 rep->setMySubtreeResultsOwnerHandle(*this);
856 }
857 }
858
859 SimbodyMatterSubtreeResults&
operator =(const SimbodyMatterSubtreeResults & src)860 SimbodyMatterSubtreeResults::operator=(const SimbodyMatterSubtreeResults& src) {
861 if (&src != this) {
862 if (rep && this == rep->myHandle)
863 delete rep;
864 rep = 0;
865 if (src.rep) {
866 rep = new SubtreeResultsRep(*src.rep);
867 rep->setMySubtreeResultsOwnerHandle(*this);
868 }
869 }
870 return *this;
871 }
872
getNumSubtreeBodies() const873 int SimbodyMatterSubtreeResults::getNumSubtreeBodies() const {
874 return getRep().getNumSubtreeBodies();
875 }
876
getNumSubtreeQs() const877 int SimbodyMatterSubtreeResults::getNumSubtreeQs() const {
878 return getRep().getNumSubtreeQs();
879 }
880
getNumSubtreeUs() const881 int SimbodyMatterSubtreeResults::getNumSubtreeUs() const {
882 return getRep().getNumSubtreeUs();
883 }
884
reallocateBodies(int nb)885 void SimbodyMatterSubtreeResults::reallocateBodies(int nb) {
886 updRep().reallocateBodies(nb);
887 }
888
addMobilities(SubtreeBodyIndex sb,QIndex qStart,int nq,UIndex uStart,int nu)889 void SimbodyMatterSubtreeResults::addMobilities
890 (SubtreeBodyIndex sb, QIndex qStart, int nq, UIndex uStart, int nu)
891 {
892 updRep().addMobilities(sb, qStart, nq, uStart, nu);
893 }
894
realizeModel(const Vector & stateQ,const Vector & stateU)895 void SimbodyMatterSubtreeResults::realizeModel(const Vector& stateQ, const Vector& stateU) {
896 updRep().realizeModel(stateQ, stateU);
897 }
898
getStage() const899 Stage SimbodyMatterSubtreeResults::getStage() const {
900 return getRep().stage;
901 }
902
getQSubset() const903 const Array_<QIndex>& SimbodyMatterSubtreeResults::getQSubset() const {
904 assert(getRep().stage >= Stage::Model);
905 return getRep().qSubset;
906 }
907
getUSubset() const908 const Array_<UIndex>& SimbodyMatterSubtreeResults::getUSubset() const {
909 assert(getRep().stage >= Stage::Model);
910 return getRep().uSubset;
911 }
912
findSubtreeBodyQ(SubtreeBodyIndex sbid,SubtreeQIndex & qStart,int & nq) const913 void SimbodyMatterSubtreeResults::findSubtreeBodyQ(SubtreeBodyIndex sbid, SubtreeQIndex& qStart, int& nq) const {
914 assert(getStage() >= Stage::Model);
915 const pair<SubtreeQIndex,int>& seg = getRep().qSeg[sbid];
916 qStart = seg.first;
917 nq = seg.second;
918 }
919
findSubtreeBodyU(SubtreeBodyIndex sbid,SubtreeUIndex & uStart,int & nu) const920 void SimbodyMatterSubtreeResults::findSubtreeBodyU(SubtreeBodyIndex sbid, SubtreeUIndex& uStart, int& nu) const {
921 assert(getStage() >= Stage::Model);
922 const pair<SubtreeUIndex,int>& seg = getRep().uSeg[sbid];
923 uStart = seg.first;
924 nu = seg.second;
925 }
926
getSubtreeQ() const927 const Vector& SimbodyMatterSubtreeResults::getSubtreeQ() const {
928 assert(getStage() >= Stage::Position);
929 return getRep().subQ;
930 }
getSubtreeBodyTransform(SubtreeBodyIndex sbid) const931 const Transform& SimbodyMatterSubtreeResults::getSubtreeBodyTransform(SubtreeBodyIndex sbid) const {
932 assert(getStage() >= Stage::Position);
933 return getRep().bodyTransforms[sbid];
934 }
935
getSubtreeU() const936 const Vector& SimbodyMatterSubtreeResults::getSubtreeU() const {
937 assert(getStage() >= Stage::Velocity);
938 return getRep().subU;
939 }
getSubtreeBodyVelocity(SubtreeBodyIndex sbid) const940 const SpatialVec& SimbodyMatterSubtreeResults::getSubtreeBodyVelocity(SubtreeBodyIndex sbid) const {
941 assert(getStage() >= Stage::Velocity);
942 return getRep().bodyVelocities[sbid];
943 }
944
getSubtreeUDot() const945 const Vector& SimbodyMatterSubtreeResults::getSubtreeUDot() const {
946 assert(getStage() >= Stage::Acceleration);
947 return getRep().subUDot;
948 }
getSubtreeBodyAcceleration(SubtreeBodyIndex sbid) const949 const SpatialVec& SimbodyMatterSubtreeResults::getSubtreeBodyAcceleration(SubtreeBodyIndex sbid) const {
950 assert(getStage() >= Stage::Acceleration);
951 return getRep().bodyAccelerations[sbid];
952 }
953
operator <<(std::ostream & o,const SimbodyMatterSubtree & sub)954 std::ostream& operator<<(std::ostream& o, const SimbodyMatterSubtree& sub) {
955 o << "SUBTREE:" << endl;
956 o << " ancestor=" << sub.getAncestorMobilizedBodyIndex();
957
958 o << " terminalBodies=";
959 for (int i=0; i < (int)sub.getTerminalBodies().size(); ++i)
960 o << sub.getTerminalBodies()[i] << " ";
961 o << endl;
962
963 o << " allBodies=";
964 for (int i=0; i < (int)sub.getAllBodies().size(); ++i)
965 o << sub.getAllBodies()[i] << " ";
966 o << endl;
967
968 for (SubtreeBodyIndex b(0); b < (int)sub.getAllBodies().size(); ++b) {
969 o << " parent[" << b << "]=" << sub.getParentSubtreeBodyIndex(b);
970
971 o << " children[" << b << "]=";
972 for (int i=0; i < (int)sub.getChildSubtreeBodyIndices(b).size(); ++i)
973 o << sub.getChildSubtreeBodyIndices(b)[i] << " ";
974 o << endl;
975 }
976
977 return o;
978 }
979
operator <<(std::ostream & o,const Array_<QIndex> & q)980 static std::ostream& operator<<(std::ostream& o, const Array_<QIndex>& q) {
981 for (int i=0; i<(int)q.size(); ++i)
982 o << q[i] << " ";
983 return o;
984 }
985
operator <<(std::ostream & o,const Array_<UIndex> & u)986 static std::ostream& operator<<(std::ostream& o, const Array_<UIndex>& u) {
987 for (int i=0; i<(int)u.size(); ++i)
988 o << u[i] << " ";
989 return o;
990 }
991
operator <<(std::ostream & o,const SimbodyMatterSubtreeResults & sr)992 std::ostream& operator<<(std::ostream& o, const SimbodyMatterSubtreeResults& sr) {
993 o << "SUBTREE RESULTS (stage=" << sr.getStage() << "):" << endl;
994
995 if (sr.getStage() >= Stage::Topology)
996 o << " " << sr.getNumSubtreeBodies() << " subtree bodies" << endl;
997
998 if (sr.getStage() >= Stage::Model) {
999 o << " nq=" << sr.getNumSubtreeQs() << ", nu=" << sr.getNumSubtreeUs() << endl;
1000 o << " QSubset: " << sr.getQSubset() << endl;
1001 o << " USubset: " << sr.getUSubset() << endl;
1002
1003 for (SubtreeBodyIndex sb(1); sb < sr.getNumSubtreeBodies(); ++sb) {
1004 SubtreeQIndex qstart; int nq;
1005 SubtreeUIndex ustart; int nu;
1006 sr.findSubtreeBodyQ(sb,qstart,nq);
1007 sr.findSubtreeBodyU(sb,ustart,nu);
1008 o << " body " << sb << " q=" << qstart << ".." << qstart+nq-1
1009 << " u=" << ustart << ".." << ustart+nu-1 << endl;
1010 }
1011 }
1012
1013 if (sr.getStage() >= Stage::Position) {
1014 o << " POSITION RESULTS AVAILABLE:\n";
1015 o << " q=" << sr.getSubtreeQ() << endl;
1016 for (SubtreeBodyIndex sb(0); sb < sr.getNumSubtreeBodies(); ++sb)
1017 o << " X_AB" << sb << "=" << sr.getSubtreeBodyTransform(sb);
1018 }
1019
1020 if (sr.getStage() >= Stage::Velocity) {
1021 o << " VELOCITY RESULTS AVAILABLE\n";
1022 o << " u=" << sr.getSubtreeU() << endl;
1023 for (SubtreeBodyIndex sb(0); sb < sr.getNumSubtreeBodies(); ++sb)
1024 o << " V_AB" << sb << "=" << sr.getSubtreeBodyVelocity(sb) << endl;
1025 }
1026
1027 if (sr.getStage() >= Stage::Acceleration) {
1028 o << " ACCELERATION RESULTS AVAILABLE\n";
1029 o << " udot=" << sr.getSubtreeUDot() << endl;
1030 for (SubtreeBodyIndex sb(0); sb < sr.getNumSubtreeBodies(); ++sb)
1031 o << " A_AB" << sb << "=" << sr.getSubtreeBodyAcceleration(sb) << endl;
1032 }
1033
1034 o << "END SUBTREE RESULTS." << endl;
1035
1036 return o;
1037 }
1038
1039
1040 ////////////////////////////////////////////////////////
1041 // SIMBODY MATTER SUBSYSTEM :: SUBTREE :: SUBTREE REP //
1042 ////////////////////////////////////////////////////////
1043
1044 void SimbodyMatterSubtree::SubtreeRep::
initializeSubtreeResults(const State & s,SimbodyMatterSubtreeResults::SubtreeResultsRep & sr) const1045 initializeSubtreeResults(const State& s, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
1046 const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
1047 SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Model,
1048 "Subtree::initializeSubtreeResults()");
1049
1050 const int nSubtreeBodies = getNumSubtreeBodies();
1051 sr.reallocateBodies(nSubtreeBodies);
1052
1053 int nSubtreeQ=0, nSubtreeU=0;
1054 // start at 1 because ancestor has no relevant mobilities
1055 for (SubtreeBodyIndex sb(1); sb < nSubtreeBodies; ++sb) {
1056 const MobilizedBodyIndex mb = allBodies[sb];
1057
1058 QIndex qStart; int nq;
1059 UIndex uStart; int nu;
1060 matter.findMobilizerQs(s, mb, qStart, nq);
1061 matter.findMobilizerUs(s, mb, uStart, nu);
1062 nSubtreeQ += nq; nSubtreeU += nu;
1063
1064 sr.addMobilities(sb, qStart, nq, uStart, nu);
1065 }
1066
1067 sr.realizeModel(matter.getQ(s), matter.getU(s));
1068 assert(nSubtreeQ == sr.getNumSubtreeQs() && nSubtreeU == sr.getNumSubtreeUs());
1069 }
1070
1071 bool SimbodyMatterSubtree::SubtreeRep::
isCompatibleSubtreeResults(const SimbodyMatterSubtreeResults::SubtreeResultsRep & sr) const1072 isCompatibleSubtreeResults(const SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
1073 // hardly exhaustive but better than nothing
1074 return sr.getStage() >= Stage::Model
1075 && sr.getNumSubtreeBodies() == getNumSubtreeBodies();
1076 }
1077
1078 void SimbodyMatterSubtree::SubtreeRep::
copyPositionsFromState(const State & s,SimbodyMatterSubtreeResults::SubtreeResultsRep & sr) const1079 copyPositionsFromState(const State& s, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
1080 const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
1081 SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Position,
1082 "Subtree::calcPositionsFromState()");
1083
1084 assert(isCompatibleSubtreeResults(sr));
1085
1086 // Copy the q's; adjust the body transforms to be relative to the ancestor
1087 // body instead of ground.
1088 sr.packStateQIntoSubtreeQ(matter.getQ(s), sr.updSubQ());
1089
1090 if (getAncestorMobilizedBodyIndex() == GroundIndex) {
1091 for (SubtreeBodyIndex sb(1); sb < getNumSubtreeBodies(); ++sb) {
1092 const MobilizedBodyIndex mb = getSubtreeBodyMobilizedBodyIndex(sb);
1093 const Transform& X_GB = matter.getBodyTransform(s,mb);
1094 sr.setSubtreeBodyTransform(sb, X_GB); // =X_AB
1095 }
1096 } else {
1097 // Ancestor A differs from Ground G so we have to adjust all the
1098 // Subtree body transforms to measure from A instead of G.
1099 const Transform& X_GA = matter.getBodyTransform(s,getAncestorMobilizedBodyIndex());
1100 for (SubtreeBodyIndex sb(1); sb < getNumSubtreeBodies(); ++sb) {
1101 const MobilizedBodyIndex mb = getSubtreeBodyMobilizedBodyIndex(sb);
1102 const Transform& X_GB = matter.getBodyTransform(s,mb);
1103 sr.setSubtreeBodyTransform(sb, ~X_GA * X_GB); // X_AB
1104 }
1105 }
1106
1107 sr.setStage(Stage::Position);
1108 }
1109
1110 // Here we are given a new set of Subtree q's, and we want to calculate the resulting A-relative
1111 // transforms for all the Subtree bodies. This requires calculating the cross-mobilizer transforms
1112 // X_FM for each Subtree mobilizer and propagating them outwards towards the terminal bodies.
1113 void SimbodyMatterSubtree::SubtreeRep::
calcPositionsFromSubtreeQ(const State & state,const Vector & subQ,SimbodyMatterSubtreeResults::SubtreeResultsRep & results) const1114 calcPositionsFromSubtreeQ(const State& state, const Vector& subQ,
1115 SimbodyMatterSubtreeResults::SubtreeResultsRep& results) const
1116 {
1117 const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
1118 SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(state), Stage::Instance,
1119 "calcPositionsFromSubtreeQ()");
1120
1121 assert(isCompatibleSubtreeResults(results));
1122 assert(subQ.size() == results.getNumSubtreeQs());
1123
1124 results.updSubQ() = subQ; // copy in the q's
1125
1126 // For high speed, find memory address for the first subQ; they are sequential after this.
1127 const Real* allSubQ = &results.getSubQ()[0];
1128
1129 // Iterate from the ancestor outward to propagate the transforms to the terminal bodies.
1130 for (SubtreeBodyIndex sb(1); sb < getNumSubtreeBodies(); ++sb) {
1131 const SubtreeBodyIndex sp = getParentSubtreeBodyIndex(sb);
1132 const MobilizedBodyIndex mb = getSubtreeBodyMobilizedBodyIndex(sb);
1133
1134 SubtreeQIndex firstSubQ; int nq;
1135 results.findSubtreeBodyQ(sb, firstSubQ, nq);
1136
1137 const Transform X_PB = matter.calcMobilizerTransformFromQ(state, mb, nq, &allSubQ[firstSubQ]);
1138 const Transform& X_AP = results.getSubtreeBodyTransform(sp);
1139 results.setSubtreeBodyTransform(sb, X_AP*X_PB);
1140 }
1141
1142 results.setStage(Stage::Position);
1143 }
1144
1145 void SimbodyMatterSubtree::SubtreeRep::
perturbPositions(const State & s,SubtreeQIndex subQIndex,Real perturbation,SimbodyMatterSubtreeResults::SubtreeResultsRep & sr) const1146 perturbPositions(const State& s, SubtreeQIndex subQIndex, Real perturbation, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
1147 const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
1148 SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Instance,
1149 "perturbPositions()");
1150
1151 assert(isCompatibleSubtreeResults(sr));
1152 assert(sr.getStage() >= Stage::Position);
1153
1154 assert(!"not implemented yet");
1155 }
1156
1157 void SimbodyMatterSubtree::SubtreeRep::
copyVelocitiesFromState(const State & s,SimbodyMatterSubtreeResults::SubtreeResultsRep & sr) const1158 copyVelocitiesFromState(const State& s, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
1159 const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
1160 SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Velocity,
1161 "calcVelocitiesFromState()");
1162
1163 assert(isCompatibleSubtreeResults(sr));
1164
1165 // Copy the u's; adjust the body velocities to be measured relative to,
1166 // and expressed in, the ancestor body instead of ground.
1167 sr.packStateUIntoSubtreeU(matter.getU(s), sr.updSubU());
1168
1169 if (getAncestorMobilizedBodyIndex() == GroundIndex) {
1170 for (SubtreeBodyIndex sb(1); sb < getNumSubtreeBodies(); ++sb) {
1171 const MobilizedBodyIndex mb = getSubtreeBodyMobilizedBodyIndex(sb);
1172 const SpatialVec& V_GB = matter.getBodyVelocity(s,mb);
1173 sr.setSubtreeBodyVelocity(sb, V_GB); // =V_AB
1174 }
1175 } else {
1176 // Ancestor A differs from Ground G so we have to adjust all the
1177 // Subtree body velocities to measure from A instead of G.
1178 const Transform& X_GA = matter.getBodyTransform(s,getAncestorMobilizedBodyIndex());
1179 const SpatialVec& V_GA = matter.getBodyVelocity(s,getAncestorMobilizedBodyIndex());
1180 for (SubtreeBodyIndex sb(1); sb < getNumSubtreeBodies(); ++sb) {
1181 const MobilizedBodyIndex mb = getSubtreeBodyMobilizedBodyIndex(sb);
1182 const Transform& X_GB = matter.getBodyTransform(s,mb);
1183 const SpatialVec& V_GB = matter.getBodyVelocity(s,mb);
1184
1185 const Vec3 p_AB_G = X_GB.p() - X_GA.p();
1186 const Vec3 p_AB_G_dot = V_GB[1] - V_GA[1]; // time deriv of p taken in G
1187
1188 const Vec3 w_AB_G = V_GB[0] - V_GA[0]; // relative angular velocity
1189 const Vec3 v_AB_G = p_AB_G_dot - V_GA[0] % p_AB_G; // time deriv of p in A, exp in G
1190 const SpatialVec V_AB = ~X_GA.R() * SpatialVec(w_AB_G, v_AB_G); // re-express in A
1191 sr.setSubtreeBodyVelocity(sb, V_AB);
1192 }
1193 }
1194
1195 sr.setStage(Stage::Velocity);
1196 }
1197
1198 void SimbodyMatterSubtree::SubtreeRep::
calcVelocitiesFromSubtreeU(const State & s,const Vector & subU,SimbodyMatterSubtreeResults::SubtreeResultsRep & sr) const1199 calcVelocitiesFromSubtreeU(const State& s, const Vector& subU, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
1200 const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
1201 SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Instance,
1202 "calcVelocitiesFromSubtreeU()");
1203
1204 assert(isCompatibleSubtreeResults(sr));
1205
1206 assert(!"not implemented yet");
1207 }
1208
1209 void SimbodyMatterSubtree::SubtreeRep::
calcVelocitiesFromZeroU(const State & s,SimbodyMatterSubtreeResults::SubtreeResultsRep & sr) const1210 calcVelocitiesFromZeroU(const State& s, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
1211 const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
1212 SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Instance,
1213 "calcVelocitiesFromZeroU()");
1214
1215 assert(isCompatibleSubtreeResults(sr));
1216
1217 assert(!"not implemented yet");
1218 }
1219
1220 void SimbodyMatterSubtree::SubtreeRep::
perturbVelocities(const State & s,SubtreeUIndex subUIndex,Real perturbation,SimbodyMatterSubtreeResults::SubtreeResultsRep & sr) const1221 perturbVelocities(const State& s, SubtreeUIndex subUIndex, Real perturbation, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
1222 const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
1223 SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Instance,
1224 "perturbVelocities()");
1225
1226 assert(isCompatibleSubtreeResults(sr));
1227 assert(sr.getStage() >= Stage::Velocity);
1228
1229 assert(!"not implemented yet");
1230 }
1231
1232 void SimbodyMatterSubtree::SubtreeRep::
copyAccelerationsFromState(const State & s,SimbodyMatterSubtreeResults::SubtreeResultsRep & sr) const1233 copyAccelerationsFromState(const State& s, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
1234 const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
1235 SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Acceleration,
1236 "calcAccelerationsFromState()");
1237
1238 assert(isCompatibleSubtreeResults(sr));
1239
1240 // Copy the udot's; adjust the body accelerations to be measured relative to,
1241 // and expressed in, the ancestor body instead of ground.
1242 sr.packStateUIntoSubtreeU(matter.getUDot(s), sr.updSubUDot());
1243
1244 if (getAncestorMobilizedBodyIndex() == GroundIndex) {
1245 for (SubtreeBodyIndex sb(1); sb < getNumSubtreeBodies(); ++sb) {
1246 const MobilizedBodyIndex mb = getSubtreeBodyMobilizedBodyIndex(sb);
1247 const SpatialVec& A_GB = matter.getBodyAcceleration(s,mb);
1248 sr.setSubtreeBodyAcceleration(sb, A_GB); // =A_AB
1249 }
1250 } else {
1251 // Ancestor A differs from Ground G so we have to adjust all the
1252 // Subtree body accelerations to measure from A instead of G.
1253 const Transform& X_GA = matter.getBodyTransform(s,getAncestorMobilizedBodyIndex());
1254 const SpatialVec& V_GA = matter.getBodyVelocity(s,getAncestorMobilizedBodyIndex());
1255 const SpatialVec& A_GA = matter.getBodyAcceleration(s,getAncestorMobilizedBodyIndex());
1256 for (SubtreeBodyIndex sb(1); sb < getNumSubtreeBodies(); ++sb) {
1257 const MobilizedBodyIndex mb = getSubtreeBodyMobilizedBodyIndex(sb);
1258 const Transform& X_GB = matter.getBodyTransform(s,mb);
1259 const SpatialVec& V_GB = matter.getBodyVelocity(s,mb);
1260 const SpatialVec& A_GB = matter.getBodyAcceleration(s,mb);
1261
1262 const Vec3 p_AB_G = X_GB.p() - X_GA.p();
1263 const Vec3 p_AB_G_dot = V_GB[1] - V_GA[1]; // taken in G
1264 const Vec3 p_AB_G_dotdot = A_GB[1] - A_GA[1]; // taken in G
1265
1266 const Vec3 v_AB_G = p_AB_G_dot - V_GA[0] % p_AB_G; // taken in A, exp. in G
1267 const Vec3 b_AB_G = A_GB[0] - A_GA[0]; // relative angular acceleration
1268 const Vec3 a_AB_G = p_AB_G_dotdot - (A_GA[0] % p_AB_G + V_GA[0] % p_AB_G_dot); // taken in A, exp. in G
1269 const SpatialVec A_AB = ~X_GA.R() * SpatialVec(b_AB_G, a_AB_G); // re-express in A
1270 sr.setSubtreeBodyAcceleration(sb, A_AB);
1271 }
1272 }
1273
1274 sr.setStage(Stage::Acceleration);
1275 }
1276
1277 void SimbodyMatterSubtree::SubtreeRep::
calcAccelerationsFromSubtreeUDot(const State & s,const Vector & subUDot,SimbodyMatterSubtreeResults::SubtreeResultsRep & sr) const1278 calcAccelerationsFromSubtreeUDot(const State& s, const Vector& subUDot, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
1279 const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
1280 SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Instance,
1281 "calcAccelerationsFromSubtreeUDot()");
1282
1283 assert(isCompatibleSubtreeResults(sr));
1284
1285 assert(!"not implemented yet");
1286 }
1287
1288 void SimbodyMatterSubtree::SubtreeRep::
calcAccelerationsFromZeroUDot(const State & s,SimbodyMatterSubtreeResults::SubtreeResultsRep & sr) const1289 calcAccelerationsFromZeroUDot(const State& s, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
1290 const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
1291 SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Instance,
1292 "calcAccelerationsFromZeroUDot()");
1293
1294 assert(isCompatibleSubtreeResults(sr));
1295
1296 assert(!"not implemented yet");
1297 }
1298
1299 void SimbodyMatterSubtree::SubtreeRep::
perturbAccelerations(const State & s,SubtreeUIndex subUDotIndex,Real perturbation,SimbodyMatterSubtreeResults::SubtreeResultsRep & sr) const1300 perturbAccelerations(const State& s, SubtreeUIndex subUDotIndex, Real perturbation, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
1301 const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
1302 SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Instance,
1303 "perturbAccelerations()");
1304
1305 assert(isCompatibleSubtreeResults(sr));
1306 assert(sr.getStage() >= Stage::Acceleration);
1307
1308 assert(!"not implemented yet");
1309 }
1310
1311
1312 } // namespace SimTK
1313
1314