1 #ifndef SimTK_SIMBODY_MATTER_SUBTREE_H_ 2 #define SimTK_SIMBODY_MATTER_SUBTREE_H_ 3 4 /* -------------------------------------------------------------------------- * 5 * Simbody(tm) * 6 * -------------------------------------------------------------------------- * 7 * This is part of the SimTK 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/home/simbody. * 11 * * 12 * Portions copyright (c) 2007-12 Stanford University and the Authors. * 13 * Authors: Michael Sherman * 14 * Contributors: * 15 * * 16 * Licensed under the Apache License, Version 2.0 (the "License"); you may * 17 * not use this file except in compliance with the License. You may obtain a * 18 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * 19 * * 20 * Unless required by applicable law or agreed to in writing, software * 21 * distributed under the License is distributed on an "AS IS" BASIS, * 22 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * 23 * See the License for the specific language governing permissions and * 24 * limitations under the License. * 25 * -------------------------------------------------------------------------- */ 26 27 #include "SimTKcommon.h" 28 #include "simbody/internal/common.h" 29 30 #include <cassert> 31 #include <iosfwd> 32 33 namespace SimTK { 34 35 class SimbodyMatterSubsystem; 36 class MobilizedBody; 37 class SimbodyMatterSubtree; 38 class SimbodyMatterSubtreeResults; 39 40 /** A SimbodyMatterSubtree is a view of a connected subgraph of the tree of 41 mobilized bodies in a SimbodyMatterSubsystem. It is used to perform kinematic 42 operations on the subgraph to facilitate the handling of constraints, which 43 typically involve only small subgraphs. 44 45 A SimbodyMatterSubtree is characterized by a single ancestor body A and a set 46 of terminal mobilized bodies T={Ti}, where A is the outmost body that is on 47 the inboard path of each Ti. Note that a SimbodyMatterSubtree's "terminal" 48 bodies do not have to be terminal in the full tree. The SimbodyMatterSubtree 49 includes T and all "branch" mobilized bodies B={Bij} found on any path from a 50 Ti to A, and A itself which serves as Ground. A may be one of the terminal 51 bodies. A's mobilizer is *not* part of the SimbodyMatterSubtree. The path 52 from Ti to A is called the ith branch of the SimbodyMatterSubtree; branches 53 can overlap. 54 @verbatim 55 . . 56 . . . 57 . . . 58 T0 T1 T2 } 59 * * . * } 60 B0 * * B1 * } 61 * * * } A SimbodyMatterSubtree with 62 . * * B2 } three branches. 63 . . * * } 64 * * } 65 B0,B1 * * } 66 A } 67 . 68 . 69 ... 70 Ground 71 @endverbatim 72 Each body in the SimbodyMatterSubtree is assigned an index called a 73 SubtreeBodyIndex, with the Ancestor being SubtreeBodyIndex 0 and other ids 74 assigned such that ids increase going outwards along a branch. Maps are kept 75 in the SimbodyMatterSubtree object to track its relationship to the full tree. 76 77 A SimbodyMatterSubtree can be constructed at Topology stage and needed ones 78 can thus be precalculated and stored in the SimbodyMatterSubsystem Topology 79 Cache (i.e., in the System not the State). Calculations done on the 80 SimbodyMatterSubtree, on the other hand, require further state information 81 and cannot be stored as part of the System. For those, we define a companion 82 class below called SimbodyMatterSubtreeResults. 83 84 A SimbodyMatterSubtreeResults object is initialized at Model stage, at which 85 point we can determine the mobilities u and generalized coordinates q. These 86 are assigned SubtreeUIndex's and SubtreeQIndex's in the same order that the 87 SimbodyMatterSubtree bodies are numbered. Maps are kept in the 88 SimbodyMatterSubtreeResults object to track the relationship between the 89 SimbodyMatterSubtree mobilities and those in the full tree. 90 91 Note that SimbodyMatterSubtree operations are elaborate \e operators, not 92 \e responses. That means the results are not stored in the State, but rather 93 in the private SimbodyMatterSubtreeResults objects. 94 95 Operators here perform kinematic operations based on perturbations of the 96 global System State values. The supported perturbations are: 97 General 98 1a same as global System state (except answers are in A rather than G) 99 1b all mobility variables set 100 2 all mobility variables from 1a or 1b, except for one which is perturbed 101 (q,u,udot) 102 103 Linear 104 3 all mobility variables are zero (u,udot) 105 4 all mobility variables are zero *again*, except for one which is 1 106 (u,udot) 107 Steps 1 and 2 are designed to work together, as are 3 and 4: first evaluate 108 nominal kinematics; then perturb. **/ 109 class SimTK_SIMBODY_EXPORT SimbodyMatterSubtree { 110 public: 111 SimbodyMatterSubtree(); 112 SimbodyMatterSubtree(const SimbodyMatterSubtree&); 113 SimbodyMatterSubtree& operator=(const SimbodyMatterSubtree&); 114 ~SimbodyMatterSubtree(); 115 116 explicit SimbodyMatterSubtree(const SimbodyMatterSubsystem&); 117 SimbodyMatterSubtree(const SimbodyMatterSubsystem&, 118 const Array_<MobilizedBodyIndex>& terminalBodies); 119 120 void setSimbodyMatterSubsystem(const SimbodyMatterSubsystem& matter); 121 const SimbodyMatterSubsystem& getSimbodyMatterSubsystem() const; 122 123 // This doesn't change the associated SimbodyMatterSubsystem if there 124 // is one, but does remove all the bodies from the SimbodyMatterSubtree. 125 void clear(); 126 127 SimbodyMatterSubtree& addTerminalBody(MobilizedBodyIndex); 128 129 void realizeTopology(); 130 131 int getNumSubtreeBodies() const; // includes ancestor 132 MobilizedBodyIndex getAncestorMobilizedBodyIndex() const; 133 134 // These are in the same order they were added; body[i] is the terminus 135 // of branch i. 136 const Array_<MobilizedBodyIndex>& getTerminalBodies() const; 137 138 // These are indexed by SubtreeBodyIndex starting with 0 for the ancestor 139 // body and monotonically increasing outwards along a branch. 140 const Array_<MobilizedBodyIndex>& getAllBodies() const; 141 142 // 0 returns an invalid Index 143 SubtreeBodyIndex getParentSubtreeBodyIndex(SubtreeBodyIndex) const; 144 const Array_<SubtreeBodyIndex>& 145 getChildSubtreeBodyIndices(SubtreeBodyIndex) const; 146 147 // MODEL STAGE 148 149 // State must be realized to at least Stage::Model for this call to work. 150 // The supplied SimbodyMatterSubtreeResults object is allocated and properly initialized to 151 // be able to hold computation results from this SimbodyMatterSubtree. 152 void initializeSubtreeResults(const State&, SimbodyMatterSubtreeResults&) const; 153 154 // This can be used as a sanity check that initializeSubtreeResults() was 155 // already called in this SimbodyMatterSubtree to produce these 156 // SimbodyMatterSubtreeResults. It is by no means exhaustive but will catch 157 // egregious errors. 158 bool isCompatibleSubtreeResults(const SimbodyMatterSubtreeResults&) const; 159 160 // POSITION STAGE 161 162 // State must be realized to at least Stage::Position for this to work. SimbodyMatterSubtreeResults 163 // must have already been initialized to work with this SimbodyMatterSubtree. SimbodyMatterSubtreeResults stage 164 // will be Stage::Position after this call. All body transforms will be the same as 165 // the corresponding ones in the state, except they will be measured from the ancestor 166 // frame instead of ground. SimbodyMatterSubtree q's will be identical to corresponding State q's. 167 void copyPositionsFromState(const State&, SimbodyMatterSubtreeResults&) const; 168 169 // State must be realized to Stage::Instance. subQ must be the right length for this 170 // SimbodyMatterSubtree, and SimbodyMatterSubtreeResults must have been properly initialized. SimbodyMatterSubtreeResults 171 // stage will be Stage::Position after this call. 172 void calcPositionsFromSubtreeQ(const State&, const Vector& subQ, SimbodyMatterSubtreeResults&) const; 173 174 // Calculates a perturbed position result starting with the subQ's and position results 175 // which must already be in SimbodyMatterSubtreeResults. 176 void perturbPositions(const State&, SubtreeQIndex subQIndex, Real perturbation, SimbodyMatterSubtreeResults&) const; 177 178 179 // VELOCITY STAGE 180 181 // State must be realized to at least Stage::Velocity for this to work. SimbodyMatterSubtreeResults 182 // must already be at Stage::Position. SimbodyMatterSubtreeResults stage 183 // will be Stage::Velocity after this call. All subtree body spatial velocities will be 184 // the same as in the State, except measured relative to A and expressed in A. SimbodyMatterSubtree u's 185 // will be identical to corresponding State u's. 186 void copyVelocitiesFromState(const State&, SimbodyMatterSubtreeResults&) const; 187 188 // State must be realized to Stage::Instance. subU must be the right length for this 189 // SimbodyMatterSubtree, and SimbodyMatterSubtreeResults must already be at Stage::Position. SimbodyMatterSubtreeResults 190 // stage will be Stage::Velocity after this call. 191 void calcVelocitiesFromSubtreeU(const State&, const Vector& subU, SimbodyMatterSubtreeResults&) const; 192 193 // State must be realized to Stage::Instance and SimbodyMatterSubtreeResults must already be at 194 // Stage::Position. SimbodyMatterSubtreeResults stage will be Stage::Velocity after this call, but 195 // all SimbodyMatterSubtree u's and body velocities will be zero. 196 void calcVelocitiesFromZeroU(const State&, SimbodyMatterSubtreeResults&) const; 197 198 // Calculates a perturbed velocity result starting with the subU's and velocity results 199 // which must already be in SimbodyMatterSubtreeResults. 200 void perturbVelocities(const State&, SubtreeUIndex subUIndex, Real perturbation, SimbodyMatterSubtreeResults&) const; 201 202 203 // ACCELERATION STAGE 204 205 // State must be realized to at least Stage::Acceleration for this to work. SimbodyMatterSubtreeResults 206 // must already be at Stage::Velocity. SimbodyMatterSubtreeResults stage 207 // will be Stage::Acceleration after this call. All subtree body spatial accelerations will be 208 // the same as in the State, except measured relative to A and expressed in A. SimbodyMatterSubtree udots 209 // will be identical to corresponding State udots. 210 void copyAccelerationsFromState(const State&, SimbodyMatterSubtreeResults&) const; 211 212 // State must be realized to Stage::Instance. subUDot must be the right length for this 213 // SimbodyMatterSubtree, and SimbodyMatterSubtreeResults must already be at Stage::Velocity. SimbodyMatterSubtreeResults 214 // stage will be Stage::Acceleration after this call. 215 void calcAccelerationsFromSubtreeUDot(const State&, const Vector& subUDot, SimbodyMatterSubtreeResults&) const; 216 217 // State must be realized to Stage::Instance and SimbodyMatterSubtreeResults must already be at 218 // Stage::Velocity. SimbodyMatterSubtreeResults stage will be Stage::Acceleration after this call. 219 // All SimbodyMatterSubtree udots's will be zero, body accelerations will have only their bias values 220 // (coriolis accelerations from nonzero u's). 221 void calcAccelerationsFromZeroUDot(const State&, SimbodyMatterSubtreeResults&) const; 222 223 // Calculates a perturbed velocity result starting with the subUDot's and acceleration results 224 // which must already be in SimbodyMatterSubtreeResults. 225 void perturbAccelerations(const State&, SubtreeUIndex subUDotIndex, Real perturbation, SimbodyMatterSubtreeResults&) const; 226 227 class SubtreeRep; 228 private: 229 SubtreeRep* rep; getRep()230 const SubtreeRep& getRep() const {assert(rep);return *rep;} updRep()231 SubtreeRep& updRep() {assert(rep);return *rep;} 232 }; 233 234 SimTK_SIMBODY_EXPORT std::ostream& 235 operator<<(std::ostream&, const SimbodyMatterSubtree&); 236 237 /* 238 * This is the writable "cache" for a SimbodyMatterSubtree. Once the full State has 239 * been realized to the Model stage, a SimbodyMatterSubtree can initialize one of these 240 * objects and then use it to hold operator results. 241 */ 242 class SimTK_SIMBODY_EXPORT SimbodyMatterSubtreeResults { 243 public: 244 SimbodyMatterSubtreeResults(); 245 SimbodyMatterSubtreeResults(const SimbodyMatterSubtreeResults&); 246 SimbodyMatterSubtreeResults& operator=(const SimbodyMatterSubtreeResults&); 247 ~SimbodyMatterSubtreeResults(); 248 249 void clear(); 250 251 void reallocateBodies(int nBodies); 252 void addMobilities(SubtreeBodyIndex, QIndex qStart, int nq, UIndex uStart, int nu); 253 void realizeModel(const Vector& stateQ, const Vector& stateU); 254 255 Stage getStage() const; 256 257 int getNumSubtreeBodies() const; 258 int getNumSubtreeQs() const; 259 int getNumSubtreeUs() const; 260 261 const Vector& getSubtreeQ() const; 262 const Transform& getSubtreeBodyTransform(SubtreeBodyIndex) const; // from ancestor frame 263 264 const Vector& getSubtreeU() const; 265 const SpatialVec& getSubtreeBodyVelocity(SubtreeBodyIndex) const; // measured & expressed in ancestor frame 266 267 const Vector& getSubtreeUDot() const; 268 const SpatialVec& getSubtreeBodyAcceleration(SubtreeBodyIndex) const; // measured & expressed in ancestor frame 269 270 // These are indexed by SubtreeQIndex and SubtreeUIndex. 271 const Array_<QIndex>& getQSubset() const; // subset of Subsystem Qs used by this SimbodyMatterSubtree 272 const Array_<UIndex>& getUSubset() const; // subset of Subsystem Us used by this SimbodyMatterSubtree 273 274 void findSubtreeBodyQ(SubtreeBodyIndex, SubtreeQIndex& qStart, int& nq) const; // indices into QSubset 275 void findSubtreeBodyU(SubtreeBodyIndex, SubtreeUIndex& uStart, int& nu) const; // indices into USubset 276 277 class SubtreeResultsRep; 278 private: 279 friend class SimbodyMatterSubtree; 280 SubtreeResultsRep* rep; getRep()281 const SubtreeResultsRep& getRep() const {assert(rep);return *rep;} updRep()282 SubtreeResultsRep& updRep() {assert(rep);return *rep;} 283 }; 284 285 SimTK_SIMBODY_EXPORT std::ostream& 286 operator<<(std::ostream&, const SimbodyMatterSubtreeResults&); 287 288 } // namespace SimTK 289 290 #endif // SimTK_SIMBODY_MATTER_SUBTREE_H_ 291